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 "TrajectoryVehicle.hpp"
13 : : #include <ad/physics/Operation.hpp>
14 : : #include <algorithm>
15 : : #include <chrono>
16 : : #include "ad/rss/unstructured/DebugDrawing.hpp"
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 : 20 : bool TrajectoryVehicle::calculateTrajectorySets(situation::VehicleState const &vehicleState,
31 : : Polygon &brakePolygon,
32 : : Polygon &continueForwardPolygon)
33 : : {
34 : 20 : ad::physics::Duration timeToStop;
35 [ + - ]: 20 : auto result = situation::calculateTimeToStop(vehicleState.objectState.speed,
36 : : vehicleState.dynamics.responseTime,
37 : : vehicleState.dynamics.maxSpeedOnAcceleration,
38 : : vehicleState.dynamics.alphaLon.accelMax,
39 : : vehicleState.dynamics.alphaLon.brakeMin,
40 : : timeToStop);
41 : :
42 : 40 : TrajectorySetStep responseTimeFrontSide;
43 : 20 : TrajectorySetStep responseTimeBackSide;
44 [ + - ]: 20 : if (result)
45 : : {
46 [ + - ]: 20 : result = getResponseTimeTrajectoryPoints(vehicleState, responseTimeFrontSide, responseTimeBackSide);
47 [ - + ]: 20 : if (!result)
48 : : {
49 [ # # ]: 0 : spdlog::debug("TrajectoryVehicle::calculateTrajectorySets>> Could not calculate reponse time trajectory points.");
50 : : }
51 : : else
52 : : {
53 [ + - ]: 20 : spdlog::trace("Trajectory points at response time: front left {}, front right {}, back left {}, back right {}",
54 : 20 : responseTimeFrontSide.left.size(),
55 : 20 : responseTimeFrontSide.right.size(),
56 : 20 : responseTimeBackSide.left.size(),
57 : 40 : responseTimeBackSide.right.size());
58 : : }
59 : : }
60 : :
61 : 20 : TrajectorySetStepVehicleLocation brakeMinStepVehicleLocations;
62 [ + - ]: 20 : auto timeAfterResponseTime = timeToStop - vehicleState.dynamics.responseTime;
63 [ + - ]: 20 : if (result)
64 : : {
65 [ + - ]: 20 : result = calculateBrake(vehicleState,
66 : : timeAfterResponseTime,
67 : : responseTimeFrontSide,
68 : : responseTimeBackSide,
69 : : brakePolygon,
70 : : brakeMinStepVehicleLocations);
71 [ - + ]: 20 : if (!result)
72 : : {
73 [ # # ]: 0 : spdlog::debug("TrajectoryVehicle::calculateTrajectorySets>> calculateBrake() failed.");
74 : : }
75 : : }
76 : :
77 [ + - ]: 20 : if (result)
78 : : {
79 [ + - ]: 20 : result = calculateContinueForward(vehicleState,
80 : : timeAfterResponseTime,
81 : : responseTimeFrontSide,
82 : : brakePolygon,
83 : : brakeMinStepVehicleLocations,
84 : : continueForwardPolygon);
85 [ - + ]: 20 : if (!result)
86 : : {
87 : : // fallback
88 [ # # ]: 0 : spdlog::warn("TrajectoryVehicle::calculateTrajectorySets>> calculateContinueForward() failed. Use brakePolygon "
89 : : "as fallback.");
90 : 0 : result = true;
91 [ # # ]: 0 : continueForwardPolygon = brakePolygon;
92 : : }
93 : : }
94 [ + - + - : 20 : DEBUG_DRAWING_POLYGON(brakePolygon, "red", "vehicle_brake");
+ - + - ]
95 [ + - + - : 20 : DEBUG_DRAWING_POLYGON(continueForwardPolygon, "green", "vehicle_continue_forward");
+ - + - ]
96 : 40 : return result;
97 : : }
98 : :
99 : : ad::physics::AngularVelocity
100 : 3920 : TrajectoryVehicle::calculateYawRate(ad::physics::AngularVelocity const &yawRate,
101 : : ad::physics::Duration const &timeInMovement,
102 : : ad::physics::AngularAcceleration const &maxYawRateChange,
103 : : ad::physics::RatioValue const &ratio) const
104 : : {
105 [ + - + - ]: 7840 : return yawRate + maxYawRateChange * timeInMovement * ratio;
106 : : }
107 : :
108 : 20 : bool TrajectoryVehicle::getResponseTimeTrajectoryPoints(situation::VehicleState const &vehicleState,
109 : : TrajectorySetStep &frontSide,
110 : : TrajectorySetStep &backSide) const
111 : : {
112 : : //-------------
113 : : // back
114 : : //-------------
115 : : auto ratioDiffBack = physics::RatioValue(
116 : 20 : 2.0 / (2.0 * vehicleState.dynamics.unstructuredSettings.vehicleBackIntermediateYawRateChangeRatioSteps + 2.0));
117 [ + - ]: 20 : backSide.left.reserve(vehicleState.dynamics.unstructuredSettings.vehicleBackIntermediateYawRateChangeRatioSteps + 1);
118 [ + - ]: 20 : backSide.right.reserve(vehicleState.dynamics.unstructuredSettings.vehicleBackIntermediateYawRateChangeRatioSteps + 1);
119 : : auto result
120 [ + - ]: 20 : = getResponseTimeTrajectoryPoints(vehicleState, vehicleState.dynamics.alphaLon.brakeMax, ratioDiffBack, backSide);
121 : :
122 : : //-------------
123 : : // front
124 : : //-------------
125 [ + - ]: 20 : if (result)
126 : : {
127 : : auto ratioDiffFront = physics::RatioValue(
128 : 20 : 2.0 / (2.0 * vehicleState.dynamics.unstructuredSettings.vehicleFrontIntermediateYawRateChangeRatioSteps + 2.0));
129 : 20 : frontSide.left.reserve(vehicleState.dynamics.unstructuredSettings.vehicleFrontIntermediateYawRateChangeRatioSteps
130 [ + - ]: 20 : + 1);
131 : 20 : frontSide.right.reserve(vehicleState.dynamics.unstructuredSettings.vehicleFrontIntermediateYawRateChangeRatioSteps
132 [ + - ]: 20 : + 1);
133 : 20 : result = getResponseTimeTrajectoryPoints(
134 [ + - ]: 20 : vehicleState, vehicleState.dynamics.alphaLon.accelMax, ratioDiffFront, frontSide);
135 : : }
136 : 20 : return result;
137 : : }
138 : :
139 : 40 : bool TrajectoryVehicle::getResponseTimeTrajectoryPoints(situation::VehicleState const &vehicleState,
140 : : physics::Acceleration const &acceleration,
141 : : physics::RatioValue const &ratioDiff,
142 : : TrajectorySetStep &step) const
143 : : {
144 : 40 : physics::Duration timeInMovementUntilResponseTime = vehicleState.dynamics.responseTime;
145 [ + - ]: 40 : auto result = getTimeInMovement(vehicleState.objectState.speed, acceleration, timeInMovementUntilResponseTime);
146 : :
147 : : // right
148 [ + - + + : 140 : for (auto ratioValue = physics::RatioValue(-1.0); (ratioValue < physics::RatioValue(0.0)) && result;
+ - + + ]
149 [ + - ]: 100 : ratioValue += ratioDiff)
150 : : {
151 [ + - ]: 100 : auto currentPoint = TrajectoryPoint(vehicleState);
152 : 100 : TrajectoryPoint pt;
153 : 200 : result = calculateTrajectoryPoint(
154 [ + - ]: 100 : currentPoint, vehicleState.dynamics, timeInMovementUntilResponseTime, acceleration, ratioValue, pt);
155 [ + - ]: 100 : step.right.push_back(pt);
156 : : }
157 : :
158 : : // left
159 [ + - + + : 140 : for (auto ratioValue = ratioDiff; (ratioValue <= physics::RatioValue(1.0)) && result; ratioValue += ratioDiff)
+ - + + +
- ]
160 : : {
161 [ + - ]: 100 : auto currentPoint = TrajectoryPoint(vehicleState);
162 : 100 : TrajectoryPoint pt;
163 : 200 : result = calculateTrajectoryPoint(
164 [ + - ]: 100 : currentPoint, vehicleState.dynamics, timeInMovementUntilResponseTime, acceleration, ratioValue, pt);
165 [ + - ]: 100 : step.left.push_back(pt);
166 : : }
167 : :
168 : : // center
169 [ + - ]: 40 : if (result)
170 : : {
171 [ + - ]: 40 : result = calculateTrajectoryPoint(TrajectoryPoint(vehicleState),
172 [ + - ]: 40 : vehicleState.dynamics,
173 : : timeInMovementUntilResponseTime,
174 : : acceleration,
175 : 40 : physics::RatioValue(0.),
176 : 40 : step.center);
177 : : }
178 : :
179 : 40 : return result;
180 : : }
181 : :
182 : 100 : bool TrajectoryVehicle::getTimeInMovement(ad::physics::Speed const &speed,
183 : : ad::physics::Acceleration const &acceleration,
184 : : ad::physics::Duration &timeInMovement) const
185 : : {
186 : 100 : auto result = true;
187 [ + - + + ]: 100 : if (acceleration < physics::Acceleration(0.))
188 : : {
189 [ + - + + ]: 60 : if (speed == physics::Speed(0.))
190 : : {
191 : 9 : timeInMovement = physics::Duration(0.);
192 : : }
193 : : else
194 : : {
195 : 51 : physics::Duration timeToStop;
196 : : result
197 [ + - ]: 51 : = situation::calculateTimeToStop(speed, physics::Duration(0.), speed, acceleration, acceleration, timeToStop);
198 : :
199 [ + - ]: 51 : timeInMovement = std::min(timeInMovement, timeToStop);
200 : : }
201 : : }
202 : 100 : return result;
203 : : }
204 : :
205 : 440 : bool TrajectoryVehicle::calculateTrajectoryPoint(TrajectoryPoint const ¤tPoint,
206 : : world::RssDynamics const &dynamics,
207 : : ad::physics::Duration const &duration,
208 : : ad::physics::Acceleration const &acceleration,
209 : : ad::physics::RatioValue const &yawRateChangeRatio,
210 : : TrajectoryPoint &resultTrajectoryPoint) const
211 : : {
212 : 440 : auto result = true;
213 : 440 : resultTrajectoryPoint = currentPoint;
214 : :
215 : 440 : auto currentTime = ad::physics::Duration(0.0);
216 : :
217 [ + - + + : 4360 : while ((currentTime < duration) && result)
+ - + + ]
218 : : {
219 : 3920 : auto timeStep = dynamics.unstructuredSettings.vehicleTrajectoryCalculationStep;
220 [ + - + - : 3920 : if (duration >= currentTime + dynamics.unstructuredSettings.vehicleTrajectoryCalculationStep)
+ + ]
221 : : {
222 [ + - ]: 3717 : currentTime += dynamics.unstructuredSettings.vehicleTrajectoryCalculationStep;
223 : : }
224 : : else
225 : : {
226 [ + - ]: 203 : timeStep = duration - currentTime;
227 : 203 : currentTime = duration;
228 : : }
229 : :
230 : : resultTrajectoryPoint.yawRate = calculateYawRate(
231 [ + - ]: 3920 : currentPoint.yawRate, currentTime, dynamics.unstructuredSettings.vehicleYawRateChange, yawRateChangeRatio);
232 : :
233 [ + - ]: 3920 : result = calculateTrajectoryPointOnCircle(resultTrajectoryPoint, acceleration, timeStep, dynamics);
234 : : }
235 : :
236 : 440 : return result;
237 : : }
238 : :
239 : 4480 : bool TrajectoryVehicle::calculateTrajectoryPointOnCircle(TrajectoryPoint ¤tPoint,
240 : : physics::Acceleration const &acceleration,
241 : : physics::Duration const &duration,
242 : : ::ad::rss::world::RssDynamics const &dynamics) const
243 : : {
244 : 4480 : ad::physics::Distance currentDistance;
245 : 4480 : physics::Speed finalSpeed;
246 : :
247 [ + - + + ]: 4480 : if (duration == physics::Duration(0.))
248 : : {
249 : 60 : return true;
250 : : }
251 : :
252 [ + - ]: 4420 : auto result = situation::calculateAcceleratedLimitedMovement(
253 : : currentPoint.speed, dynamics.maxSpeedOnAcceleration, acceleration, duration, finalSpeed, currentDistance);
254 [ - + ]: 4420 : if (!result)
255 : : {
256 [ # # ]: 0 : spdlog::debug(
257 : : "TrajectoryVehicle::calculateTrajectoryPointOnCircle>> calculateAcceleratedLimitedMovement() failed.");
258 : : }
259 : :
260 : 4420 : auto currentRadius = physics::Distance::getMax();
261 [ + - + + ]: 4420 : if (currentPoint.yawRate != physics::AngularVelocity(0.))
262 : : {
263 : : currentRadius
264 : 4100 : = ad::physics::Distance(static_cast<double>(currentPoint.speed) / static_cast<double>(currentPoint.yawRate));
265 : : }
266 : :
267 [ + - + + ]: 4420 : if (std::fabs(currentRadius) < dynamics.unstructuredSettings.vehicleMinRadius)
268 : : {
269 [ + - + + ]: 78 : if (currentRadius > physics::Distance(0.))
270 : : {
271 : 3 : currentRadius = dynamics.unstructuredSettings.vehicleMinRadius;
272 : : }
273 : : else
274 : : {
275 [ + - ]: 75 : currentRadius = -dynamics.unstructuredSettings.vehicleMinRadius;
276 : : }
277 : : }
278 : :
279 : : Point const circleOrigin
280 [ + - + - ]: 4420 : = getCircleOrigin(currentPoint.position, currentRadius, currentPoint.angle - ad::physics::cPI_2);
281 [ + - ]: 4420 : auto diffAngle = physics::Angle(currentDistance / currentRadius);
282 : :
283 [ + - ]: 4420 : currentPoint.angle += diffAngle;
284 [ + - + - ]: 4420 : currentPoint.position = rotateAroundPoint(circleOrigin, currentPoint.position - circleOrigin, diffAngle);
285 : 4420 : currentPoint.speed = finalSpeed;
286 : 4420 : return result;
287 : : }
288 : :
289 : 20 : bool TrajectoryVehicle::calculateBrake(situation::VehicleState const &vehicleState,
290 : : ad::physics::Duration const &timeAfterResponseTime,
291 : : TrajectorySetStep const &responseTimeFrontSide,
292 : : TrajectorySetStep const &responseTimeBackSide,
293 : : Polygon &resultPolygon,
294 : : TrajectorySetStepVehicleLocation &brakeMinStepVehicleLocation) const
295 : : {
296 : : //-------------
297 : : // back
298 : : //-------------
299 : 20 : auto timeToStopBrakeMax = physics::Duration(0.);
300 : 20 : TrajectorySetStepVehicleLocation brakeMaxVehicleLocations;
301 : 20 : auto result = true;
302 : :
303 [ + - - + ]: 20 : if (responseTimeBackSide.center.speed > physics::Speed(0.))
304 : : {
305 [ # # ]: 0 : result = situation::calculateTimeToStop(responseTimeBackSide.center.speed,
306 : : timeAfterResponseTime,
307 : : vehicleState.dynamics.maxSpeedOnAcceleration,
308 : : vehicleState.dynamics.alphaLon.brakeMax,
309 : : vehicleState.dynamics.alphaLon.brakeMax,
310 : : timeToStopBrakeMax);
311 [ # # ]: 0 : if (!result)
312 : : {
313 [ # # ]: 0 : spdlog::debug(
314 : : "TrajectoryVehicle::calculateBrake>> Could not calculate time to stop. speed {}, timeAfterResponseTime {}",
315 : 0 : responseTimeBackSide.center.speed,
316 : : timeAfterResponseTime);
317 : : }
318 : : }
319 [ + - ]: 20 : auto backSide = responseTimeBackSide;
320 [ + - ]: 20 : if (result)
321 : : {
322 : 20 : calculateTrajectorySetStepOnCircle(
323 [ + - ]: 20 : vehicleState, timeToStopBrakeMax, vehicleState.dynamics.alphaLon.brakeMax, backSide);
324 : : }
325 [ + - ]: 20 : if (result)
326 : : {
327 : : result
328 [ + - + - ]: 20 : = calculateStepPolygon(vehicleState, backSide, "vehicle_brake_brakeMax", resultPolygon, brakeMaxVehicleLocations);
329 [ - + ]: 20 : if (!result)
330 : : {
331 [ # # ]: 0 : spdlog::debug("TrajectoryVehicle::calculateBrake>> Could not calculate brake max step polygon.");
332 : : }
333 : : }
334 : :
335 : : //-------------
336 : : // front + sides
337 : : //-------------
338 [ + - ]: 20 : if (result)
339 : : {
340 : : // front
341 [ + - ]: 40 : auto front = responseTimeFrontSide;
342 [ + - ]: 20 : if (result)
343 : : {
344 : 20 : result = calculateTrajectorySetStepOnCircle(
345 [ + - ]: 20 : vehicleState, timeAfterResponseTime, vehicleState.dynamics.alphaLon.brakeMin, front);
346 : : }
347 : :
348 : : // sides
349 : 40 : std::vector<physics::Acceleration> accelerations;
350 [ + - ]: 20 : auto accelStepSize = (-vehicleState.dynamics.alphaLon.brakeMax + vehicleState.dynamics.alphaLon.brakeMin)
351 [ + - + - ]: 40 : / (1.0 + vehicleState.dynamics.unstructuredSettings.vehicleBrakeIntermediateAccelerationSteps);
352 [ + - ]: 40 : for (auto acceleration = vehicleState.dynamics.alphaLon.brakeMax + accelStepSize;
353 [ + - + + ]: 40 : acceleration < vehicleState.dynamics.alphaLon.brakeMin;
354 [ + - ]: 20 : acceleration += accelStepSize)
355 : : {
356 [ + - ]: 20 : accelerations.push_back(acceleration);
357 : : }
358 [ + - ]: 20 : accelerations.push_back(vehicleState.dynamics.alphaLon.brakeMin);
359 : :
360 : 40 : std::vector<TrajectorySetStep> sideSteps;
361 [ + + + - : 60 : for (auto itAcceleration = accelerations.cbegin(); (itAcceleration != accelerations.cend()) && result;
+ + ]
362 : 40 : ++itAcceleration)
363 : : {
364 : 40 : physics::Duration calculationTime = timeAfterResponseTime;
365 [ + - ]: 40 : result = getTimeInMovement(responseTimeFrontSide.center.speed, *itAcceleration, calculationTime);
366 : :
367 [ + - ]: 40 : if (result)
368 : : {
369 : 80 : TrajectorySetStep sideStep;
370 : 40 : sideStep.center = responseTimeFrontSide.center;
371 [ + - ]: 40 : sideStep.left.push_back(responseTimeFrontSide.left.back());
372 [ + - ]: 40 : sideStep.right.push_back(responseTimeFrontSide.right.front());
373 : :
374 [ + - ]: 40 : result = calculateTrajectorySetStepOnCircle(vehicleState, calculationTime, *itAcceleration, sideStep);
375 [ + - ]: 40 : sideSteps.push_back(sideStep);
376 : : }
377 [ - + ]: 40 : if (!result)
378 : : {
379 [ # # ]: 0 : spdlog::debug("TrajectoryVehicle::calculateFrontAndSidePolygon>> Could not calculate step polygon for "
380 : : "speed {}, acceleration {}, calcTime {}",
381 : 0 : responseTimeFrontSide.center.speed,
382 : 0 : *itAcceleration,
383 : : calculationTime);
384 : : }
385 : : }
386 : :
387 [ + - + - ]: 20 : result = calculateFrontAndSidePolygon(vehicleState,
388 : : brakeMaxVehicleLocations,
389 : : sideSteps,
390 : : front,
391 : : "vehicle_brake",
392 : : resultPolygon,
393 : : brakeMinStepVehicleLocation);
394 [ - + ]: 20 : if (!result)
395 : : {
396 [ # # ]: 0 : spdlog::debug("TrajectoryVehicle::calculateBrake>> Could not calculate front and side polygon.");
397 : : }
398 : : }
399 : 40 : return result;
400 : : }
401 : :
402 : 20 : bool TrajectoryVehicle::calculateContinueForward(situation::VehicleState const &vehicleState,
403 : : physics::Duration const &timeAfterResponseTime,
404 : : TrajectorySetStep const &responseTimeFrontSide,
405 : : Polygon const &brakePolygon,
406 : : TrajectorySetStepVehicleLocation const &brakeMinStepVehicleLocation,
407 : : Polygon &resultPolygon) const
408 : : {
409 : : //-----------
410 : : // Front
411 : : //-----------
412 : : auto ratioDiffFront = physics::RatioValue(
413 : : 2.0
414 : 20 : / (2.0 * vehicleState.dynamics.unstructuredSettings.vehicleContinueForwardIntermediateYawRateChangeRatioSteps
415 : 20 : + 2.0));
416 [ + - ]: 40 : auto front = responseTimeFrontSide;
417 : : // center-front, with no change of the current yaw rate
418 : 40 : auto result = calculateTrajectorySetStepOnCircle(
419 [ + - ]: 20 : vehicleState, timeAfterResponseTime, vehicleState.dynamics.alphaLon.accelMax, front);
420 [ + - - + ]: 20 : if (DEBUG_DRAWING_IS_ENABLED())
421 : : {
422 : 0 : int idx = 0;
423 [ # # ]: 0 : for (auto const &pt : front.left)
424 : : {
425 [ # # # # : 0 : DEBUG_DRAWING_POLYGON(TrafficParticipantLocation(pt, vehicleState).toPolygon(),
# # # # #
# # # #
# ]
426 : : "blue",
427 : : "vehicle_continue_forward_front_left_" + std::to_string(idx));
428 : 0 : idx++;
429 : : }
430 : 0 : idx = 0;
431 [ # # ]: 0 : for (auto const &pt : front.right)
432 : : {
433 [ # # # # : 0 : DEBUG_DRAWING_POLYGON(TrafficParticipantLocation(pt, vehicleState).toPolygon(),
# # # # #
# # # #
# ]
434 : : "blue",
435 : : "vehicle_continue_forward_front_right_" + std::to_string(idx));
436 : 0 : idx++;
437 : : }
438 [ # # # # : 0 : DEBUG_DRAWING_POLYGON(TrafficParticipantLocation(front.center, vehicleState).toPolygon(),
# # # # #
# # # ]
439 : : "blue",
440 : : "vehicle_continue_forward_front_center");
441 : : }
442 : :
443 : : // center-left, with maximum changing of the yaw rate
444 : 20 : front.left.reserve(
445 : 20 : front.left.size()
446 [ + - ]: 20 : + vehicleState.dynamics.unstructuredSettings.vehicleContinueForwardIntermediateYawRateChangeRatioSteps + 1);
447 [ + - + + : 100 : for (auto ratioValue = ratioDiffFront; (ratioValue <= physics::RatioValue(1.0)) && result;
+ - + + ]
448 [ + - ]: 80 : ratioValue += ratioDiffFront)
449 : : {
450 : 80 : TrajectoryPoint resultPt;
451 [ + - ]: 80 : result = calculateTrajectoryPoint(responseTimeFrontSide.left.back(),
452 : 80 : vehicleState.dynamics,
453 : : timeAfterResponseTime,
454 : 80 : vehicleState.dynamics.alphaLon.accelMax,
455 : : ratioValue,
456 : : resultPt);
457 [ + - ]: 80 : front.left.push_back(resultPt);
458 [ + - - + ]: 80 : if (DEBUG_DRAWING_IS_ENABLED())
459 : : {
460 [ # # # # : 0 : DEBUG_DRAWING_POLYGON(TrafficParticipantLocation(resultPt, vehicleState).toPolygon(),
# # # # #
# # # #
# ]
461 : : "pink",
462 : : "vehicle_continue_forward_maxLeft_" + std::to_string(ratioValue));
463 : : }
464 : : }
465 : :
466 : : // center-right, with maximum changing of the yaw rate
467 : 40 : std::vector<TrajectoryPoint> right;
468 : 20 : right.reserve(vehicleState.dynamics.unstructuredSettings.vehicleContinueForwardIntermediateYawRateChangeRatioSteps
469 [ + - ]: 20 : + 1);
470 [ + - + + : 100 : for (auto ratioValue = physics::RatioValue(-1.0); (ratioValue < physics::RatioValue(0.0)) && result;
+ - + + ]
471 [ + - ]: 80 : ratioValue += ratioDiffFront)
472 : : {
473 : 80 : TrajectoryPoint resultPt;
474 [ + - ]: 80 : result = calculateTrajectoryPoint(responseTimeFrontSide.right.front(),
475 : 80 : vehicleState.dynamics,
476 : : timeAfterResponseTime,
477 : 80 : vehicleState.dynamics.alphaLon.accelMax,
478 : : ratioValue,
479 : : resultPt);
480 [ + - ]: 80 : right.push_back(resultPt);
481 [ + - - + ]: 80 : if (DEBUG_DRAWING_IS_ENABLED())
482 : : {
483 [ # # # # : 0 : DEBUG_DRAWING_POLYGON(TrafficParticipantLocation(resultPt, vehicleState).toPolygon(),
# # # # #
# # # #
# ]
484 : : "pink",
485 : : std::string("vehicle_continue_forward_maxRight_" + std::to_string(ratioValue)));
486 : : }
487 : : }
488 : 40 : auto previousRight = std::move(front.right);
489 [ + - ]: 20 : front.right = right;
490 [ + - ]: 20 : front.right.insert(front.right.end(), previousRight.begin(), previousRight.end());
491 : :
492 : : //-----------
493 : : // Sides
494 : : //-----------
495 : 40 : std::vector<physics::Acceleration> accelerations;
496 : 0 : auto accelStepSize = (vehicleState.dynamics.alphaLon.accelMax - vehicleState.dynamics.alphaLon.brakeMin)
497 [ + - + - ]: 20 : / (1.0 + vehicleState.dynamics.unstructuredSettings.vehicleContinueForwardIntermediateAccelerationSteps);
498 [ + - ]: 20 : for (auto acceleration = vehicleState.dynamics.alphaLon.brakeMin + accelStepSize;
499 [ + - - + ]: 20 : acceleration < vehicleState.dynamics.alphaLon.accelMax;
500 [ # # ]: 0 : acceleration += accelStepSize)
501 : : {
502 [ # # ]: 0 : accelerations.push_back(acceleration);
503 : : }
504 [ + - ]: 20 : accelerations.push_back(vehicleState.dynamics.alphaLon.accelMax);
505 : 20 : std::vector<TrajectorySetStep> sideSteps;
506 [ + + + - : 40 : for (auto itAcceleration = accelerations.begin(); (itAcceleration != accelerations.end()) && result; ++itAcceleration)
+ + ]
507 : : {
508 : 20 : physics::Duration calculationTime = timeAfterResponseTime;
509 [ + - ]: 20 : result = getTimeInMovement(responseTimeFrontSide.center.speed, *itAcceleration, calculationTime);
510 : :
511 : 40 : TrajectorySetStep sideStep;
512 [ + - ]: 20 : if (result)
513 : : {
514 : : // left
515 : 20 : TrajectoryPoint resultPt;
516 [ + - ]: 20 : result = calculateTrajectoryPoint(responseTimeFrontSide.left.back(),
517 : 20 : vehicleState.dynamics,
518 : : calculationTime,
519 : 20 : *itAcceleration,
520 : 20 : physics::RatioValue(1.0),
521 : : resultPt);
522 [ + - ]: 20 : sideStep.left.push_back(resultPt);
523 [ + - - + ]: 20 : if (DEBUG_DRAWING_IS_ENABLED())
524 : : {
525 [ # # # # : 0 : DEBUG_DRAWING_POLYGON(TrafficParticipantLocation(resultPt, vehicleState).toPolygon(),
# # # # #
# # # #
# ]
526 : : "pink",
527 : : "vehicle_continue_forward_final_location_intermediate_left_"
528 : : + std::to_string(*itAcceleration));
529 : : }
530 : : }
531 : :
532 : : // center
533 [ + - ]: 20 : if (result)
534 : : {
535 : 20 : sideStep.center = responseTimeFrontSide.center;
536 : : result
537 [ + - ]: 20 : = calculateTrajectoryPointOnCircle(sideStep.center, *itAcceleration, calculationTime, vehicleState.dynamics);
538 : : }
539 : :
540 : : // right
541 [ + - ]: 20 : if (result)
542 : : {
543 : 20 : TrajectoryPoint resultPt;
544 [ + - ]: 20 : result = calculateTrajectoryPoint(responseTimeFrontSide.right.front(),
545 : 20 : vehicleState.dynamics,
546 : : calculationTime,
547 : 20 : *itAcceleration,
548 : 20 : physics::RatioValue(-1.0),
549 : : resultPt);
550 [ + - ]: 20 : sideStep.right.push_back(resultPt);
551 [ + - - + ]: 20 : if (DEBUG_DRAWING_IS_ENABLED())
552 : : {
553 [ # # # # : 0 : DEBUG_DRAWING_POLYGON(TrafficParticipantLocation(resultPt, vehicleState).toPolygon(),
# # # # #
# # # #
# ]
554 : : "pink",
555 : : "vehicle_continue_forward_final_location_intermediate_right_"
556 : : + std::to_string(*itAcceleration));
557 : : }
558 : : }
559 [ + - ]: 20 : sideSteps.push_back(sideStep);
560 : : }
561 : :
562 [ + - ]: 20 : resultPolygon = brakePolygon;
563 : 20 : TrajectorySetStepVehicleLocation unusedStepVehicleLocation;
564 [ + - + - ]: 20 : result = calculateFrontAndSidePolygon(vehicleState,
565 : : brakeMinStepVehicleLocation,
566 : : sideSteps,
567 : : front,
568 : : "vehicle_continue_forward",
569 : : resultPolygon,
570 : : unusedStepVehicleLocation);
571 [ - + ]: 20 : if (!result)
572 : : {
573 [ # # ]: 0 : spdlog::debug("TrajectoryVehicle::calculateContinueForward>> Could not calculate front and side polygon.");
574 : : }
575 : 40 : return result;
576 : : }
577 : :
578 : 100 : bool TrajectoryVehicle::calculateTrajectorySetStepOnCircle(situation::VehicleState const &vehicleState,
579 : : physics::Duration const &timeAfterResponseTime,
580 : : physics::Acceleration const &acceleration,
581 : : TrajectorySetStep &step) const
582 : : {
583 : 100 : auto result = true;
584 : :
585 : : // left
586 [ + + + - : 320 : for (auto it = step.left.begin(); (it != step.left.end()) && result; ++it)
+ + ]
587 : : {
588 [ + - ]: 220 : result = calculateTrajectoryPointOnCircle(*it, acceleration, timeAfterResponseTime, vehicleState.dynamics);
589 : : }
590 : :
591 : : // center
592 [ + - ]: 100 : if (result)
593 : : {
594 : 100 : result = calculateTrajectoryPointOnCircle(step.center, acceleration, timeAfterResponseTime, vehicleState.dynamics);
595 : : }
596 : :
597 : : // right
598 [ + + + - : 320 : for (auto it = step.right.begin(); (it != step.right.end()) && result; ++it)
+ + ]
599 : : {
600 [ + - ]: 220 : result = calculateTrajectoryPointOnCircle(*it, acceleration, timeAfterResponseTime, vehicleState.dynamics);
601 : : }
602 : :
603 : 100 : return result;
604 : : }
605 : :
606 : : } // namespace unstructured
607 : : } // namespace rss
608 : : } // namespace ad
|
