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/TrajectoryPedestrian.hpp"
13 : : #include <ad/geometry/DebugDrawing.hpp>
14 : : #include <ad/physics/Operation.hpp>
15 : : #include "ad/rss/core/Logging.hpp"
16 : : #include "ad/rss/core/Physics.hpp"
17 : :
18 : : /*!
19 : : * @brief namespace ad
20 : : */
21 : : namespace ad {
22 : : /*!
23 : : * @brief namespace rss
24 : : */
25 : : namespace rss {
26 : : /*!
27 : : * @brief namespace unstructured
28 : : */
29 : : namespace unstructured {
30 : :
31 : : const physics::Distance TrajectoryPedestrian::maxRadius(1000.);
32 : : const physics::Angle TrajectoryPedestrian::circleStepWidth(physics::c2PI / 20.);
33 : :
34 : 2 : bool TrajectoryPedestrian::calculateTrajectorySets(core::RelativeObjectState const &vehicleState,
35 : : ::ad::geometry::Polygon &brakePolygon,
36 : : ::ad::geometry::Polygon &continueForwardPolygon)
37 : : {
38 : 2 : physics::Duration timeToStopSpeedMax;
39 [ + - ]: 2 : auto result = core::calculateTimeToStop(vehicleState.unstructured_object_state.speed_range.maximum,
40 : : vehicleState.dynamics.response_time,
41 : : vehicleState.dynamics.max_speed_on_acceleration,
42 : : vehicleState.dynamics.alpha_lon.accel_max,
43 : : vehicleState.dynamics.alpha_lon.brake_min,
44 : : timeToStopSpeedMax);
45 : :
46 [ + - - + ]: 2 : if (DEBUG_DRAWING_IS_ENABLED())
47 : : {
48 : : auto vehicleLocation
49 [ # # # # ]: 0 : = TrafficParticipantLocation(TrajectoryPoint(vehicleState, TrajectoryPoint::SpeedMode::Max), vehicleState);
50 [ # # # # : 0 : DEBUG_DRAWING_POLYGON(vehicleLocation.toPolygon(), "black", "pedestrian_initial_position");
# # # # #
# ]
51 : : }
52 [ + - ]: 2 : if (result)
53 : : {
54 [ + - - + ]: 2 : if (vehicleState.unstructured_object_state.speed_range.maximum == physics::Speed(0.))
55 : : {
56 : : result
57 [ # # ]: 0 : = calculateTrajectorySetsStandingStill(vehicleState, timeToStopSpeedMax, brakePolygon, continueForwardPolygon);
58 : : }
59 : : else
60 : : {
61 : 2 : physics::Duration timeToStopSpeedMin;
62 [ + - ]: 2 : result = core::calculateTimeToStop(vehicleState.unstructured_object_state.speed_range.minimum,
63 : : vehicleState.dynamics.response_time,
64 : : vehicleState.dynamics.max_speed_on_acceleration,
65 : : vehicleState.dynamics.alpha_lon.accel_max,
66 : : vehicleState.dynamics.alpha_lon.brake_min,
67 : : timeToStopSpeedMin);
68 : :
69 : 2 : result = result
70 [ + - + - : 2 : && calculateTrajectorySetsMoving(
+ - ]
71 : : vehicleState, timeToStopSpeedMin, brakePolygon, timeToStopSpeedMax, continueForwardPolygon);
72 : : }
73 : : }
74 [ + - + - : 2 : DEBUG_DRAWING_POLYGON(brakePolygon, "red", "pedestrian_brake");
+ - + - ]
75 [ + - + - : 2 : DEBUG_DRAWING_POLYGON(continueForwardPolygon, "green", "pedestrian_continue_forward");
+ - + - ]
76 : 2 : return result;
77 : : }
78 : :
79 : 2 : bool TrajectoryPedestrian::calculateTrajectorySetsMoving(core::RelativeObjectState const &vehicleState,
80 : : physics::Duration const &timeToStopSpeedMin,
81 : : ::ad::geometry::Polygon &brakePolygon,
82 : : physics::Duration const &timeToStopSpeedMax,
83 : : ::ad::geometry::Polygon &continueForwardPolygon) const
84 : : {
85 : 2 : TrajectorySetStep responseTimeFrontSide;
86 : 2 : TrajectorySetStep responseTimeBackSide;
87 [ + - ]: 2 : auto result = getResponseTimeTrajectoryPoints(vehicleState, responseTimeFrontSide, responseTimeBackSide);
88 [ - + ]: 2 : if (!result)
89 : : {
90 [ # # # # ]: 0 : core::getLogger()->debug(
91 : : "TrajectoryPedestrian::calculateTrajectorySets>> Could not calculate reponse time trajectory points.");
92 : : }
93 : : else
94 : : {
95 [ + - + - ]: 6 : core::getLogger()->trace(
96 : : "Trajectory points at response time: front left {}, front right {}, back left {}, back right {}",
97 [ + - ]: 2 : responseTimeFrontSide.left.size(),
98 : 2 : responseTimeFrontSide.right.size(),
99 : 2 : responseTimeBackSide.left.size(),
100 : 4 : responseTimeBackSide.right.size());
101 : : }
102 : :
103 : 2 : auto timeToStopBrakeMax = physics::Duration(0.);
104 [ + - - + ]: 2 : if (responseTimeBackSide.center.speed > physics::Speed(0.))
105 : : {
106 : : auto const timeAfterResponseTimeSpeedMin
107 [ # # # # ]: 0 : = std::max(physics::Duration(0.), timeToStopSpeedMin - vehicleState.dynamics.response_time);
108 [ # # ]: 0 : result = core::calculateTimeToStop(
109 : : responseTimeBackSide.center.speed,
110 : : timeAfterResponseTimeSpeedMin, // this is the time to stop with brakemin, therefore sufficient here
111 : : vehicleState.dynamics.max_speed_on_acceleration,
112 : : vehicleState.dynamics.alpha_lon.brake_max,
113 : : vehicleState.dynamics.alpha_lon.brake_max,
114 : : timeToStopBrakeMax);
115 [ # # ]: 0 : if (!result)
116 : : {
117 [ # # # # ]: 0 : core::getLogger()->debug(
118 : : "TrajectoryPedestrian::calculateTrajectorySets>> Could not calculate time to stop. speed {}, "
119 : : "timeAfterResponseTime {}",
120 : : responseTimeBackSide.center.speed,
121 : : timeAfterResponseTimeSpeedMin);
122 : : }
123 : : }
124 : :
125 : 2 : physics::Distance brakeMaxDistanceAfterResponseTime = physics::Distance(0.);
126 [ + - ]: 2 : if (result)
127 : : {
128 : 2 : physics::Speed unusedSpeed;
129 [ + - ]: 2 : result = core::calculateAcceleratedLimitedMovement(responseTimeBackSide.center.speed,
130 : : vehicleState.dynamics.max_speed_on_acceleration,
131 : : vehicleState.dynamics.alpha_lon.brake_max,
132 : : timeToStopBrakeMax,
133 : : unusedSpeed,
134 : : brakeMaxDistanceAfterResponseTime);
135 : : }
136 : :
137 : : auto const timeAfterResponseTimeSpeedMax
138 [ + - + - ]: 2 : = std::max(physics::Duration(0.), timeToStopSpeedMax - vehicleState.dynamics.response_time);
139 : 2 : physics::Distance brakeMinDistanceAfterResponseTime = physics::Distance(0.);
140 [ + - ]: 2 : if (result)
141 : : {
142 : 2 : physics::Speed unusedSpeed;
143 [ + - ]: 2 : result = core::calculateAcceleratedLimitedMovement(responseTimeFrontSide.center.speed,
144 : : vehicleState.dynamics.max_speed_on_acceleration,
145 : : vehicleState.dynamics.alpha_lon.brake_min,
146 : : timeAfterResponseTimeSpeedMax,
147 : : unusedSpeed,
148 : : brakeMinDistanceAfterResponseTime);
149 [ - + ]: 2 : if (!result)
150 : : {
151 [ # # # # ]: 0 : core::getLogger()->debug(
152 : : "TrajectoryPedestrian::calculateTrajectorySets>> Could not calculate speed and distance offset for "
153 : : "t={} and minBrake={}.",
154 : : timeAfterResponseTimeSpeedMax,
155 [ # # ]: 0 : vehicleState.dynamics.alpha_lon.brake_min);
156 : : }
157 : : }
158 : :
159 : 2 : physics::Distance accelMaxDistanceAfterResponseTime = physics::Distance(0.);
160 [ + - ]: 2 : if (result)
161 : : {
162 : 2 : physics::Speed unusedSpeed;
163 [ + - ]: 2 : result = core::calculateAcceleratedLimitedMovement(responseTimeFrontSide.center.speed,
164 : : vehicleState.dynamics.max_speed_on_acceleration,
165 : : vehicleState.dynamics.alpha_lon.accel_max,
166 : : timeAfterResponseTimeSpeedMax,
167 : : unusedSpeed,
168 : : accelMaxDistanceAfterResponseTime);
169 [ - + ]: 2 : if (!result)
170 : : {
171 [ # # # # ]: 0 : core::getLogger()->debug(
172 : : "TrajectoryPedestrian::calculateTrajectorySets>> Could not calculate speed and distance offset for "
173 : : "t={} and accel_max={}.",
174 : : timeAfterResponseTimeSpeedMax,
175 [ # # ]: 0 : vehicleState.dynamics.alpha_lon.accel_max);
176 : : }
177 : : }
178 : :
179 : : //=======================
180 : : // calculate brakePolygon
181 : : //=======================
182 : : //-------------
183 : : // brake front
184 : : //-------------
185 : 2 : TrajectorySetStep brakeFront;
186 [ + - ]: 2 : calculateTrajectoryPointsStraight(brakeMinDistanceAfterResponseTime, responseTimeFrontSide, brakeFront);
187 : :
188 : : //-------------
189 : : // brake sides
190 : : //-------------
191 : 2 : std::vector<TrajectorySetStep> brakeSideSteps;
192 : 2 : physics::Speed unusedSpeed;
193 : 2 : physics::Distance accelMaxDistanceAtResponseTime;
194 [ + - ]: 2 : result = core::calculateAcceleratedLimitedMovement(vehicleState.unstructured_object_state.speed_range.maximum,
195 : : vehicleState.dynamics.max_speed_on_acceleration,
196 : : vehicleState.dynamics.alpha_lon.accel_max,
197 : : vehicleState.dynamics.response_time,
198 : : unusedSpeed,
199 : : accelMaxDistanceAtResponseTime);
200 : : auto stepWidth = accelMaxDistanceAtResponseTime
201 [ + - ]: 2 : / (1.0 + vehicleState.dynamics.unstructured_settings.pedestrian_brake_intermediate_acceleration_steps);
202 [ - - + - : 2 : for (auto distance = stepWidth; distance < accelMaxDistanceAtResponseTime; distance += stepWidth)
- + ]
203 : : {
204 [ # # ]: 0 : auto left = TrajectoryPoint(vehicleState, TrajectoryPoint::SpeedMode::Max);
205 : 0 : auto right = left;
206 : 0 : auto center = left;
207 [ # # ]: 0 : calculateTrajectoryPoint(left, vehicleState.dynamics, distance, physics::RatioValue(1.0));
208 [ # # ]: 0 : calculateTrajectoryPoint(right, vehicleState.dynamics, distance, physics::RatioValue(-1.0));
209 [ # # ]: 0 : calculateTrajectoryPoint(center, vehicleState.dynamics, distance, physics::RatioValue(0.0));
210 : 0 : TrajectorySetStep step;
211 [ # # ]: 0 : step.left.push_back(left);
212 [ # # ]: 0 : step.right.push_back(right);
213 : 0 : step.center = center;
214 [ # # ]: 0 : brakeSideSteps.push_back(step);
215 : 0 : }
216 : :
217 : : //-------------
218 : : // brake back
219 : : //-------------
220 : 2 : TrajectorySetStepVehicleLocation brakeMaxStepVehicleLocation;
221 [ + - ]: 2 : if (result)
222 : : {
223 : 2 : TrajectoryPoint finalRightMaxBrakeDistance;
224 [ + - ]: 2 : calculateTrajectoryPointStraight(
225 : 2 : responseTimeBackSide.right.front(), brakeMaxDistanceAfterResponseTime, finalRightMaxBrakeDistance);
226 : 2 : TrajectoryPoint finalLeftMaxBrakeDistance;
227 [ + - ]: 2 : calculateTrajectoryPointStraight(
228 : 2 : responseTimeBackSide.left.back(), brakeMaxDistanceAfterResponseTime, finalLeftMaxBrakeDistance);
229 : :
230 [ + - ]: 2 : auto finalRightMaxBrakeDistanceLocation = TrafficParticipantLocation(finalRightMaxBrakeDistance, vehicleState);
231 [ + - ]: 2 : auto finalLeftMaxBrakeDistanceLocation = TrafficParticipantLocation(finalLeftMaxBrakeDistance, vehicleState);
232 : :
233 : 2 : TrajectoryPoint finalCenterMaxBrakeDistance;
234 [ + - ]: 2 : calculateTrajectoryPointStraight(
235 : : responseTimeBackSide.center, brakeMaxDistanceAfterResponseTime, finalCenterMaxBrakeDistance);
236 [ + - ]: 2 : brakeMaxStepVehicleLocation.center = TrafficParticipantLocation(finalCenterMaxBrakeDistance, vehicleState);
237 : 2 : brakeMaxStepVehicleLocation.left = finalLeftMaxBrakeDistanceLocation;
238 : 2 : brakeMaxStepVehicleLocation.right = finalRightMaxBrakeDistanceLocation;
239 : :
240 : 2 : ::ad::geometry::MultiPoint back;
241 [ + - ]: 2 : boost::geometry::append(back, finalRightMaxBrakeDistanceLocation.backRight);
242 [ + - ]: 2 : boost::geometry::append(back, finalRightMaxBrakeDistanceLocation.frontRight);
243 [ + - ]: 2 : boost::geometry::append(back, finalLeftMaxBrakeDistanceLocation.backLeft);
244 [ + - ]: 2 : boost::geometry::append(back, finalLeftMaxBrakeDistanceLocation.frontLeft);
245 [ + - ]: 2 : boost::geometry::convex_hull(back, brakePolygon);
246 : 2 : }
247 : :
248 [ + - ]: 2 : if (result)
249 : : {
250 : 2 : TrajectorySetStepVehicleLocation unusedStepVehicleLocation;
251 [ + - + - ]: 2 : result = calculateFrontAndSidePolygon(vehicleState,
252 : : brakeMaxStepVehicleLocation,
253 : : brakeSideSteps,
254 : : brakeFront,
255 : : "pedestrian_brake",
256 : : brakePolygon,
257 : : unusedStepVehicleLocation);
258 [ - + ]: 2 : if (!result)
259 : : {
260 [ # # # # ]: 0 : core::getLogger()->debug(
261 : : "TrajectoryPedestrian::calculateTrajectorySets>> Could not calculate continueForward polygon.");
262 : : }
263 : : }
264 : :
265 : : //-------------
266 : : // Close gap between pedestrian bounds and polygon:
267 : : // Depending on speed and response time the polygon detaches from the pedestrian
268 : : // this simple method fills the space between the brake polygon and the pedestrian without intermediate steps
269 : : //-------------
270 [ + - ]: 2 : if (result)
271 : : {
272 [ + - + - ]: 2 : result = calculateResponseTimePolygon(
273 : : vehicleState, brakeMaxStepVehicleLocation, "pedestrian_brake_response_time", brakePolygon);
274 [ - + ]: 2 : if (!result)
275 : : {
276 [ # # # # ]: 0 : core::getLogger()->debug("TrajectoryVehicle::calculateBrake>> Could not calculate simple response time polygon.");
277 : : }
278 : : }
279 : :
280 : : //=================================
281 : : // calculate continueForwardPolygon
282 : : //=================================
283 [ + - ]: 2 : continueForwardPolygon = brakePolygon;
284 : : //-------------
285 : : // continueForward front
286 : : //-------------
287 : 2 : TrajectorySetStep continueForwardFront;
288 [ + - ]: 2 : if (result)
289 : : {
290 [ + - ]: 2 : calculateTrajectoryPointsStraight(accelMaxDistanceAfterResponseTime, responseTimeFrontSide, continueForwardFront);
291 : :
292 : : // max left
293 : : auto ratioDiff = physics::RatioValue(2.0
294 : : / (2.0
295 : 2 : * vehicleState.dynamics.unstructured_settings
296 : 2 : .pedestrian_continue_forward_intermediate_heading_change_ratio_steps
297 : 2 : + 2.0));
298 [ + - + - : 4 : for (auto ratioValue = ratioDiff; ratioValue <= physics::RatioValue(1.0); ratioValue += ratioDiff)
+ + ]
299 : : {
300 : 2 : auto pt = responseTimeFrontSide.left.back();
301 [ + - ]: 2 : calculateTrajectoryPoint(pt, vehicleState.dynamics, accelMaxDistanceAfterResponseTime, ratioValue);
302 [ + - ]: 2 : continueForwardFront.left.push_back(pt);
303 : : }
304 : :
305 : : // max right
306 [ + - + - : 4 : for (auto ratioValue = physics::RatioValue(-1.0); (ratioValue < physics::RatioValue(0.0)); ratioValue += ratioDiff)
+ + ]
307 : : {
308 : 2 : auto pt = responseTimeFrontSide.right.front();
309 [ + - ]: 2 : calculateTrajectoryPoint(pt, vehicleState.dynamics, accelMaxDistanceAfterResponseTime, ratioValue);
310 [ + - ]: 2 : continueForwardFront.right.push_back(pt);
311 : : }
312 : : }
313 : :
314 : : //-------------
315 : : // continueForward sides
316 : : //-------------
317 : 2 : std::vector<TrajectorySetStep> continueForwardSideSteps;
318 [ + - ]: 2 : if (result)
319 : : {
320 : 0 : stepWidth = (accelMaxDistanceAfterResponseTime - brakeMaxDistanceAfterResponseTime)
321 [ + - + - ]: 2 : / (1.0 + vehicleState.dynamics.unstructured_settings.pedestrian_continue_forward_intermediate_acceleration_steps);
322 [ + - + + ]: 6 : for (auto distance = brakeMaxDistanceAfterResponseTime; distance <= accelMaxDistanceAfterResponseTime;
323 [ + - ]: 4 : distance += stepWidth)
324 : : {
325 : 4 : auto left = responseTimeFrontSide.left.back();
326 : 4 : auto right = responseTimeFrontSide.right.front();
327 : 4 : auto center = responseTimeFrontSide.center;
328 [ + - ]: 4 : calculateTrajectoryPoint(left, vehicleState.dynamics, distance, physics::RatioValue(1.0));
329 [ + - ]: 4 : calculateTrajectoryPoint(right, vehicleState.dynamics, distance, physics::RatioValue(-1.0));
330 [ + - ]: 4 : calculateTrajectoryPoint(center, vehicleState.dynamics, distance, physics::RatioValue(0.0));
331 : 4 : TrajectorySetStep step;
332 [ + - ]: 4 : step.left.push_back(left);
333 [ + - ]: 4 : step.right.push_back(right);
334 : 4 : step.center = center;
335 [ + - ]: 4 : continueForwardSideSteps.push_back(step);
336 : 4 : }
337 : : }
338 [ + - ]: 2 : if (result)
339 : : {
340 : 2 : TrajectorySetStepVehicleLocation unusedStepVehicleLocation;
341 [ + - + - ]: 2 : result = calculateFrontAndSidePolygon(vehicleState,
342 : : brakeMaxStepVehicleLocation,
343 : : continueForwardSideSteps,
344 : : continueForwardFront,
345 : : "pedestrian_continue_forward",
346 : : continueForwardPolygon,
347 : : unusedStepVehicleLocation);
348 [ - + ]: 2 : if (!result)
349 : : {
350 [ # # # # ]: 0 : core::getLogger()->debug(
351 : : "TrajectoryPedestrian::calculateTrajectorySets>> Could not calculate continueForward polygon.");
352 : : }
353 : : }
354 : :
355 : : //-------------
356 : : // Close gap between pedestrian bounds and polygon:
357 : : // Depending on speed and response time the polygon detaches from the pedestrian
358 : : // this simple method fills the space between the brake polygon and the pedestrian without intermediate steps
359 : : //-------------
360 : : // no need to add this here, because in pedestrian case this is identical with what was already added to the brake
361 : : // polygon if (result)
362 : : // {
363 : : // result = calculateResponseTimePolygon(
364 : : // vehicleState, brakeMaxStepVehicleLocation, "pedestrian_continue_forward_response_time",
365 : : // continueForwardPolygon);
366 : : // if (!result)
367 : : // {
368 : : // core::getLogger()->debug("TrajectoryVehicle::calculateBrake>> Could not calculate simple response time
369 : : // polygon.");
370 : : // }
371 : : // }
372 : :
373 : 2 : return result;
374 : 2 : }
375 : :
376 : 0 : bool TrajectoryPedestrian::calculateTrajectorySetsStandingStill(core::RelativeObjectState const &vehicleState,
377 : : physics::Duration const &timeToStop,
378 : : ::ad::geometry::Polygon &brakePolygon,
379 : : ::ad::geometry::Polygon &continueForwardPolygon) const
380 : : {
381 : : // If pedestrian is standing, he might start walking in any direction
382 : 0 : physics::Speed speed;
383 : 0 : physics::Distance brakeMinMaxDistance;
384 [ # # ]: 0 : auto result = core::calculateSpeedAndDistanceOffset(timeToStop,
385 : : vehicleState.unstructured_object_state.speed_range.maximum,
386 : : vehicleState.dynamics.response_time,
387 : : vehicleState.dynamics.max_speed_on_acceleration,
388 : : vehicleState.dynamics.alpha_lon.accel_max,
389 : : vehicleState.dynamics.alpha_lon.brake_min,
390 : : speed,
391 : : brakeMinMaxDistance);
392 [ # # # # ]: 0 : ::ad::geometry::calculateCircleArc(::ad::geometry::toPoint(vehicleState.unstructured_object_state.center_point),
393 : : brakeMinMaxDistance,
394 : 0 : physics::Angle(0.),
395 : : physics::c2PI,
396 : : circleStepWidth,
397 : : brakePolygon);
398 : :
399 : 0 : physics::Distance accelMaxMaxDistance;
400 [ # # ]: 0 : if (result)
401 : : {
402 [ # # ]: 0 : result = core::calculateSpeedAndDistanceOffset(timeToStop,
403 : : vehicleState.unstructured_object_state.speed_range.maximum,
404 : : vehicleState.dynamics.response_time,
405 : : vehicleState.dynamics.max_speed_on_acceleration,
406 : : vehicleState.dynamics.alpha_lon.accel_max,
407 : : vehicleState.dynamics.alpha_lon.accel_max,
408 : : speed,
409 : : accelMaxMaxDistance);
410 [ # # # # ]: 0 : ::ad::geometry::calculateCircleArc(::ad::geometry::toPoint(vehicleState.unstructured_object_state.center_point),
411 : : accelMaxMaxDistance,
412 : 0 : physics::Angle(0.),
413 : : physics::c2PI,
414 : : circleStepWidth,
415 : : continueForwardPolygon);
416 : : }
417 : :
418 : 0 : return result;
419 : : }
420 : :
421 : 0 : ::ad::geometry::Polygon TrajectoryPedestrian::calculateSidePolygon(core::RelativeObjectState const &vehicleState,
422 : : TrajectoryPoint const &finalPointMin,
423 : : TrajectoryPoint const &finalPointMax) const
424 : : {
425 : 0 : ::ad::geometry::MultiPoint side;
426 [ # # # # ]: 0 : boost::geometry::append(side, getVehicleCorner(finalPointMax, vehicleState, VehicleCorner::frontRight));
427 [ # # # # ]: 0 : boost::geometry::append(side, getVehicleCorner(finalPointMax, vehicleState, VehicleCorner::frontLeft));
428 [ # # # # ]: 0 : boost::geometry::append(side, getVehicleCorner(finalPointMin, vehicleState, VehicleCorner::backLeft));
429 [ # # # # ]: 0 : boost::geometry::append(side, getVehicleCorner(finalPointMin, vehicleState, VehicleCorner::backRight));
430 : :
431 [ # # ]: 0 : ::ad::geometry::Polygon hull;
432 [ # # ]: 0 : boost::geometry::convex_hull(side, hull);
433 : 0 : return hull;
434 : 0 : }
435 : :
436 : 18 : void TrajectoryPedestrian::calculateTrajectoryPointStraight(TrajectoryPoint const ¤tPoint,
437 : : physics::Distance const &distance,
438 : : TrajectoryPoint &resultPoint) const
439 : : {
440 : 18 : resultPoint = currentPoint;
441 [ + - + + ]: 18 : if (distance > physics::Distance(0.))
442 : : {
443 : 12 : resultPoint.position = currentPoint.position
444 [ + - + - : 12 : + ::ad::geometry::toPoint(std::cos(currentPoint.angle) * distance, std::sin(currentPoint.angle) * distance);
+ - + - +
- ]
445 : : }
446 : 18 : }
447 : :
448 : 4 : void TrajectoryPedestrian::calculateTrajectoryPointsStraight(physics::Distance const &distance,
449 : : TrajectorySetStep const &step,
450 : : TrajectorySetStep &resultStep) const
451 : : {
452 [ + - + - ]: 4 : if (distance > physics::Distance(0.))
453 : : {
454 [ + + ]: 8 : for (auto const &left : step.left)
455 : : {
456 : 4 : TrajectoryPoint pt;
457 [ + - ]: 4 : calculateTrajectoryPointStraight(left, distance, pt);
458 [ + - ]: 4 : resultStep.left.push_back(pt);
459 : : }
460 [ + + ]: 8 : for (auto const &right : step.right)
461 : : {
462 : 4 : TrajectoryPoint pt;
463 [ + - ]: 4 : calculateTrajectoryPointStraight(right, distance, pt);
464 [ + - ]: 4 : resultStep.right.push_back(pt);
465 : : }
466 : 4 : calculateTrajectoryPointStraight(step.center, distance, resultStep.center);
467 : : }
468 : : else
469 : : {
470 : 0 : resultStep = step;
471 : : }
472 : 4 : }
473 : :
474 : 2 : bool TrajectoryPedestrian::getResponseTimeTrajectoryPoints(core::RelativeObjectState const &vehicleState,
475 : : TrajectorySetStep &frontSide,
476 : : TrajectorySetStep &backSide) const
477 : : {
478 : 2 : auto result = true;
479 : : //-------------
480 : : // back
481 : : //-------------
482 : : auto ratioDiffBack = physics::RatioValue(
483 : : 2.0
484 : 2 : / (2.0 * vehicleState.dynamics.unstructured_settings.pedestrian_back_intermediate_heading_change_ratio_steps
485 : 2 : + 2.0));
486 [ + - + + : 8 : for (auto ratioValue = physics::RatioValue(-1.0); (ratioValue <= physics::RatioValue(1.0)) && result;
+ - + + ]
487 [ + - ]: 6 : ratioValue += ratioDiffBack)
488 : : {
489 [ + - ]: 6 : TrajectoryPoint pt(vehicleState, TrajectoryPoint::SpeedMode::Min);
490 : 12 : result = calculateTrajectoryPoint(pt,
491 : 6 : vehicleState.dynamics,
492 : 6 : vehicleState.dynamics.response_time,
493 [ + - ]: 6 : vehicleState.dynamics.alpha_lon.brake_max,
494 : : ratioValue);
495 [ + - + + ]: 6 : if (ratioValue == physics::RatioValue(0.))
496 : : {
497 : 2 : backSide.center = pt;
498 : : }
499 [ + - + + ]: 4 : else if (ratioValue > physics::RatioValue(0.))
500 : : {
501 [ + - ]: 2 : backSide.left.push_back(pt);
502 : : }
503 : : else
504 : : {
505 [ + - ]: 2 : backSide.right.push_back(pt);
506 : : }
507 : : }
508 : :
509 : : //-------------
510 : : // front
511 : : //-------------
512 : : auto ratioDiffFront = physics::RatioValue(
513 : : 2.0
514 : 2 : / (2.0 * vehicleState.dynamics.unstructured_settings.pedestrian_front_intermediate_heading_change_ratio_steps
515 : 2 : + 2.0));
516 [ + - + + : 8 : for (auto ratioValue = physics::RatioValue(-1.0); (ratioValue <= physics::RatioValue(1.0)) && result;
+ - + + ]
517 [ + - ]: 6 : ratioValue += ratioDiffFront)
518 : : {
519 [ + - ]: 6 : TrajectoryPoint pt(vehicleState, TrajectoryPoint::SpeedMode::Max);
520 : 12 : result = calculateTrajectoryPoint(pt,
521 : 6 : vehicleState.dynamics,
522 : 6 : vehicleState.dynamics.response_time,
523 [ + - ]: 6 : vehicleState.dynamics.alpha_lon.accel_max,
524 : : ratioValue);
525 [ + - + + ]: 6 : if (ratioValue == physics::RatioValue(0.))
526 : : {
527 : 2 : frontSide.center = pt;
528 : : }
529 [ + - + + ]: 4 : else if (ratioValue > physics::RatioValue(0.))
530 : : {
531 [ + - ]: 2 : frontSide.left.push_back(pt);
532 : : }
533 : : else
534 : : {
535 [ + - ]: 2 : frontSide.right.push_back(pt);
536 : : }
537 : : }
538 : :
539 : 2 : return result;
540 : : }
541 : :
542 : 12 : bool TrajectoryPedestrian::calculateTrajectoryPoint(TrajectoryPoint ¤tPoint,
543 : : world::RssDynamics const &dynamics,
544 : : physics::Duration const &duration,
545 : : ad::physics::Acceleration const &acceleration,
546 : : ad::physics::RatioValue const &angleChangeRatio) const
547 : : {
548 : 12 : ad::physics::Distance distance;
549 : 24 : auto result = core::calculateAcceleratedLimitedMovement(
550 [ + - ]: 12 : currentPoint.speed, dynamics.max_speed_on_acceleration, acceleration, duration, currentPoint.speed, distance);
551 : :
552 [ + - ]: 12 : calculateTrajectoryPoint(currentPoint, dynamics, distance, angleChangeRatio);
553 : 12 : return result;
554 : : }
555 : :
556 : 28 : void TrajectoryPedestrian::calculateTrajectoryPoint(TrajectoryPoint ¤tPoint,
557 : : world::RssDynamics const &dynamics,
558 : : physics::Distance const &distance,
559 : : ad::physics::RatioValue const &angleChangeRatio) const
560 : : {
561 [ + - + + ]: 28 : if (distance == physics::Distance(0.))
562 : : {
563 : 6 : return;
564 : : }
565 : :
566 [ + + ]: 22 : if (std::fabs(angleChangeRatio.mRatioValue) > dynamics.unstructured_settings.pedestrian_turning_radius / maxRadius)
567 : : {
568 : : // move on circle
569 [ + - ]: 16 : auto radius = dynamics.unstructured_settings.pedestrian_turning_radius / angleChangeRatio;
570 : :
571 [ + - ]: 16 : auto startingAngle = currentPoint.angle - ad::physics::cPI_2;
572 [ + - ]: 16 : auto circleOrigin = ::ad::geometry::getCircleOrigin(currentPoint.position, radius, startingAngle);
573 : :
574 [ + - ]: 16 : auto angleChange = ad::physics::Angle(distance / radius);
575 : :
576 [ + - + - ]: 16 : currentPoint.position = ::ad::geometry::getPointOnCircle(circleOrigin, radius, startingAngle + angleChange);
577 [ + - ]: 16 : currentPoint.angle += angleChange;
578 : : }
579 : : else
580 : : {
581 : : // straight line
582 : 6 : currentPoint.position = currentPoint.position
583 [ + - + - : 6 : + ::ad::geometry::toPoint(std::cos(currentPoint.angle) * distance, std::sin(currentPoint.angle) * distance);
+ - + - +
- ]
584 : : }
585 : : }
586 : :
587 : : } // namespace unstructured
588 : : } // namespace rss
589 : : } // namespace ad
|
