|
12 | 12 |
|
13 | 13 | #include "TrajectoryCommon.hpp" |
14 | 14 | #include <ad/physics/Operation.hpp> |
| 15 | +#include "ad/rss/unstructured/DebugDrawing.hpp" |
15 | 16 |
|
16 | 17 | /*! |
17 | 18 | * @brief namespace ad |
@@ -57,6 +58,180 @@ Point getVehicleCorner(TrajectoryPoint const &point, |
57 | 58 | return resultPoint; |
58 | 59 | } |
59 | 60 |
|
| 61 | +bool calculateStepPolygon(situation::VehicleState const &vehicleState, |
| 62 | + TrajectorySetStep const &step, |
| 63 | + std::string const &debugNamespace, |
| 64 | + Polygon &polygon, |
| 65 | + TrajectorySetStepVehicleLocation &stepVehicleLocation) |
| 66 | +{ |
| 67 | + MultiPoint frontPtsLeft; |
| 68 | + MultiPoint frontPtsRight; |
| 69 | + |
| 70 | + int idx = 0; |
| 71 | + for (auto it = step.left.cbegin(); it != step.left.cend(); ++it) |
| 72 | + { |
| 73 | + auto vehicleLocationLeft = TrafficParticipantLocation(*it, vehicleState); |
| 74 | + DEBUG_DRAWING_POLYGON(vehicleLocationLeft.toPolygon(), "blue", debugNamespace + "_left_" + std::to_string(idx)); |
| 75 | + boost::geometry::append(frontPtsLeft, vehicleLocationLeft.toMultiPoint()); |
| 76 | + if (it == step.left.end() - 1) |
| 77 | + { |
| 78 | + stepVehicleLocation.left = vehicleLocationLeft; |
| 79 | + } |
| 80 | + ++idx; |
| 81 | + } |
| 82 | + |
| 83 | + // center |
| 84 | + auto vehicleLocationCenter = TrafficParticipantLocation(step.center, vehicleState); |
| 85 | + DEBUG_DRAWING_POLYGON(vehicleLocationCenter.toPolygon(), "blue", debugNamespace + "_center"); |
| 86 | + boost::geometry::append(frontPtsLeft, vehicleLocationCenter.toMultiPoint()); |
| 87 | + stepVehicleLocation.center = vehicleLocationCenter; |
| 88 | + boost::geometry::append(frontPtsRight, vehicleLocationCenter.toMultiPoint()); |
| 89 | + |
| 90 | + idx = 0; |
| 91 | + for (auto it = step.right.cbegin(); it != step.right.cend(); ++it) |
| 92 | + { |
| 93 | + auto vehicleLocationRight = TrafficParticipantLocation(*it, vehicleState); |
| 94 | + DEBUG_DRAWING_POLYGON(vehicleLocationRight.toPolygon(), "blue", debugNamespace + "_right_" + std::to_string(idx)); |
| 95 | + boost::geometry::append(frontPtsRight, vehicleLocationRight.toMultiPoint()); |
| 96 | + if (it == step.right.begin()) |
| 97 | + { |
| 98 | + stepVehicleLocation.right = vehicleLocationRight; |
| 99 | + } |
| 100 | + ++idx; |
| 101 | + } |
| 102 | + |
| 103 | + Polygon hullLeft; |
| 104 | + Polygon hullRight; |
| 105 | + boost::geometry::convex_hull(frontPtsLeft, hullLeft); |
| 106 | + boost::geometry::convex_hull(frontPtsRight, hullRight); |
| 107 | + auto result = combinePolygon(hullLeft, hullRight, polygon); |
| 108 | + return result; |
| 109 | +} |
| 110 | + |
| 111 | +bool calculateEstimationBetweenSteps(Polygon &polygon, |
| 112 | + TrajectorySetStepVehicleLocation const &previousStepVehicleLocation, |
| 113 | + TrajectorySetStepVehicleLocation const ¤tStepVehicleLocation, |
| 114 | + std::string const &debugNamespace) |
| 115 | +{ |
| 116 | + // Fill potential gap between two calculation steps by using the previous and current step |
| 117 | + if (previousStepVehicleLocation == currentStepVehicleLocation) |
| 118 | + { |
| 119 | + return true; |
| 120 | + } |
| 121 | + |
| 122 | + Polygon hull; |
| 123 | + MultiPoint interimPtsLeft; |
| 124 | + boost::geometry::append(interimPtsLeft, previousStepVehicleLocation.left.toMultiPoint()); |
| 125 | + boost::geometry::append(interimPtsLeft, previousStepVehicleLocation.center.toMultiPoint()); |
| 126 | + boost::geometry::append(interimPtsLeft, currentStepVehicleLocation.left.toMultiPoint()); |
| 127 | + boost::geometry::append(interimPtsLeft, currentStepVehicleLocation.center.toMultiPoint()); |
| 128 | + Polygon hullLeft; |
| 129 | + boost::geometry::convex_hull(interimPtsLeft, hullLeft); |
| 130 | + |
| 131 | + MultiPoint interimPtsRight; |
| 132 | + boost::geometry::append(interimPtsRight, previousStepVehicleLocation.right.toMultiPoint()); |
| 133 | + boost::geometry::append(interimPtsRight, previousStepVehicleLocation.center.toMultiPoint()); |
| 134 | + boost::geometry::append(interimPtsRight, currentStepVehicleLocation.right.toMultiPoint()); |
| 135 | + boost::geometry::append(interimPtsRight, currentStepVehicleLocation.center.toMultiPoint()); |
| 136 | + Polygon hullRight; |
| 137 | + boost::geometry::convex_hull(interimPtsRight, hullRight); |
| 138 | + auto result = combinePolygon(hullRight, hullLeft, hull); |
| 139 | + if (!result) |
| 140 | + { |
| 141 | + spdlog::debug("unstructured::calculateEstimationBetweenSteps>> Could not create estimation polygon ({})." |
| 142 | + "left {}, right {}", |
| 143 | + debugNamespace, |
| 144 | + std::to_string(hullLeft), |
| 145 | + std::to_string(hullRight)); |
| 146 | + } |
| 147 | + |
| 148 | + if (result) |
| 149 | + { |
| 150 | + DEBUG_DRAWING_POLYGON(hull, "yellow", debugNamespace + "_hull_front"); |
| 151 | + result = combinePolygon(polygon, hull, polygon); |
| 152 | + if (!result) |
| 153 | + { |
| 154 | + spdlog::warn("unstructured::calculateEstimationBetweenSteps>> Could not combine front estimation polygon ({}) " |
| 155 | + "with final polygon." |
| 156 | + "polygon {}, hull {}", |
| 157 | + debugNamespace, |
| 158 | + std::to_string(polygon), |
| 159 | + std::to_string(hull)); |
| 160 | + } |
| 161 | + } |
| 162 | + return result; |
| 163 | +} |
| 164 | + |
| 165 | +bool calculateFrontAndSidePolygon(situation::VehicleState const &vehicleState, |
| 166 | + TrajectorySetStepVehicleLocation const &initialStepVehicleLocation, |
| 167 | + std::vector<TrajectorySetStep> const &sideSteps, |
| 168 | + TrajectorySetStep const &front, |
| 169 | + std::string const &debugNamespace, |
| 170 | + Polygon &resultPolygon, |
| 171 | + TrajectorySetStepVehicleLocation &frontSideStepVehicleLocation) |
| 172 | +{ |
| 173 | + auto result = true; |
| 174 | + auto previousStepVehicleLocation = initialStepVehicleLocation; |
| 175 | + //------------- |
| 176 | + // sides |
| 177 | + //------------- |
| 178 | + auto idx = 0; |
| 179 | + for (auto sideStepIt = sideSteps.cbegin(); (sideStepIt != sideSteps.cend()) && result; ++sideStepIt) |
| 180 | + { |
| 181 | + Polygon stepPolygon; |
| 182 | + TrajectorySetStepVehicleLocation currentStepVehicleLocation; |
| 183 | + result = calculateStepPolygon( |
| 184 | + vehicleState, *sideStepIt, debugNamespace + "_" + std::to_string(idx), stepPolygon, currentStepVehicleLocation); |
| 185 | + if (!result) |
| 186 | + { |
| 187 | + spdlog::debug("unstructured::calculateFrontAndSidePolygon>> Could not calculate step polygon"); |
| 188 | + } |
| 189 | + if (result) |
| 190 | + { |
| 191 | + result = calculateEstimationBetweenSteps(resultPolygon, |
| 192 | + previousStepVehicleLocation, |
| 193 | + currentStepVehicleLocation, |
| 194 | + debugNamespace + "_step_estimation_" + std::to_string(idx)); |
| 195 | + if (!result) |
| 196 | + { |
| 197 | + spdlog::debug("unstructured::calculateFrontAndSidePolygon>> Could not calculate between steps"); |
| 198 | + } |
| 199 | + previousStepVehicleLocation = currentStepVehicleLocation; |
| 200 | + } |
| 201 | + |
| 202 | + if (result) |
| 203 | + { |
| 204 | + result = combinePolygon(resultPolygon, stepPolygon, resultPolygon); |
| 205 | + } |
| 206 | + idx++; |
| 207 | + } |
| 208 | + |
| 209 | + //------------- |
| 210 | + // front |
| 211 | + //------------- |
| 212 | + Polygon frontPolygon; |
| 213 | + if (result) |
| 214 | + { |
| 215 | + result = calculateStepPolygon( |
| 216 | + vehicleState, front, debugNamespace + "_front", frontPolygon, frontSideStepVehicleLocation); |
| 217 | + } |
| 218 | + if (result) |
| 219 | + { |
| 220 | + result = calculateEstimationBetweenSteps( |
| 221 | + resultPolygon, previousStepVehicleLocation, frontSideStepVehicleLocation, debugNamespace + "_front_estimation"); |
| 222 | + if (!result) |
| 223 | + { |
| 224 | + spdlog::debug("unstructured::calculateFrontAndSidePolygon>> Could not calculate between last step and " |
| 225 | + "front polygon."); |
| 226 | + } |
| 227 | + } |
| 228 | + if (result) |
| 229 | + { |
| 230 | + result = combinePolygon(resultPolygon, frontPolygon, resultPolygon); |
| 231 | + } |
| 232 | + return result; |
| 233 | +} |
| 234 | + |
60 | 235 | } // namespace unstructured |
61 | 236 | } // namespace rss |
62 | 237 | } // namespace ad |
0 commit comments