Branch data Line data Source code
1 : : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
2 : : //
3 : : // Copyright (C) 2020-2021 Intel Corporation
4 : : //
5 : : // SPDX-License-Identifier: LGPL-2.1-only
6 : : //
7 : : // ----------------- END LICENSE BLOCK -----------------------------------
8 : : /**
9 : : * @file
10 : : */
11 : :
12 : : #include "ad/rss/unstructured/TrajectoryCommon.hpp"
13 : : #include <ad/geometry/DebugDrawing.hpp>
14 : : #include <ad/physics/Operation.hpp>
15 : : #include "ad/rss/core/Logging.hpp"
16 : :
17 : : /*!
18 : : * @brief namespace ad
19 : : */
20 : : namespace ad {
21 : : /*!
22 : : * @brief namespace rss
23 : : */
24 : : namespace rss {
25 : : /*!
26 : : * @brief namespace unstructured
27 : : */
28 : : namespace unstructured {
29 : :
30 : 3904 : ::ad::geometry::Point getVehicleCorner(TrajectoryPoint const &point,
31 : : core::RelativeObjectState const &vehicleState,
32 : : VehicleCorner const corner)
33 : : {
34 : : ::ad::geometry::Point resultPoint;
35 [ + - ]: 3904 : auto const vehicleAngle = point.angle - physics::cPI_2;
36 [ + - ]: 3904 : auto front_distance = vehicleState.unstructured_object_state.dimension.length / 2.0;
37 [ + + ]: 3904 : if (vehicleState.object_type == world::ObjectType::EgoVehicle)
38 : : {
39 : : // in case of the ego vehicle we add the desired min safety distance to keep a minimum distance from others
40 [ + - ]: 2160 : front_distance += vehicleState.dynamics.min_longitudinal_safety_distance;
41 : : }
42 [ + + + + : 3904 : switch (corner)
- ]
43 : : {
44 : 976 : case VehicleCorner::frontLeft:
45 : 976 : resultPoint = ::ad::geometry::rotateAroundPoint(
46 [ + - ]: 976 : point.position,
47 [ + - + - : 976 : ::ad::geometry::toPoint(-vehicleState.unstructured_object_state.dimension.width / 2.0, front_distance),
+ - ]
48 : : vehicleAngle);
49 : 976 : break;
50 : 976 : case VehicleCorner::frontRight:
51 : 976 : resultPoint = ::ad::geometry::rotateAroundPoint(
52 [ + - ]: 976 : point.position,
53 [ + - + - ]: 976 : ::ad::geometry::toPoint(vehicleState.unstructured_object_state.dimension.width / 2.0, front_distance),
54 : : vehicleAngle);
55 : 976 : break;
56 : 976 : case VehicleCorner::backLeft:
57 : 976 : resultPoint = ::ad::geometry::rotateAroundPoint(
58 [ + - ]: 976 : point.position,
59 [ + - + - : 976 : ::ad::geometry::toPoint(-vehicleState.unstructured_object_state.dimension.width / 2.0,
+ - ]
60 [ + - + - ]: 976 : -vehicleState.unstructured_object_state.dimension.length / 2.0),
61 : : vehicleAngle);
62 : 976 : break;
63 : 976 : case VehicleCorner::backRight:
64 : 976 : resultPoint = ::ad::geometry::rotateAroundPoint(
65 [ + - ]: 976 : point.position,
66 [ + - + - ]: 976 : ::ad::geometry::toPoint(vehicleState.unstructured_object_state.dimension.width / 2.0,
67 [ + - + - ]: 976 : -vehicleState.unstructured_object_state.dimension.length / 2.0),
68 : : vehicleAngle);
69 : 976 : break;
70 : 0 : default:
71 [ # # ]: 0 : throw std::runtime_error("unstructured::getVehicleCorner>> invalid corner requested");
72 : : break;
73 : : }
74 : 3904 : return resultPoint;
75 : : }
76 : :
77 : 140 : bool calculateStepPolygon(core::RelativeObjectState const &vehicleState,
78 : : TrajectorySetStep const &step,
79 : : std::string const &debugNamespace,
80 : : ::ad::geometry::Polygon &polygon,
81 : : TrajectorySetStepVehicleLocation &stepVehicleLocation)
82 : : {
83 : 140 : ::ad::geometry::MultiPoint frontPtsLeft;
84 : 140 : ::ad::geometry::MultiPoint frontPtsRight;
85 : :
86 : 140 : int idx = 0;
87 [ + + ]: 532 : for (auto it = step.left.cbegin(); it != step.left.cend(); ++it)
88 : : {
89 [ + - ]: 392 : auto vehicleLocationLeft = TrafficParticipantLocation(*it, vehicleState);
90 [ + - + - : 392 : DEBUG_DRAWING_POLYGON(vehicleLocationLeft.toPolygon(), "blue", debugNamespace + "_left_" + std::to_string(idx));
+ - + - +
- + - +
- ]
91 [ + - + - ]: 392 : boost::geometry::append(frontPtsLeft, vehicleLocationLeft.toMultiPoint());
92 [ + + ]: 392 : if (it == step.left.end() - 1)
93 : : {
94 : 140 : stepVehicleLocation.left = vehicleLocationLeft;
95 : : }
96 : 392 : ++idx;
97 : : }
98 : :
99 : : // center
100 [ + - ]: 140 : auto vehicleLocationCenter = TrafficParticipantLocation(step.center, vehicleState);
101 [ + - + - : 140 : DEBUG_DRAWING_POLYGON(vehicleLocationCenter.toPolygon(), "blue", debugNamespace + "_center");
+ - + - +
- ]
102 [ + - + - ]: 140 : boost::geometry::append(frontPtsLeft, vehicleLocationCenter.toMultiPoint());
103 : 140 : stepVehicleLocation.center = vehicleLocationCenter;
104 [ + - + - ]: 140 : boost::geometry::append(frontPtsRight, vehicleLocationCenter.toMultiPoint());
105 : :
106 : 140 : idx = 0;
107 [ + + ]: 532 : for (auto it = step.right.cbegin(); it != step.right.cend(); ++it)
108 : : {
109 [ + - ]: 392 : auto vehicleLocationRight = TrafficParticipantLocation(*it, vehicleState);
110 [ + - + - : 392 : DEBUG_DRAWING_POLYGON(vehicleLocationRight.toPolygon(), "blue", debugNamespace + "_right_" + std::to_string(idx));
+ - + - +
- + - +
- ]
111 [ + - + - ]: 392 : boost::geometry::append(frontPtsRight, vehicleLocationRight.toMultiPoint());
112 [ + + ]: 392 : if (it == step.right.begin())
113 : : {
114 : 140 : stepVehicleLocation.right = vehicleLocationRight;
115 : : }
116 : 392 : ++idx;
117 : : }
118 : :
119 [ + - ]: 140 : ::ad::geometry::Polygon hullLeft;
120 [ + - ]: 140 : ::ad::geometry::Polygon hullRight;
121 [ + - ]: 140 : boost::geometry::convex_hull(frontPtsLeft, hullLeft);
122 [ + - ]: 140 : boost::geometry::convex_hull(frontPtsRight, hullRight);
123 [ + - ]: 140 : auto result = ::ad::geometry::combinePolygon(hullLeft, hullRight, polygon);
124 : 140 : return result;
125 : 140 : }
126 : :
127 : 118 : bool calculateEstimationBetweenSteps(::ad::geometry::Polygon &polygon,
128 : : TrajectorySetStepVehicleLocation const &previousStepVehicleLocation,
129 : : TrajectorySetStepVehicleLocation const ¤tStepVehicleLocation,
130 : : std::string const &debugNamespace)
131 : : {
132 : : // Fill potential gap between two calculation steps by using the previous and current step
133 [ + - + + ]: 118 : if (previousStepVehicleLocation == currentStepVehicleLocation)
134 : : {
135 : 42 : return true;
136 : : }
137 : :
138 [ + - ]: 76 : ::ad::geometry::Polygon hull;
139 : 76 : ::ad::geometry::MultiPoint interimPtsLeft;
140 [ + - + - ]: 76 : boost::geometry::append(interimPtsLeft, previousStepVehicleLocation.left.toMultiPoint());
141 [ + - + - ]: 76 : boost::geometry::append(interimPtsLeft, previousStepVehicleLocation.center.toMultiPoint());
142 [ + - + - ]: 76 : boost::geometry::append(interimPtsLeft, currentStepVehicleLocation.left.toMultiPoint());
143 [ + - + - ]: 76 : boost::geometry::append(interimPtsLeft, currentStepVehicleLocation.center.toMultiPoint());
144 [ + - ]: 76 : ::ad::geometry::Polygon hullLeft;
145 [ + - ]: 76 : boost::geometry::convex_hull(interimPtsLeft, hullLeft);
146 : :
147 : 76 : ::ad::geometry::MultiPoint interimPtsRight;
148 [ + - + - ]: 76 : boost::geometry::append(interimPtsRight, previousStepVehicleLocation.right.toMultiPoint());
149 [ + - + - ]: 76 : boost::geometry::append(interimPtsRight, previousStepVehicleLocation.center.toMultiPoint());
150 [ + - + - ]: 76 : boost::geometry::append(interimPtsRight, currentStepVehicleLocation.right.toMultiPoint());
151 [ + - + - ]: 76 : boost::geometry::append(interimPtsRight, currentStepVehicleLocation.center.toMultiPoint());
152 [ + - ]: 76 : ::ad::geometry::Polygon hullRight;
153 [ + - ]: 76 : boost::geometry::convex_hull(interimPtsRight, hullRight);
154 [ + - ]: 76 : auto result = ::ad::geometry::combinePolygon(hullRight, hullLeft, hull);
155 [ - + ]: 76 : if (!result)
156 : : {
157 [ # # # # ]: 0 : core::getLogger()->debug("unstructured::calculateEstimationBetweenSteps>> Could not create estimation polygon ({})."
158 : : "left {}, right {}",
159 : : debugNamespace,
160 [ # # ]: 0 : std::to_string(hullLeft),
161 [ # # ]: 0 : std::to_string(hullRight));
162 : : }
163 : :
164 [ + - ]: 76 : if (result)
165 : : {
166 [ + - + - : 76 : DEBUG_DRAWING_POLYGON(hull, "yellow", debugNamespace + "_hull_front");
+ - + - ]
167 [ + - ]: 76 : result = ::ad::geometry::combinePolygon(polygon, hull, polygon);
168 [ - + ]: 76 : if (!result)
169 : : {
170 [ # # # # ]: 0 : core::getLogger()->warn(
171 : : "unstructured::calculateEstimationBetweenSteps>> Could not combine front estimation polygon ({}) "
172 : : "with final polygon."
173 : : "polygon {}, hull {}",
174 : : debugNamespace,
175 [ # # ]: 0 : std::to_string(polygon),
176 [ # # ]: 0 : std::to_string(hull));
177 : : }
178 : : }
179 : 76 : return result;
180 : 76 : }
181 : :
182 : 48 : bool calculateFrontAndSidePolygon(core::RelativeObjectState const &vehicleState,
183 : : TrajectorySetStepVehicleLocation const &initialStepVehicleLocation,
184 : : std::vector<TrajectorySetStep> const &sideSteps,
185 : : TrajectorySetStep const &front,
186 : : std::string const &debugNamespace,
187 : : ::ad::geometry::Polygon &resultPolygon,
188 : : TrajectorySetStepVehicleLocation &frontSideStepVehicleLocation)
189 : : {
190 : 48 : auto result = true;
191 : 48 : auto previousStepVehicleLocation = initialStepVehicleLocation;
192 : : //-------------
193 : : // sides
194 : : //-------------
195 : 48 : auto idx = 0;
196 [ + + + - : 118 : for (auto sideStepIt = sideSteps.cbegin(); (sideStepIt != sideSteps.cend()) && result; ++sideStepIt)
+ + ]
197 : : {
198 [ + - ]: 70 : ::ad::geometry::Polygon stepPolygon;
199 : 70 : TrajectorySetStepVehicleLocation currentStepVehicleLocation;
200 [ + - ]: 70 : result = calculateStepPolygon(
201 [ + - + - : 140 : vehicleState, *sideStepIt, debugNamespace + "_" + std::to_string(idx), stepPolygon, currentStepVehicleLocation);
+ - ]
202 [ - + ]: 70 : if (!result)
203 : : {
204 [ # # # # ]: 0 : core::getLogger()->debug("unstructured::calculateFrontAndSidePolygon>> Could not calculate step polygon");
205 : : }
206 [ + - ]: 70 : if (result)
207 : : {
208 [ + - ]: 70 : result = calculateEstimationBetweenSteps(resultPolygon,
209 : : previousStepVehicleLocation,
210 : : currentStepVehicleLocation,
211 [ + - + - : 140 : debugNamespace + "_step_estimation_" + std::to_string(idx));
+ - ]
212 [ - + ]: 70 : if (!result)
213 : : {
214 [ # # # # ]: 0 : core::getLogger()->debug("unstructured::calculateFrontAndSidePolygon>> Could not calculate between steps");
215 : : }
216 : 70 : previousStepVehicleLocation = currentStepVehicleLocation;
217 : : }
218 : :
219 [ + - ]: 70 : if (result)
220 : : {
221 [ + - ]: 70 : result = ::ad::geometry::combinePolygon(resultPolygon, stepPolygon, resultPolygon);
222 : : }
223 : 70 : idx++;
224 : 70 : }
225 : :
226 : : //-------------
227 : : // front
228 : : //-------------
229 [ + - ]: 48 : ::ad::geometry::Polygon frontPolygon;
230 [ + - ]: 48 : if (result)
231 : : {
232 [ + - ]: 48 : result = calculateStepPolygon(
233 [ + - ]: 96 : vehicleState, front, debugNamespace + "_front", frontPolygon, frontSideStepVehicleLocation);
234 : : }
235 [ + - ]: 48 : if (result)
236 : : {
237 [ + - ]: 48 : result = calculateEstimationBetweenSteps(
238 [ + - ]: 96 : resultPolygon, previousStepVehicleLocation, frontSideStepVehicleLocation, debugNamespace + "_front_estimation");
239 [ - + ]: 48 : if (!result)
240 : : {
241 [ # # # # ]: 0 : core::getLogger()->debug("unstructured::calculateFrontAndSidePolygon>> Could not calculate between last step and "
242 : : "front polygon.");
243 : : }
244 : : }
245 [ + - ]: 48 : if (result)
246 : : {
247 [ + - ]: 48 : result = ::ad::geometry::combinePolygon(resultPolygon, frontPolygon, resultPolygon);
248 : : }
249 : 48 : return result;
250 : 48 : }
251 : :
252 : 46 : bool calculateResponseTimePolygon(core::RelativeObjectState const &vehicleState,
253 : : TrajectorySetStepVehicleLocation const &initialStepVehicleLocation,
254 : : std::string const &debugNamespace,
255 : : ::ad::geometry::Polygon &resultPolygon)
256 : : {
257 : 46 : ::ad::geometry::MultiPoint pts;
258 [ + - + - ]: 46 : boost::geometry::append(pts, initialStepVehicleLocation.left.toMultiPoint());
259 [ + - + - ]: 46 : boost::geometry::append(pts, initialStepVehicleLocation.center.toMultiPoint());
260 [ + - + - ]: 46 : boost::geometry::append(pts, initialStepVehicleLocation.right.toMultiPoint());
261 [ + - ]: 46 : TrajectoryPoint currentTrajectoryPoint(vehicleState, TrajectoryPoint::SpeedMode::Min);
262 [ + - ]: 46 : TrafficParticipantLocation currentLocation(currentTrajectoryPoint, vehicleState);
263 [ + - + - ]: 46 : boost::geometry::append(pts, currentLocation.toMultiPoint());
264 [ + - ]: 46 : ::ad::geometry::Polygon hull;
265 [ + - ]: 46 : boost::geometry::convex_hull(pts, hull);
266 : :
267 [ + - ]: 46 : auto result = ::ad::geometry::combinePolygon(resultPolygon, hull, resultPolygon);
268 [ - + ]: 46 : if (!result)
269 : : {
270 [ # # # # ]: 0 : core::getLogger()->debug("unstructured::calculateResponseTimePolygon>> Could not create combined polygon ({})."
271 : : "hull {}",
272 : : debugNamespace,
273 [ # # ]: 0 : std::to_string(hull));
274 : : }
275 : 46 : return result;
276 : 46 : }
277 : :
278 : : } // namespace unstructured
279 : : } // namespace rss
280 : : } // namespace ad
|
