Branch data Line data Source code
1 : : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
2 : : //
3 : : // Copyright (C) 2018-2021 Intel Corporation
4 : : //
5 : : // SPDX-License-Identifier: LGPL-2.1-only
6 : : //
7 : : // ----------------- END LICENSE BLOCK -----------------------------------
8 : : #include "TestSupport.hpp"
9 : : #include <ad/geometry/HeadingRange.hpp>
10 : :
11 : : namespace ad {
12 : : namespace rss {
13 : :
14 : : const TestSupport cTestSupport;
15 : :
16 : 63 : void resetRssState(state::LongitudinalRssState &state)
17 : : {
18 : 63 : state.response = state::LongitudinalResponse::None;
19 : 63 : state.alpha_lon = getEgoRssDynamics().alpha_lon;
20 : 63 : state.is_safe = true;
21 : 63 : state.rss_state_information.current_distance = physics::Distance::getMax();
22 : 63 : state.rss_state_information.safe_distance = physics::Distance::getMax();
23 : 63 : state.rss_state_information.evaluator = state::RssStateEvaluator::None;
24 : 63 : }
25 : :
26 : 125 : void resetRssState(state::LateralRssState &state)
27 : : {
28 : 125 : state.response = state::LateralResponse::None;
29 : 125 : state.alpha_lat = getEgoRssDynamics().alpha_lat;
30 : 125 : state.is_safe = true;
31 : 125 : state.rss_state_information.current_distance = physics::Distance::getMax();
32 : 125 : state.rss_state_information.safe_distance = physics::Distance::getMax();
33 : 125 : state.rss_state_information.evaluator = state::RssStateEvaluator::None;
34 : 125 : }
35 : :
36 : 55 : void resetRssState(state::UnstructuredConstellationRssState &state)
37 : : {
38 : 55 : state.response = state::UnstructuredConstellationResponse::None;
39 : 55 : state.alpha_lon = getEgoRssDynamics().alpha_lon;
40 : 55 : state.heading_range.begin = ad::physics::Angle(0.0);
41 : 55 : state.heading_range.end = ad::physics::c2PI;
42 : 55 : state.is_safe = true;
43 : 55 : }
44 : :
45 : 55 : void resetRssState(state::RssState &rssState,
46 : : core::RelativeConstellationId const constellation_id,
47 : : world::ObjectId const object_id,
48 : : world::ConstellationType const constellation_type)
49 : : {
50 : 55 : rssState.constellation_id = constellation_id;
51 : 55 : rssState.object_id = object_id;
52 : 55 : rssState.constellation_type = constellation_type;
53 : 55 : resetRssState(rssState.longitudinal_state);
54 : 55 : resetRssState(rssState.lateral_state_left);
55 : 55 : resetRssState(rssState.lateral_state_right);
56 : 55 : resetRssState(rssState.unstructured_constellation_state);
57 : 55 : }
58 : :
59 : 27 : void resetRssState(state::ProperResponse &properResponse)
60 : : {
61 : 27 : properResponse.is_safe = true;
62 : 27 : properResponse.time_index = 1u;
63 : 27 : properResponse.dangerous_objects.clear();
64 : 27 : properResponse.longitudinal_response = state::LongitudinalResponse::None;
65 : 27 : properResponse.lateral_response_left = state::LateralResponse::None;
66 : 27 : properResponse.lateral_response_right = state::LateralResponse::None;
67 : :
68 : 27 : properResponse.heading_ranges.clear();
69 : 27 : ::ad::geometry::HeadingRange initialHeadingRange;
70 : 27 : initialHeadingRange.begin = ad::physics::Angle(0.0);
71 : 27 : initialHeadingRange.end = ad::physics::c2PI;
72 [ + - ]: 27 : properResponse.heading_ranges.push_back(initialHeadingRange);
73 : 27 : }
74 : :
75 : 1063 : world::RssDynamics getObjectRssDynamics()
76 : : {
77 : 1063 : world::RssDynamics rssDynamics;
78 : :
79 : 1063 : rssDynamics.alpha_lon.accel_max = cMaximumLongitudinalAcceleration;
80 : 1063 : rssDynamics.alpha_lon.brake_max = cMaximumLongitudinalBrakingDeceleleration;
81 : 1063 : rssDynamics.alpha_lon.brake_min = cMinimumLongitudinalBrakingDeceleleration;
82 : 1063 : rssDynamics.alpha_lon.brake_min_correct = cMinimumLongitudinalBrakingDecelelerationCorrect;
83 : :
84 : 1063 : rssDynamics.alpha_lat.accel_max = cMaximumLateralAcceleration;
85 : 1063 : rssDynamics.alpha_lat.brake_min = cMinimumLateralBrakingDeceleleration;
86 : :
87 : 1063 : rssDynamics.lateral_fluctuation_margin = ad::physics::Distance(0.1);
88 : 1063 : rssDynamics.response_time = cResponseTimeOtherVehicles;
89 : :
90 : 1063 : rssDynamics.min_longitudinal_safety_distance = ad::physics::Distance(0.);
91 : :
92 : 1063 : rssDynamics.unstructured_settings.pedestrian_turning_radius = ad::physics::Distance(2.0);
93 : 1063 : rssDynamics.unstructured_settings.drive_away_max_angle = ad::physics::Angle(2.4);
94 : 1063 : rssDynamics.unstructured_settings.vehicle_yaw_rate_change = ad::physics::AngularAcceleration(0.3);
95 : 1063 : rssDynamics.unstructured_settings.vehicle_min_radius = ad::physics::Distance(3.5);
96 : 1063 : rssDynamics.unstructured_settings.vehicle_trajectory_calculation_step = ad::physics::Duration(0.2);
97 : :
98 : 1063 : rssDynamics.unstructured_settings.vehicle_front_intermediate_yaw_rate_change_ratio_steps = 3;
99 : 1063 : rssDynamics.unstructured_settings.vehicle_back_intermediate_yaw_rate_change_ratio_steps = 0;
100 : 1063 : rssDynamics.unstructured_settings.vehicle_brake_intermediate_acceleration_steps = 1;
101 : 1063 : rssDynamics.unstructured_settings.vehicle_continue_forward_intermediate_acceleration_steps = 0;
102 : 1063 : rssDynamics.unstructured_settings.vehicle_continue_forward_intermediate_yaw_rate_change_ratio_steps = 3;
103 : 1063 : rssDynamics.unstructured_settings.pedestrian_continue_forward_intermediate_heading_change_ratio_steps = 3;
104 : 1063 : rssDynamics.unstructured_settings.pedestrian_continue_forward_intermediate_acceleration_steps = 0;
105 : 1063 : rssDynamics.unstructured_settings.pedestrian_brake_intermediate_acceleration_steps = 3;
106 : 1063 : rssDynamics.unstructured_settings.pedestrian_front_intermediate_heading_change_ratio_steps = 4;
107 : 1063 : rssDynamics.unstructured_settings.pedestrian_back_intermediate_heading_change_ratio_steps = 0;
108 : :
109 : 1063 : return rssDynamics;
110 : : }
111 : :
112 : 1959 : world::RssDynamics getEgoRssDynamics()
113 : : {
114 : 1959 : world::RssDynamics rssDynamics;
115 : :
116 : 1959 : rssDynamics.alpha_lon.accel_max = cMaximumLongitudinalAcceleration;
117 : 1959 : rssDynamics.alpha_lon.brake_max = cMaximumLongitudinalBrakingDeceleleration;
118 : 1959 : rssDynamics.alpha_lon.brake_min = cMinimumLongitudinalBrakingDeceleleration;
119 : 1959 : rssDynamics.alpha_lon.brake_min_correct = cMinimumLongitudinalBrakingDecelelerationCorrect;
120 : :
121 : 1959 : rssDynamics.alpha_lat.accel_max = cMaximumLateralAcceleration;
122 : 1959 : rssDynamics.alpha_lat.brake_min = cMinimumLateralBrakingDeceleleration;
123 : :
124 : 1959 : rssDynamics.lateral_fluctuation_margin = ad::physics::Distance(0.1);
125 : 1959 : rssDynamics.response_time = cResponseTimeEgoVehicle;
126 : :
127 : 1959 : rssDynamics.min_longitudinal_safety_distance = ad::physics::Distance(0.);
128 : :
129 : 1959 : rssDynamics.unstructured_settings.pedestrian_turning_radius = ad::physics::Distance(2.0);
130 : 1959 : rssDynamics.unstructured_settings.drive_away_max_angle = ad::physics::Angle(2.4);
131 : 1959 : rssDynamics.unstructured_settings.vehicle_yaw_rate_change = ad::physics::AngularAcceleration(0.3);
132 : 1959 : rssDynamics.unstructured_settings.vehicle_min_radius = ad::physics::Distance(3.5);
133 : 1959 : rssDynamics.unstructured_settings.vehicle_trajectory_calculation_step = ad::physics::Duration(0.2);
134 : :
135 : 1959 : rssDynamics.unstructured_settings.vehicle_front_intermediate_yaw_rate_change_ratio_steps = 3;
136 : 1959 : rssDynamics.unstructured_settings.vehicle_back_intermediate_yaw_rate_change_ratio_steps = 0;
137 : 1959 : rssDynamics.unstructured_settings.vehicle_brake_intermediate_acceleration_steps = 1;
138 : 1959 : rssDynamics.unstructured_settings.vehicle_continue_forward_intermediate_acceleration_steps = 0;
139 : 1959 : rssDynamics.unstructured_settings.vehicle_continue_forward_intermediate_yaw_rate_change_ratio_steps = 3;
140 : 1959 : rssDynamics.unstructured_settings.pedestrian_continue_forward_intermediate_heading_change_ratio_steps = 3;
141 : 1959 : rssDynamics.unstructured_settings.pedestrian_continue_forward_intermediate_acceleration_steps = 0;
142 : 1959 : rssDynamics.unstructured_settings.pedestrian_brake_intermediate_acceleration_steps = 3;
143 : 1959 : rssDynamics.unstructured_settings.pedestrian_front_intermediate_heading_change_ratio_steps = 4;
144 : 1959 : rssDynamics.unstructured_settings.pedestrian_back_intermediate_heading_change_ratio_steps = 0;
145 : 1959 : return rssDynamics;
146 : : }
147 : :
148 : 5862 : world::Object createObject(double const lonVelocity, double const latVelocity)
149 : : {
150 : 5862 : world::Object object;
151 : :
152 : 5862 : object.object_type = world::ObjectType::OtherVehicle;
153 : 5862 : object.velocity.speed_lon_min = kmhToMeterPerSec(lonVelocity);
154 : 5862 : object.velocity.speed_lon_max = kmhToMeterPerSec(lonVelocity);
155 : 5862 : object.velocity.speed_lat_min = kmhToMeterPerSec(latVelocity);
156 : 5862 : object.velocity.speed_lat_max = kmhToMeterPerSec(latVelocity);
157 [ + - ]: 5862 : object.state = createObjectState(lonVelocity, latVelocity);
158 : 5862 : return object;
159 : 0 : }
160 : :
161 : 6079 : world::ObjectState createObjectState(double const lonVelocity, double const latVelocity)
162 : : {
163 : 6079 : world::ObjectState state;
164 : 6079 : state.yaw = ad::physics::Angle(0.0);
165 : 6079 : state.steering_angle = ad::physics::Angle(0.0);
166 : 6079 : state.dimension.length = ad::physics::Distance(4.0);
167 : 6079 : state.dimension.width = ad::physics::Distance(2.0);
168 : 6079 : state.yaw_rate = ad::physics::AngularVelocity(0.0);
169 : 6079 : state.center_point.x = ad::physics::Distance(0.0);
170 : 6079 : state.center_point.y = ad::physics::Distance(0.0);
171 [ + - + - : 6079 : auto const speed = ad::physics::Speed(std::sqrt(kmhToMeterPerSec(lonVelocity) * kmhToMeterPerSec(lonVelocity)
+ - ]
172 [ + - ]: 6079 : + kmhToMeterPerSec(latVelocity) * kmhToMeterPerSec(latVelocity)));
173 : 6079 : state.speed_range.minimum = speed;
174 : 6079 : state.speed_range.maximum = speed;
175 : 12158 : return state;
176 : : }
177 : :
178 : : core::RelativeObjectState
179 : 217 : createRelativeVehicleState(world::ObjectType const object_type, double const lonVelocity, double const latVelocity)
180 : : {
181 : 217 : core::RelativeObjectState state;
182 : :
183 : 217 : state.object_type = object_type;
184 : 217 : state.dynamics = getObjectRssDynamics();
185 : 217 : state.structured_object_state.velocity.speed_lon_min = kmhToMeterPerSec(lonVelocity);
186 : 217 : state.structured_object_state.velocity.speed_lon_max = state.structured_object_state.velocity.speed_lon_min;
187 : 217 : state.structured_object_state.velocity.speed_lat_min = kmhToMeterPerSec(latVelocity);
188 : 217 : state.structured_object_state.velocity.speed_lat_max = state.structured_object_state.velocity.speed_lat_min;
189 : 217 : state.structured_object_state.distance_to_enter_intersection = Distance(0.);
190 : 217 : state.structured_object_state.distance_to_leave_intersection = Distance(1000.);
191 : 217 : state.structured_object_state.has_priority = false;
192 : 217 : state.structured_object_state.is_in_correct_lane = true;
193 : 217 : state.unstructured_object_state = createObjectState(lonVelocity, latVelocity);
194 : 217 : return state;
195 : : }
196 : :
197 : 46 : core::RelativePosition createRelativeLongitudinalPosition(core::LongitudinalRelativePosition const &position,
198 : : Distance const &distance)
199 : : {
200 : 46 : core::RelativePosition relative_position;
201 : 46 : relative_position.lateral_distance = Distance(0.);
202 : 46 : relative_position.lateral_position = core::LateralRelativePosition::Overlap;
203 : 46 : relative_position.longitudinal_distance = distance;
204 : 46 : relative_position.longitudinal_position = position;
205 : 46 : return relative_position;
206 : : }
207 : :
208 : 29 : core::RelativePosition createRelativeLateralPosition(core::LateralRelativePosition const &position,
209 : : Distance const &distance)
210 : : {
211 : 29 : core::RelativePosition relative_position;
212 : 29 : relative_position.lateral_distance = distance;
213 : 29 : relative_position.lateral_position = position;
214 : 29 : relative_position.longitudinal_distance = Distance(0.);
215 : 29 : relative_position.longitudinal_position = core::LongitudinalRelativePosition::Overlap;
216 : 29 : return relative_position;
217 : : }
218 : :
219 : 3126 : Distance calculateLongitudinalStoppingDistance(Speed const &objectSpeed,
220 : : Speed const &objectMaxSpeedOnAcceleration,
221 : : Acceleration const &acceleration,
222 : : Acceleration const &deceleration,
223 : : Duration const &response_time)
224 : : {
225 : 3126 : Duration accelerationTime = response_time;
226 [ + - + - : 3126 : Speed speedMax = std::min(objectMaxSpeedOnAcceleration, objectSpeed + response_time * acceleration);
+ - ]
227 [ + - + + ]: 3126 : if (objectSpeed >= objectMaxSpeedOnAcceleration)
228 : : {
229 : : // no acceleration if already faster than max_speed_on_acceleration
230 : 5 : accelerationTime = Duration(0.);
231 : 5 : speedMax = objectSpeed;
232 : : }
233 [ + - + + ]: 3121 : else if (acceleration != Acceleration(0.))
234 : : {
235 : : // maybe the acceleration time is less, if max_speed_on_acceleration is reached before response time
236 [ + - + - : 3120 : accelerationTime = std::min(accelerationTime, std::fabs(objectMaxSpeedOnAcceleration - objectSpeed) / acceleration);
+ - ]
237 : : }
238 [ + - ]: 3126 : Distance dMin = objectSpeed * accelerationTime;
239 [ + - + - : 3126 : dMin += 0.5 * acceleration * accelerationTime * accelerationTime;
+ - + - ]
240 [ + - + - : 3126 : dMin += speedMax * (response_time - accelerationTime);
+ - ]
241 [ + - + - : 3126 : dMin += (speedMax * speedMax) / (2. * -deceleration);
+ - + - +
- ]
242 : 3126 : return dMin;
243 : : }
244 : :
245 : 663 : Distance calculateLongitudinalMinSafeDistance(physics::Speed const &followingObjectSpeed,
246 : : world::RssDynamics const &followingObjectRssDynamics,
247 : : physics::Speed const &leadingObjectSpeed,
248 : : world::RssDynamics const &leadingObjectRssDynamics)
249 : : {
250 : : Distance const followingStoppingDistance
251 : 663 : = calculateLongitudinalStoppingDistance(followingObjectSpeed,
252 : 663 : followingObjectRssDynamics.max_speed_on_acceleration,
253 : 663 : followingObjectRssDynamics.alpha_lon.accel_max,
254 : 663 : followingObjectRssDynamics.alpha_lon.brake_min,
255 [ + - ]: 663 : followingObjectRssDynamics.response_time);
256 : : Distance const leadingStoppingDistance
257 : 663 : = calculateLongitudinalStoppingDistance(leadingObjectSpeed,
258 : 663 : leadingObjectRssDynamics.max_speed_on_acceleration,
259 : 663 : leadingObjectRssDynamics.alpha_lon.accel_max,
260 [ + - ]: 663 : leadingObjectRssDynamics.alpha_lon.brake_max,
261 : 663 : Duration(0));
262 [ + - ]: 663 : Distance dMin = followingStoppingDistance - leadingStoppingDistance;
263 [ + - ]: 663 : dMin = std::max(dMin, Distance(0.));
264 [ + - ]: 663 : dMin += Distance(followingObjectRssDynamics.min_longitudinal_safety_distance);
265 : 663 : return dMin;
266 : : }
267 : :
268 : : Distance
269 : 885 : calculateLongitudinalMinSafeDistanceOppositeDirection(physics::Speed const &objectInCorrectLaneSpeed,
270 : : world::RssDynamics const &objectInCorrectLaneRssDynamics,
271 : : physics::Speed const &objectNotInCorrectLaneSpeed,
272 : : world::RssDynamics const &objectNotInCorrectLaneRssDynamics)
273 : : {
274 : : Distance const correctStoppingDistance
275 : 885 : = calculateLongitudinalStoppingDistance(objectInCorrectLaneSpeed,
276 : 885 : objectInCorrectLaneRssDynamics.max_speed_on_acceleration,
277 : 885 : objectInCorrectLaneRssDynamics.alpha_lon.accel_max,
278 : 885 : objectInCorrectLaneRssDynamics.alpha_lon.brake_min_correct,
279 [ + - ]: 885 : objectInCorrectLaneRssDynamics.response_time);
280 : : Distance const notCorrectStoppingDistance
281 : 885 : = calculateLongitudinalStoppingDistance(objectNotInCorrectLaneSpeed,
282 : 885 : objectNotInCorrectLaneRssDynamics.max_speed_on_acceleration,
283 : 885 : objectNotInCorrectLaneRssDynamics.alpha_lon.accel_max,
284 : 885 : objectNotInCorrectLaneRssDynamics.alpha_lon.brake_min,
285 [ + - ]: 885 : objectNotInCorrectLaneRssDynamics.response_time);
286 [ + - ]: 885 : Distance dMin = correctStoppingDistance + notCorrectStoppingDistance;
287 : 885 : dMin += std::max(objectInCorrectLaneRssDynamics.min_longitudinal_safety_distance,
288 [ + - + - ]: 885 : objectNotInCorrectLaneRssDynamics.min_longitudinal_safety_distance);
289 : 885 : return dMin;
290 : : }
291 : :
292 : 745 : Distance calculateLateralMinSafeDistance(physics::Speed const &leftObjectSpeed,
293 : : world::RssDynamics const &leftObjectRssDynamics,
294 : : physics::Speed const &rightObjectSpeed,
295 : : world::RssDynamics const &rightObjectRssDynamics)
296 : : {
297 : : Speed lObjectVelAfterResTime
298 [ + - + - ]: 745 : = leftObjectSpeed + leftObjectRssDynamics.response_time * leftObjectRssDynamics.alpha_lat.accel_max;
299 : : Speed rObjectVelAfterResTime
300 [ + - + - ]: 745 : = rightObjectSpeed - rightObjectRssDynamics.response_time * rightObjectRssDynamics.alpha_lat.accel_max;
301 [ + - + - : 745 : Distance dMin = (leftObjectSpeed + lObjectVelAfterResTime) / 2. * leftObjectRssDynamics.response_time;
+ - ]
302 [ + - + + ]: 745 : if (lObjectVelAfterResTime > Speed(0.))
303 : : {
304 [ + - + - : 655 : dMin += lObjectVelAfterResTime * lObjectVelAfterResTime / (2. * -leftObjectRssDynamics.alpha_lat.brake_min);
+ - + - +
- ]
305 : : }
306 [ + - + - : 745 : dMin -= (rightObjectSpeed + rObjectVelAfterResTime) / 2. * rightObjectRssDynamics.response_time;
+ - + - ]
307 [ + - + + ]: 745 : if (rObjectVelAfterResTime < Speed(0.))
308 : : {
309 [ + - + - : 655 : dMin += rObjectVelAfterResTime * rObjectVelAfterResTime / (2. * -rightObjectRssDynamics.alpha_lat.brake_min);
+ - + - +
- ]
310 : : }
311 [ + - + - : 745 : dMin += 0.5 * (rightObjectRssDynamics.lateral_fluctuation_margin + leftObjectRssDynamics.lateral_fluctuation_margin);
+ - ]
312 [ + - ]: 745 : return std::max(dMin, Distance(0.));
313 : : }
314 : 1 : TestSupport::TestSupport()
315 : : {
316 : 1 : resetRssState(cLongitudinalSafe);
317 : 1 : cLongitudinalSafe.is_safe = true;
318 : 1 : cLongitudinalSafe.response = state::LongitudinalResponse::None;
319 : 1 : resetRssState(cLongitudinalNone);
320 : 1 : cLongitudinalNone.is_safe = false;
321 : 1 : cLongitudinalNone.response = state::LongitudinalResponse::None;
322 : 1 : resetRssState(cLongitudinalBrakeMin);
323 : 1 : cLongitudinalBrakeMin.is_safe = false;
324 : 1 : cLongitudinalBrakeMin.response = state::LongitudinalResponse::BrakeMin;
325 : 1 : resetRssState(cLongitudinalBrakeMinCorrect);
326 : 1 : cLongitudinalBrakeMinCorrect.is_safe = false;
327 : 1 : cLongitudinalBrakeMinCorrect.response = state::LongitudinalResponse::BrakeMinCorrect;
328 : 1 : resetRssState(cLateralSafe);
329 : 1 : cLateralSafe.is_safe = true;
330 : 1 : cLateralSafe.response = state::LateralResponse::None;
331 : 1 : resetRssState(cLateralNone);
332 : 1 : cLateralNone.is_safe = false;
333 : 1 : cLateralNone.response = state::LateralResponse::None;
334 : 1 : resetRssState(cLateralBrakeMin);
335 : 1 : cLateralBrakeMin.is_safe = false;
336 : 1 : cLateralBrakeMin.response = state::LateralResponse::BrakeMin;
337 : 1 : }
338 : :
339 : 131 : state::LateralRssState TestSupport::stateWithInformation(state::LateralRssState const &lateralState,
340 : : core::RelativeConstellation const &constellation)
341 : : {
342 : 131 : state::LateralRssState resultState = lateralState;
343 : :
344 : 131 : resultState.alpha_lat = constellation.ego_state.dynamics.alpha_lat;
345 : 131 : resultState.rss_state_information.evaluator = state::RssStateEvaluator::LateralDistance;
346 : 131 : resultState.rss_state_information.current_distance = constellation.relative_position.lateral_distance;
347 [ + + + - ]: 131 : switch (constellation.constellation_type)
348 : : {
349 : 85 : case world::ConstellationType::OppositeDirection:
350 : : case world::ConstellationType::SameDirection:
351 [ + + ]: 85 : if (constellation.relative_position.lateral_position == core::LateralRelativePosition::AtLeft)
352 : : {
353 : : resultState.rss_state_information.safe_distance
354 : 13 : = calculateLateralMinSafeDistance(constellation.ego_state.structured_object_state.velocity.speed_lat_max,
355 : 13 : constellation.ego_state.dynamics,
356 : 13 : constellation.other_state.structured_object_state.velocity.speed_lat_min,
357 : 13 : constellation.other_state.dynamics);
358 : : }
359 [ + + ]: 72 : else if (constellation.relative_position.lateral_position == core::LateralRelativePosition::AtRight)
360 : : {
361 : : resultState.rss_state_information.safe_distance
362 : 10 : = calculateLateralMinSafeDistance(constellation.other_state.structured_object_state.velocity.speed_lat_max,
363 : 10 : constellation.other_state.dynamics,
364 : 10 : constellation.ego_state.structured_object_state.velocity.speed_lat_min,
365 : 10 : constellation.ego_state.dynamics);
366 : : }
367 : : else
368 : : {
369 : 62 : resultState.rss_state_information.safe_distance = Distance(0.);
370 : : }
371 : 85 : break;
372 : 44 : case world::ConstellationType::IntersectionEgoHasPriority:
373 : : case world::ConstellationType::IntersectionObjectHasPriority:
374 : : case world::ConstellationType::IntersectionSamePriority:
375 : 44 : resultState.rss_state_information.safe_distance = Distance(0.);
376 : 44 : break;
377 : 2 : case world::ConstellationType::NotRelevant:
378 : 2 : resultState.rss_state_information.evaluator = state::RssStateEvaluator::None;
379 : 2 : resultState.rss_state_information.current_distance = Distance::getMax();
380 : 2 : resultState.rss_state_information.safe_distance = Distance::getMax();
381 : 2 : break;
382 : 0 : default:
383 : 0 : resultState.rss_state_information.current_distance = Distance(-1.);
384 : 0 : resultState.rss_state_information.safe_distance = Distance(-1.);
385 : 0 : break;
386 : : }
387 : :
388 : 131 : return resultState;
389 : : }
390 : :
391 : 77 : state::LongitudinalRssState TestSupport::stateWithInformation(state::LongitudinalRssState const &longitudinal_state,
392 : : core::RelativeConstellation const &constellation)
393 : : {
394 : 77 : state::LongitudinalRssState resultState = longitudinal_state;
395 : :
396 : 77 : resultState.alpha_lon = constellation.ego_state.dynamics.alpha_lon;
397 : 77 : resultState.rss_state_information.current_distance = constellation.relative_position.longitudinal_distance;
398 : :
399 [ + + + + : 77 : switch (constellation.constellation_type)
- ]
400 : : {
401 : 42 : case world::ConstellationType::SameDirection:
402 [ + + ]: 42 : if ((constellation.relative_position.longitudinal_position == core::LongitudinalRelativePosition::InFront)
403 [ - + ]: 41 : || (constellation.relative_position.longitudinal_position
404 : : == core::LongitudinalRelativePosition::OverlapFront))
405 : : {
406 : : resultState.rss_state_information.evaluator
407 : 1 : = state::RssStateEvaluator::LongitudinalDistanceSameDirectionEgoFront;
408 : 1 : resultState.rss_state_information.safe_distance = calculateLongitudinalMinSafeDistance(
409 : 1 : constellation.other_state.structured_object_state.velocity.speed_lon_max,
410 : 1 : constellation.other_state.dynamics,
411 : 1 : constellation.ego_state.structured_object_state.velocity.speed_lon_min,
412 : 1 : constellation.ego_state.dynamics);
413 : : }
414 : : else
415 : : {
416 : : resultState.rss_state_information.evaluator
417 : 41 : = state::RssStateEvaluator::LongitudinalDistanceSameDirectionOtherInFront;
418 : 41 : resultState.rss_state_information.safe_distance = calculateLongitudinalMinSafeDistance(
419 : 41 : constellation.ego_state.structured_object_state.velocity.speed_lon_max,
420 : 41 : constellation.ego_state.dynamics,
421 : 41 : constellation.other_state.structured_object_state.velocity.speed_lon_min,
422 : 41 : constellation.other_state.dynamics);
423 : : }
424 : 42 : break;
425 : 12 : case world::ConstellationType::OppositeDirection:
426 [ + + ]: 12 : if (constellation.ego_state.structured_object_state.is_in_correct_lane)
427 : : {
428 : : resultState.rss_state_information.evaluator
429 : 11 : = state::RssStateEvaluator::LongitudinalDistanceOppositeDirectionEgoCorrectLane;
430 : 11 : resultState.rss_state_information.safe_distance = calculateLongitudinalMinSafeDistanceOppositeDirection(
431 : 11 : constellation.ego_state.structured_object_state.velocity.speed_lon_max,
432 : 11 : constellation.ego_state.dynamics,
433 : 11 : constellation.other_state.structured_object_state.velocity.speed_lon_max,
434 : 11 : constellation.other_state.dynamics);
435 : : }
436 : : else
437 : : {
438 : 1 : resultState.rss_state_information.evaluator = state::RssStateEvaluator::LongitudinalDistanceOppositeDirection;
439 : 1 : resultState.rss_state_information.safe_distance = calculateLongitudinalMinSafeDistanceOppositeDirection(
440 : 1 : constellation.other_state.structured_object_state.velocity.speed_lon_max,
441 : 1 : constellation.other_state.dynamics,
442 : 1 : constellation.ego_state.structured_object_state.velocity.speed_lon_max,
443 : 1 : constellation.ego_state.dynamics);
444 : : }
445 : 12 : break;
446 : 22 : case world::ConstellationType::IntersectionEgoHasPriority:
447 : : case world::ConstellationType::IntersectionObjectHasPriority:
448 : : case world::ConstellationType::IntersectionSamePriority:
449 [ + + ]: 22 : if (!constellation.ego_state.structured_object_state.has_priority)
450 : : {
451 : : resultState.rss_state_information.current_distance
452 : 18 : = constellation.ego_state.structured_object_state.distance_to_enter_intersection;
453 : 18 : resultState.rss_state_information.safe_distance = calculateLongitudinalStoppingDistance(
454 : 18 : constellation.ego_state.structured_object_state.velocity.speed_lon_max,
455 : 18 : constellation.ego_state.dynamics.max_speed_on_acceleration,
456 : 18 : constellation.ego_state.dynamics.alpha_lon.accel_max,
457 : 18 : constellation.ego_state.dynamics.alpha_lon.brake_min,
458 : 18 : constellation.ego_state.dynamics.response_time);
459 : : resultState.rss_state_information.safe_distance
460 : 36 : = std::max(resultState.rss_state_information.safe_distance,
461 : 18 : constellation.ego_state.dynamics.min_longitudinal_safety_distance);
462 : 18 : resultState.rss_state_information.evaluator = state::RssStateEvaluator::IntersectionOtherPriorityEgoAbleToStop;
463 [ + + ]: 18 : if (resultState.rss_state_information.current_distance > resultState.rss_state_information.safe_distance)
464 : : {
465 : 2 : break;
466 : : }
467 : : }
468 [ + + ]: 20 : if (!constellation.other_state.structured_object_state.has_priority)
469 : : {
470 : : resultState.rss_state_information.current_distance
471 : 12 : = constellation.other_state.structured_object_state.distance_to_enter_intersection;
472 : 12 : resultState.rss_state_information.safe_distance = calculateLongitudinalStoppingDistance(
473 : 12 : constellation.other_state.structured_object_state.velocity.speed_lon_max,
474 : 12 : constellation.other_state.dynamics.max_speed_on_acceleration,
475 : 12 : constellation.other_state.dynamics.alpha_lon.accel_max,
476 : 12 : constellation.other_state.dynamics.alpha_lon.brake_min,
477 : 12 : constellation.other_state.dynamics.response_time);
478 : : resultState.rss_state_information.safe_distance
479 : 24 : = std::max(resultState.rss_state_information.safe_distance,
480 : 12 : constellation.other_state.dynamics.min_longitudinal_safety_distance);
481 : 12 : resultState.rss_state_information.evaluator = state::RssStateEvaluator::IntersectionEgoPriorityOtherAbleToStop;
482 [ + + ]: 12 : if (resultState.rss_state_information.current_distance > resultState.rss_state_information.safe_distance)
483 : : {
484 : 5 : break;
485 : : }
486 : : }
487 : 15 : resultState.rss_state_information.current_distance = constellation.relative_position.longitudinal_distance;
488 [ + + ]: 15 : if (constellation.relative_position.longitudinal_position == core::LongitudinalRelativePosition::InFront)
489 : : {
490 : 9 : resultState.rss_state_information.evaluator = state::RssStateEvaluator::IntersectionEgoInFront;
491 : 9 : resultState.rss_state_information.safe_distance = calculateLongitudinalMinSafeDistance(
492 : 9 : constellation.other_state.structured_object_state.velocity.speed_lon_max,
493 : 9 : constellation.other_state.dynamics,
494 : 9 : constellation.ego_state.structured_object_state.velocity.speed_lon_min,
495 : 9 : constellation.ego_state.dynamics);
496 : : }
497 [ + + ]: 6 : else if (constellation.relative_position.longitudinal_position == core::LongitudinalRelativePosition::AtBack)
498 : : {
499 : 5 : resultState.rss_state_information.evaluator = state::RssStateEvaluator::IntersectionOtherInFront;
500 : 5 : resultState.rss_state_information.safe_distance = calculateLongitudinalMinSafeDistance(
501 : 5 : constellation.ego_state.structured_object_state.velocity.speed_lon_max,
502 : 5 : constellation.ego_state.dynamics,
503 : 5 : constellation.other_state.structured_object_state.velocity.speed_lon_min,
504 : 5 : constellation.other_state.dynamics);
505 : : }
506 [ + + ]: 15 : if (resultState.rss_state_information.current_distance > resultState.rss_state_information.safe_distance)
507 : : {
508 : 6 : break;
509 : : }
510 : : else
511 : : {
512 : 9 : resultState.rss_state_information.evaluator = state::RssStateEvaluator::IntersectionOverlap;
513 : 9 : resultState.rss_state_information.current_distance = Distance(0.);
514 : 9 : resultState.rss_state_information.safe_distance = Distance(0.);
515 : : }
516 : 9 : break;
517 : 1 : case world::ConstellationType::NotRelevant:
518 : 1 : resultState.rss_state_information.evaluator = state::RssStateEvaluator::None;
519 : 1 : resultState.rss_state_information.current_distance = Distance::getMax();
520 : 1 : resultState.rss_state_information.safe_distance = Distance::getMax();
521 : 1 : break;
522 : 0 : default:
523 : 0 : resultState.rss_state_information.evaluator = state::RssStateEvaluator::None;
524 : 0 : resultState.rss_state_information.current_distance = Distance(-1.);
525 : 0 : resultState.rss_state_information.safe_distance = Distance(-1.);
526 : 0 : break;
527 : : }
528 : :
529 : 77 : return resultState;
530 : : }
531 : :
532 : 8 : void getUnstructuredVehicle(::ad::geometry::Point const ¢er_point,
533 : : bool positiveDirection,
534 : : state::UnstructuredConstellationStateInformation &stateInfo,
535 : : core::RelativeObjectState &vehicleState)
536 : : {
537 : 8 : vehicleState.unstructured_object_state.center_point.x = ad::physics::Distance(center_point.x());
538 : 8 : vehicleState.unstructured_object_state.center_point.y = ad::physics::Distance(center_point.y());
539 [ + + ]: 8 : if (positiveDirection)
540 : : {
541 : 6 : vehicleState.unstructured_object_state.yaw = ad::physics::cPI_2;
542 : : }
543 : : else
544 : : {
545 [ + - ]: 2 : vehicleState.unstructured_object_state.yaw = 3. * ad::physics::cPI_2;
546 : : }
547 : :
548 : 8 : stateInfo.brake_trajectory_set.clear();
549 : 8 : stateInfo.continue_forward_trajectory_set.clear();
550 : : // brake
551 [ + + ]: 8 : if (positiveDirection)
552 : : {
553 [ + - ]: 6 : stateInfo.brake_trajectory_set.push_back(
554 [ + - + - : 6 : ::ad::geometry::toDistance(::ad::geometry::Point(center_point.x() - 0.5, center_point.y() - 0.5)));
+ - ]
555 [ + - ]: 6 : stateInfo.continue_forward_trajectory_set.push_back(
556 [ + - + - : 6 : ::ad::geometry::toDistance(::ad::geometry::Point(center_point.x() - 0.5, center_point.y() - 0.5)));
+ - ]
557 : :
558 [ + - ]: 6 : stateInfo.brake_trajectory_set.push_back(
559 [ + - + - : 6 : ::ad::geometry::toDistance(::ad::geometry::Point(center_point.x() + 0.5, center_point.y() - 0.5)));
+ - ]
560 [ + - ]: 6 : stateInfo.continue_forward_trajectory_set.push_back(
561 [ + - + - : 6 : ::ad::geometry::toDistance(::ad::geometry::Point(center_point.x() + 0.5, center_point.y() - 0.5)));
+ - ]
562 : :
563 [ + - ]: 6 : stateInfo.brake_trajectory_set.push_back(
564 [ + - + - : 6 : ::ad::geometry::toDistance(::ad::geometry::Point(center_point.x() + 0.5, center_point.y() + 1.5)));
+ - ]
565 [ + - ]: 6 : stateInfo.continue_forward_trajectory_set.push_back(
566 [ + - + - : 6 : ::ad::geometry::toDistance(::ad::geometry::Point(center_point.x() + 0.5, center_point.y() + 2.5)));
+ - ]
567 : :
568 [ + - ]: 6 : stateInfo.brake_trajectory_set.push_back(
569 [ + - + - : 6 : ::ad::geometry::toDistance(::ad::geometry::Point(center_point.x() - 0.5, center_point.y() + 1.5)));
+ - ]
570 [ + - ]: 6 : stateInfo.continue_forward_trajectory_set.push_back(
571 [ + - + - : 12 : ::ad::geometry::toDistance(::ad::geometry::Point(center_point.x() - 0.5, center_point.y() + 2.5)));
+ - ]
572 : : }
573 : : else
574 : : {
575 [ + - ]: 2 : stateInfo.brake_trajectory_set.push_back(
576 [ + - + - : 2 : ::ad::geometry::toDistance(::ad::geometry::Point(center_point.x() + 0.5, center_point.y() + 0.5)));
+ - ]
577 [ + - ]: 2 : stateInfo.continue_forward_trajectory_set.push_back(
578 [ + - + - : 2 : ::ad::geometry::toDistance(::ad::geometry::Point(center_point.x() + 0.5, center_point.y() + 0.5)));
+ - ]
579 : :
580 [ + - ]: 2 : stateInfo.brake_trajectory_set.push_back(
581 [ + - + - : 2 : ::ad::geometry::toDistance(::ad::geometry::Point(center_point.x() - 0.5, center_point.y() + 0.5)));
+ - ]
582 [ + - ]: 2 : stateInfo.continue_forward_trajectory_set.push_back(
583 [ + - + - : 2 : ::ad::geometry::toDistance(::ad::geometry::Point(center_point.x() - 0.5, center_point.y() + 0.5)));
+ - ]
584 : :
585 [ + - ]: 2 : stateInfo.brake_trajectory_set.push_back(
586 [ + - + - : 2 : ::ad::geometry::toDistance(::ad::geometry::Point(center_point.x() - 0.5, center_point.y() - 1.5)));
+ - ]
587 [ + - ]: 2 : stateInfo.continue_forward_trajectory_set.push_back(
588 [ + - + - : 2 : ::ad::geometry::toDistance(::ad::geometry::Point(center_point.x() - 0.5, center_point.y() - 2.5)));
+ - ]
589 : :
590 [ + - ]: 2 : stateInfo.brake_trajectory_set.push_back(
591 [ + - + - : 2 : ::ad::geometry::toDistance(::ad::geometry::Point(center_point.x() + 0.5, center_point.y() - 1.5)));
+ - ]
592 [ + - ]: 2 : stateInfo.continue_forward_trajectory_set.push_back(
593 [ + - + - : 4 : ::ad::geometry::toDistance(::ad::geometry::Point(center_point.x() + 0.5, center_point.y() - 2.5)));
+ - ]
594 : : }
595 : 8 : stateInfo.brake_trajectory_set.push_back(stateInfo.brake_trajectory_set.front());
596 : 8 : stateInfo.continue_forward_trajectory_set.push_back(stateInfo.continue_forward_trajectory_set.front());
597 : 8 : }
598 : :
599 : : } // namespace rss
600 : : } // namespace ad
|
