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