Branch data Line data Source code
1 : : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
2 : : //
3 : : // Copyright (C) 2018-2020 Intel Corporation
4 : : //
5 : : // SPDX-License-Identifier: LGPL-2.1-only
6 : : //
7 : : // ----------------- END LICENSE BLOCK -----------------------------------
8 : :
9 : : #include "TestSupport.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.alphaLon = getEgoRssDynamics().alphaLon;
20 : 63 : state.isSafe = true;
21 : 63 : state.rssStateInformation.currentDistance = physics::Distance::getMax();
22 : 63 : state.rssStateInformation.safeDistance = physics::Distance::getMax();
23 : 63 : state.rssStateInformation.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.alphaLat = getEgoRssDynamics().alphaLat;
30 : 125 : state.isSafe = true;
31 : 125 : state.rssStateInformation.currentDistance = physics::Distance::getMax();
32 : 125 : state.rssStateInformation.safeDistance = physics::Distance::getMax();
33 : 125 : state.rssStateInformation.evaluator = state::RssStateEvaluator::None;
34 : 125 : }
35 : :
36 : 55 : void resetRssState(state::UnstructuredSceneRssState &state)
37 : : {
38 : 55 : state.response = state::UnstructuredSceneResponse::None;
39 : 55 : state.alphaLon = getEgoRssDynamics().alphaLon;
40 : 55 : state.headingRange.begin = ad::physics::Angle(0.0);
41 : 55 : state.headingRange.end = ad::physics::c2PI;
42 : 55 : state.isSafe = true;
43 : 55 : }
44 : :
45 : 55 : void resetRssState(state::RssState &rssState,
46 : : situation::SituationId const situationId,
47 : : world::ObjectId const objectId,
48 : : situation::SituationType const situationType)
49 : : {
50 : 55 : rssState.situationId = situationId;
51 : 55 : rssState.objectId = objectId;
52 : 55 : rssState.situationType = situationType;
53 : 55 : resetRssState(rssState.longitudinalState);
54 : 55 : resetRssState(rssState.lateralStateLeft);
55 : 55 : resetRssState(rssState.lateralStateRight);
56 : 55 : resetRssState(rssState.unstructuredSceneState);
57 : 55 : }
58 : :
59 : 27 : void resetRssState(state::ProperResponse &properResponse)
60 : : {
61 : 27 : properResponse.isSafe = true;
62 : 27 : properResponse.timeIndex = 1u;
63 : 27 : properResponse.dangerousObjects.clear();
64 : 27 : properResponse.longitudinalResponse = state::LongitudinalResponse::None;
65 : 27 : properResponse.lateralResponseLeft = state::LateralResponse::None;
66 : 27 : properResponse.lateralResponseRight = state::LateralResponse::None;
67 : :
68 : 27 : properResponse.headingRanges.clear();
69 : 27 : state::HeadingRange initialHeadingRange;
70 : 27 : initialHeadingRange.begin = ad::physics::Angle(0.0);
71 : 27 : initialHeadingRange.end = ad::physics::c2PI;
72 [ + - ]: 27 : properResponse.headingRanges.push_back(initialHeadingRange);
73 : 27 : }
74 : :
75 : 1058 : world::RssDynamics getObjectRssDynamics()
76 : : {
77 : 1058 : world::RssDynamics rssDynamics;
78 : :
79 : 1058 : rssDynamics.alphaLon.accelMax = cMaximumLongitudinalAcceleration;
80 : 1058 : rssDynamics.alphaLon.brakeMax = cMaximumLongitudinalBrakingDeceleleration;
81 : 1058 : rssDynamics.alphaLon.brakeMin = cMinimumLongitudinalBrakingDeceleleration;
82 : 1058 : rssDynamics.alphaLon.brakeMinCorrect = cMinimumLongitudinalBrakingDecelelerationCorrect;
83 : :
84 : 1058 : rssDynamics.alphaLat.accelMax = cMaximumLateralAcceleration;
85 : 1058 : rssDynamics.alphaLat.brakeMin = cMinimumLateralBrakingDeceleleration;
86 : :
87 : 1058 : rssDynamics.lateralFluctuationMargin = ad::physics::Distance(0.1);
88 : 1058 : rssDynamics.responseTime = cResponseTimeOtherVehicles;
89 : :
90 : 1058 : rssDynamics.unstructuredSettings.pedestrianTurningRadius = ad::physics::Distance(2.0);
91 : 1058 : rssDynamics.unstructuredSettings.driveAwayMaxAngle = ad::physics::Angle(2.4);
92 : 1058 : rssDynamics.unstructuredSettings.vehicleYawRateChange = ad::physics::AngularAcceleration(0.3);
93 : 1058 : rssDynamics.unstructuredSettings.vehicleMinRadius = ad::physics::Distance(3.5);
94 : 1058 : rssDynamics.unstructuredSettings.vehicleTrajectoryCalculationStep = ad::physics::Duration(0.2);
95 : :
96 : 1058 : rssDynamics.unstructuredSettings.vehicleFrontIntermediateYawRateChangeRatioSteps = 3;
97 : 1058 : rssDynamics.unstructuredSettings.vehicleBackIntermediateYawRateChangeRatioSteps = 0;
98 : 1058 : rssDynamics.unstructuredSettings.vehicleBrakeIntermediateAccelerationSteps = 1;
99 : 1058 : rssDynamics.unstructuredSettings.vehicleContinueForwardIntermediateAccelerationSteps = 0;
100 : 1058 : rssDynamics.unstructuredSettings.vehicleContinueForwardIntermediateYawRateChangeRatioSteps = 3;
101 : 1058 : rssDynamics.unstructuredSettings.pedestrianContinueForwardIntermediateHeadingChangeRatioSteps = 3;
102 : 1058 : rssDynamics.unstructuredSettings.pedestrianContinueForwardIntermediateAccelerationSteps = 0;
103 : 1058 : rssDynamics.unstructuredSettings.pedestrianBrakeIntermediateAccelerationSteps = 3;
104 : 1058 : rssDynamics.unstructuredSettings.pedestrianFrontIntermediateHeadingChangeRatioSteps = 4;
105 : 1058 : rssDynamics.unstructuredSettings.pedestrianBackIntermediateHeadingChangeRatioSteps = 0;
106 : :
107 : 1058 : return rssDynamics;
108 : : }
109 : :
110 : 1961 : world::RssDynamics getEgoRssDynamics()
111 : : {
112 : 1961 : world::RssDynamics rssDynamics;
113 : :
114 : 1961 : rssDynamics.alphaLon.accelMax = cMaximumLongitudinalAcceleration;
115 : 1961 : rssDynamics.alphaLon.brakeMax = cMaximumLongitudinalBrakingDeceleleration;
116 : 1961 : rssDynamics.alphaLon.brakeMin = cMinimumLongitudinalBrakingDeceleleration;
117 : 1961 : rssDynamics.alphaLon.brakeMinCorrect = cMinimumLongitudinalBrakingDecelelerationCorrect;
118 : :
119 : 1961 : rssDynamics.alphaLat.accelMax = cMaximumLateralAcceleration;
120 : 1961 : rssDynamics.alphaLat.brakeMin = cMinimumLateralBrakingDeceleleration;
121 : :
122 : 1961 : rssDynamics.lateralFluctuationMargin = ad::physics::Distance(0.1);
123 : 1961 : rssDynamics.responseTime = cResponseTimeEgoVehicle;
124 : :
125 : 1961 : rssDynamics.unstructuredSettings.pedestrianTurningRadius = ad::physics::Distance(2.0);
126 : 1961 : rssDynamics.unstructuredSettings.driveAwayMaxAngle = ad::physics::Angle(2.4);
127 : 1961 : rssDynamics.unstructuredSettings.vehicleYawRateChange = ad::physics::AngularAcceleration(0.3);
128 : 1961 : rssDynamics.unstructuredSettings.vehicleMinRadius = ad::physics::Distance(3.5);
129 : 1961 : rssDynamics.unstructuredSettings.vehicleTrajectoryCalculationStep = ad::physics::Duration(0.2);
130 : :
131 : 1961 : rssDynamics.unstructuredSettings.vehicleFrontIntermediateYawRateChangeRatioSteps = 3;
132 : 1961 : rssDynamics.unstructuredSettings.vehicleBackIntermediateYawRateChangeRatioSteps = 0;
133 : 1961 : rssDynamics.unstructuredSettings.vehicleBrakeIntermediateAccelerationSteps = 1;
134 : 1961 : rssDynamics.unstructuredSettings.vehicleContinueForwardIntermediateAccelerationSteps = 0;
135 : 1961 : rssDynamics.unstructuredSettings.vehicleContinueForwardIntermediateYawRateChangeRatioSteps = 3;
136 : 1961 : rssDynamics.unstructuredSettings.pedestrianContinueForwardIntermediateHeadingChangeRatioSteps = 3;
137 : 1961 : rssDynamics.unstructuredSettings.pedestrianContinueForwardIntermediateAccelerationSteps = 0;
138 : 1961 : rssDynamics.unstructuredSettings.pedestrianBrakeIntermediateAccelerationSteps = 3;
139 : 1961 : rssDynamics.unstructuredSettings.pedestrianFrontIntermediateHeadingChangeRatioSteps = 4;
140 : 1961 : rssDynamics.unstructuredSettings.pedestrianBackIntermediateHeadingChangeRatioSteps = 0;
141 : 1961 : return rssDynamics;
142 : : }
143 : :
144 : 5870 : world::Object createObject(double const lonVelocity, double const latVelocity)
145 : : {
146 : 5870 : world::Object object;
147 : :
148 : 5870 : object.objectType = world::ObjectType::OtherVehicle;
149 : 5870 : object.velocity.speedLonMin = kmhToMeterPerSec(lonVelocity);
150 : 5870 : object.velocity.speedLonMax = kmhToMeterPerSec(lonVelocity);
151 : 5870 : object.velocity.speedLatMin = kmhToMeterPerSec(latVelocity);
152 : 5870 : object.velocity.speedLatMax = kmhToMeterPerSec(latVelocity);
153 [ + - ]: 5870 : object.state = createObjectState(lonVelocity, latVelocity);
154 : 5870 : return object;
155 : : }
156 : :
157 : 6081 : world::ObjectState createObjectState(double const lonVelocity, double const latVelocity)
158 : : {
159 : 6081 : world::ObjectState state;
160 : 6081 : state.yaw = ad::physics::Angle(0.0);
161 : 6081 : state.steeringAngle = ad::physics::Angle(0.0);
162 : 6081 : state.dimension.length = ad::physics::Distance(4.0);
163 : 6081 : state.dimension.width = ad::physics::Distance(2.0);
164 : 6081 : state.yawRate = ad::physics::AngularVelocity(0.0);
165 : 6081 : state.centerPoint.x = ad::physics::Distance(0.0);
166 : 6081 : state.centerPoint.y = ad::physics::Distance(0.0);
167 [ + - ]: 6081 : state.speed = ad::physics::Speed(std::sqrt(kmhToMeterPerSec(lonVelocity) * kmhToMeterPerSec(lonVelocity)
168 [ + - + - : 12162 : + kmhToMeterPerSec(latVelocity) * kmhToMeterPerSec(latVelocity)));
+ - ]
169 : 6081 : return state;
170 : : }
171 : :
172 : : situation::VehicleState
173 : 211 : createVehicleState(world::ObjectType const objectType, double const lonVelocity, double const latVelocity)
174 : : {
175 : 211 : situation::VehicleState state;
176 : :
177 : 211 : state.velocity.speedLon.minimum = kmhToMeterPerSec(lonVelocity);
178 : 211 : state.velocity.speedLon.maximum = state.velocity.speedLon.minimum;
179 : 211 : state.velocity.speedLat.minimum = kmhToMeterPerSec(latVelocity);
180 : 211 : state.velocity.speedLat.maximum = state.velocity.speedLat.minimum;
181 : 211 : state.dynamics = getObjectRssDynamics();
182 : 211 : state.distanceToEnterIntersection = Distance(0.);
183 : 211 : state.distanceToLeaveIntersection = Distance(1000.);
184 : 211 : state.hasPriority = false;
185 : 211 : state.isInCorrectLane = true;
186 : 211 : state.objectType = objectType;
187 : 211 : state.objectState = createObjectState(lonVelocity, latVelocity);
188 : 211 : return state;
189 : : }
190 : :
191 : 44 : situation::RelativePosition createRelativeLongitudinalPosition(situation::LongitudinalRelativePosition const &position,
192 : : Distance const &distance)
193 : : {
194 : 44 : situation::RelativePosition relativePosition;
195 : 44 : relativePosition.lateralDistance = Distance(0.);
196 : 44 : relativePosition.lateralPosition = situation::LateralRelativePosition::Overlap;
197 : 44 : relativePosition.longitudinalDistance = distance;
198 : 44 : relativePosition.longitudinalPosition = position;
199 : 44 : return relativePosition;
200 : : }
201 : :
202 : 29 : situation::RelativePosition createRelativeLateralPosition(situation::LateralRelativePosition const &position,
203 : : Distance const &distance)
204 : : {
205 : 29 : situation::RelativePosition relativePosition;
206 : 29 : relativePosition.lateralDistance = distance;
207 : 29 : relativePosition.lateralPosition = position;
208 : 29 : relativePosition.longitudinalDistance = Distance(0.);
209 : 29 : relativePosition.longitudinalPosition = situation::LongitudinalRelativePosition::Overlap;
210 : 29 : return relativePosition;
211 : : }
212 : :
213 : 3120 : Distance calculateLongitudinalStoppingDistance(Speed const &objectSpeed,
214 : : Speed const &objectMaxSpeedOnAcceleration,
215 : : Acceleration const &acceleration,
216 : : Acceleration const &deceleration,
217 : : Duration const &responseTime)
218 : : {
219 : 3120 : Duration accelerationTime = responseTime;
220 [ + - + - : 3120 : Speed speedMax = std::min(objectMaxSpeedOnAcceleration, objectSpeed + responseTime * acceleration);
+ - ]
221 [ + - + + ]: 3120 : if (objectSpeed >= objectMaxSpeedOnAcceleration)
222 : : {
223 : : // no acceleration if already faster than maxSpeedOnAcceleration
224 : 5 : accelerationTime = Duration(0.);
225 : 5 : speedMax = objectSpeed;
226 : : }
227 [ + - + + ]: 3115 : else if (acceleration != Acceleration(0.))
228 : : {
229 : : // maybe the acceleration time is less, if maxSpeedOnAcceleration is reached before response time
230 [ + - + - : 3114 : accelerationTime = std::min(accelerationTime, std::fabs(objectMaxSpeedOnAcceleration - objectSpeed) / acceleration);
+ - ]
231 : : }
232 [ + - ]: 3120 : Distance dMin = objectSpeed * accelerationTime;
233 [ + - + - : 3120 : dMin += 0.5 * acceleration * accelerationTime * accelerationTime;
+ - + - ]
234 [ + - + - : 3120 : dMin += speedMax * (responseTime - accelerationTime);
+ - ]
235 [ + - + - : 3120 : dMin += (speedMax * speedMax) / (2. * -deceleration);
+ - + - +
- ]
236 : 3120 : return dMin;
237 : : }
238 : :
239 : 663 : Distance calculateLongitudinalMinSafeDistance(physics::Speed const &followingObjectSpeed,
240 : : world::RssDynamics const &followingObjectRssDynamics,
241 : : physics::Speed const &leadingObjectSpeed,
242 : : world::RssDynamics const &leadingObjectRssDynamics)
243 : : {
244 : : Distance const followingStoppingDistance
245 : : = calculateLongitudinalStoppingDistance(followingObjectSpeed,
246 : 663 : followingObjectRssDynamics.maxSpeedOnAcceleration,
247 : 663 : followingObjectRssDynamics.alphaLon.accelMax,
248 : 663 : followingObjectRssDynamics.alphaLon.brakeMin,
249 [ + - ]: 663 : followingObjectRssDynamics.responseTime);
250 : : Distance const leadingStoppingDistance
251 : : = calculateLongitudinalStoppingDistance(leadingObjectSpeed,
252 : 663 : leadingObjectRssDynamics.maxSpeedOnAcceleration,
253 : 663 : leadingObjectRssDynamics.alphaLon.accelMax,
254 : 663 : leadingObjectRssDynamics.alphaLon.brakeMax,
255 [ + - ]: 663 : Duration(0));
256 [ + - ]: 663 : Distance const dMin = followingStoppingDistance - leadingStoppingDistance;
257 [ + - ]: 663 : return std::max(dMin, Distance(0.));
258 : : }
259 : :
260 : : Distance
261 : 882 : calculateLongitudinalMinSafeDistanceOppositeDirection(physics::Speed const &objectInCorrectLaneSpeed,
262 : : world::RssDynamics const &objectInCorrectLaneRssDynamics,
263 : : physics::Speed const &objectNotInCorrectLaneSpeed,
264 : : world::RssDynamics const &objectNotInCorrectLaneRssDynamics)
265 : : {
266 : : Distance const correctStoppingDistance
267 : : = calculateLongitudinalStoppingDistance(objectInCorrectLaneSpeed,
268 : 882 : objectInCorrectLaneRssDynamics.maxSpeedOnAcceleration,
269 : 882 : objectInCorrectLaneRssDynamics.alphaLon.accelMax,
270 : 882 : objectInCorrectLaneRssDynamics.alphaLon.brakeMinCorrect,
271 [ + - ]: 882 : objectInCorrectLaneRssDynamics.responseTime);
272 : : Distance const notCorrectStoppingDistance
273 : : = calculateLongitudinalStoppingDistance(objectNotInCorrectLaneSpeed,
274 : 882 : objectNotInCorrectLaneRssDynamics.maxSpeedOnAcceleration,
275 : 882 : objectNotInCorrectLaneRssDynamics.alphaLon.accelMax,
276 : 882 : objectNotInCorrectLaneRssDynamics.alphaLon.brakeMin,
277 [ + - ]: 882 : objectNotInCorrectLaneRssDynamics.responseTime);
278 [ + - ]: 882 : Distance const dMin = correctStoppingDistance + notCorrectStoppingDistance;
279 : 882 : return dMin;
280 : : }
281 : :
282 : 745 : Distance calculateLateralMinSafeDistance(physics::Speed const &leftObjectSpeed,
283 : : world::RssDynamics const &leftObjectRssDynamics,
284 : : physics::Speed const &rightObjectSpeed,
285 : : world::RssDynamics const &rightObjectRssDynamics)
286 : : {
287 : : Speed lObjectVelAfterResTime
288 [ + - + - ]: 745 : = leftObjectSpeed + leftObjectRssDynamics.responseTime * leftObjectRssDynamics.alphaLat.accelMax;
289 : : Speed rObjectVelAfterResTime
290 [ + - + - ]: 745 : = rightObjectSpeed - rightObjectRssDynamics.responseTime * rightObjectRssDynamics.alphaLat.accelMax;
291 [ + - + - : 745 : Distance dMin = (leftObjectSpeed + lObjectVelAfterResTime) / 2. * leftObjectRssDynamics.responseTime;
+ - ]
292 [ + - + + ]: 745 : if (lObjectVelAfterResTime > Speed(0.))
293 : : {
294 [ + - + - : 655 : dMin += lObjectVelAfterResTime * lObjectVelAfterResTime / (2. * -leftObjectRssDynamics.alphaLat.brakeMin);
+ - + - +
- ]
295 : : }
296 [ + - + - : 745 : dMin -= (rightObjectSpeed + rObjectVelAfterResTime) / 2. * rightObjectRssDynamics.responseTime;
+ - + - ]
297 [ + - + + ]: 745 : if (rObjectVelAfterResTime < Speed(0.))
298 : : {
299 [ + - + - : 655 : dMin += rObjectVelAfterResTime * rObjectVelAfterResTime / (2. * -rightObjectRssDynamics.alphaLat.brakeMin);
+ - + - +
- ]
300 : : }
301 [ + - + - : 745 : dMin += 0.5 * (rightObjectRssDynamics.lateralFluctuationMargin + leftObjectRssDynamics.lateralFluctuationMargin);
+ - ]
302 [ + - ]: 745 : return std::max(dMin, Distance(0.));
303 : : }
304 : 1 : TestSupport::TestSupport()
305 : : {
306 : 1 : resetRssState(cLongitudinalSafe);
307 : 1 : cLongitudinalSafe.isSafe = true;
308 : 1 : cLongitudinalSafe.response = state::LongitudinalResponse::None;
309 : 1 : resetRssState(cLongitudinalNone);
310 : 1 : cLongitudinalNone.isSafe = false;
311 : 1 : cLongitudinalNone.response = state::LongitudinalResponse::None;
312 : 1 : resetRssState(cLongitudinalBrakeMin);
313 : 1 : cLongitudinalBrakeMin.isSafe = false;
314 : 1 : cLongitudinalBrakeMin.response = state::LongitudinalResponse::BrakeMin;
315 : 1 : resetRssState(cLongitudinalBrakeMinCorrect);
316 : 1 : cLongitudinalBrakeMinCorrect.isSafe = false;
317 : 1 : cLongitudinalBrakeMinCorrect.response = state::LongitudinalResponse::BrakeMinCorrect;
318 : 1 : resetRssState(cLateralSafe);
319 : 1 : cLateralSafe.isSafe = true;
320 : 1 : cLateralSafe.response = state::LateralResponse::None;
321 : 1 : resetRssState(cLateralNone);
322 : 1 : cLateralNone.isSafe = false;
323 : 1 : cLateralNone.response = state::LateralResponse::None;
324 : 1 : resetRssState(cLateralBrakeMin);
325 : 1 : cLateralBrakeMin.isSafe = false;
326 : 1 : cLateralBrakeMin.response = state::LateralResponse::BrakeMin;
327 : 1 : }
328 : :
329 : 125 : state::LateralRssState TestSupport::stateWithInformation(state::LateralRssState const &lateralState,
330 : : situation::Situation const &situation)
331 : : {
332 : 125 : state::LateralRssState resultState = lateralState;
333 : :
334 : 125 : resultState.alphaLat = situation.egoVehicleState.dynamics.alphaLat;
335 : 125 : resultState.rssStateInformation.evaluator = state::RssStateEvaluator::LateralDistance;
336 : 125 : resultState.rssStateInformation.currentDistance = situation.relativePosition.lateralDistance;
337 [ + + + - ]: 125 : switch (situation.situationType)
338 : : {
339 : 79 : case situation::SituationType::OppositeDirection:
340 : : case situation::SituationType::SameDirection:
341 [ + + ]: 79 : if (situation.relativePosition.lateralPosition == situation::LateralRelativePosition::AtLeft)
342 : : {
343 : : resultState.rssStateInformation.safeDistance
344 : 13 : = calculateLateralMinSafeDistance(situation.egoVehicleState.velocity.speedLat.maximum,
345 : 13 : situation.egoVehicleState.dynamics,
346 : 13 : situation.otherVehicleState.velocity.speedLat.minimum,
347 : 13 : situation.otherVehicleState.dynamics);
348 : : }
349 [ + + ]: 66 : else if (situation.relativePosition.lateralPosition == situation::LateralRelativePosition::AtRight)
350 : : {
351 : : resultState.rssStateInformation.safeDistance
352 : 10 : = calculateLateralMinSafeDistance(situation.otherVehicleState.velocity.speedLat.maximum,
353 : 10 : situation.otherVehicleState.dynamics,
354 : 10 : situation.egoVehicleState.velocity.speedLat.minimum,
355 : 10 : situation.egoVehicleState.dynamics);
356 : : }
357 : : else
358 : : {
359 : 56 : resultState.rssStateInformation.safeDistance = Distance(0.);
360 : : }
361 : 79 : break;
362 : 44 : case situation::SituationType::IntersectionEgoHasPriority:
363 : : case situation::SituationType::IntersectionObjectHasPriority:
364 : : case situation::SituationType::IntersectionSamePriority:
365 : 44 : resultState.rssStateInformation.safeDistance = Distance(0.);
366 : 44 : break;
367 : 2 : case situation::SituationType::NotRelevant:
368 : 2 : resultState.rssStateInformation.evaluator = state::RssStateEvaluator::None;
369 : 2 : resultState.rssStateInformation.currentDistance = Distance::getMax();
370 : 2 : resultState.rssStateInformation.safeDistance = Distance::getMax();
371 : 2 : break;
372 : 0 : default:
373 : 0 : resultState.rssStateInformation.currentDistance = Distance(-1.);
374 : 0 : resultState.rssStateInformation.safeDistance = Distance(-1.);
375 : 0 : break;
376 : : }
377 : :
378 : 125 : return resultState;
379 : : }
380 : :
381 : 74 : state::LongitudinalRssState TestSupport::stateWithInformation(state::LongitudinalRssState const &longitudinalState,
382 : : situation::Situation const &situation)
383 : : {
384 : 74 : state::LongitudinalRssState resultState = longitudinalState;
385 : :
386 : 74 : resultState.alphaLon = situation.egoVehicleState.dynamics.alphaLon;
387 : 74 : resultState.rssStateInformation.currentDistance = situation.relativePosition.longitudinalDistance;
388 : :
389 [ + + + + : 74 : switch (situation.situationType)
- ]
390 : : {
391 : 42 : case situation::SituationType::SameDirection:
392 [ + + ]: 42 : if ((situation.relativePosition.longitudinalPosition == situation::LongitudinalRelativePosition::InFront)
393 [ - + ]: 41 : || (situation.relativePosition.longitudinalPosition == situation::LongitudinalRelativePosition::OverlapFront))
394 : : {
395 : 1 : resultState.rssStateInformation.evaluator = state::RssStateEvaluator::LongitudinalDistanceSameDirectionEgoFront;
396 : : resultState.rssStateInformation.safeDistance
397 : 1 : = calculateLongitudinalMinSafeDistance(situation.otherVehicleState.velocity.speedLon.maximum,
398 : 1 : situation.otherVehicleState.dynamics,
399 : 1 : situation.egoVehicleState.velocity.speedLon.minimum,
400 : 1 : situation.egoVehicleState.dynamics);
401 : : }
402 : : else
403 : : {
404 : : resultState.rssStateInformation.evaluator
405 : 41 : = state::RssStateEvaluator::LongitudinalDistanceSameDirectionOtherInFront;
406 : : resultState.rssStateInformation.safeDistance
407 : 41 : = calculateLongitudinalMinSafeDistance(situation.egoVehicleState.velocity.speedLon.maximum,
408 : 41 : situation.egoVehicleState.dynamics,
409 : 41 : situation.otherVehicleState.velocity.speedLon.minimum,
410 : 41 : situation.otherVehicleState.dynamics);
411 : : }
412 : 42 : break;
413 : 9 : case situation::SituationType::OppositeDirection:
414 [ + + ]: 9 : if (situation.egoVehicleState.isInCorrectLane)
415 : : {
416 : : resultState.rssStateInformation.evaluator
417 : 8 : = state::RssStateEvaluator::LongitudinalDistanceOppositeDirectionEgoCorrectLane;
418 : : resultState.rssStateInformation.safeDistance
419 : 8 : = calculateLongitudinalMinSafeDistanceOppositeDirection(situation.egoVehicleState.velocity.speedLon.maximum,
420 : 8 : situation.egoVehicleState.dynamics,
421 : 8 : situation.otherVehicleState.velocity.speedLon.maximum,
422 : 8 : situation.otherVehicleState.dynamics);
423 : : }
424 : : else
425 : : {
426 : 1 : resultState.rssStateInformation.evaluator = state::RssStateEvaluator::LongitudinalDistanceOppositeDirection;
427 : : resultState.rssStateInformation.safeDistance
428 : 1 : = calculateLongitudinalMinSafeDistanceOppositeDirection(situation.otherVehicleState.velocity.speedLon.maximum,
429 : 1 : situation.otherVehicleState.dynamics,
430 : 1 : situation.egoVehicleState.velocity.speedLon.maximum,
431 : 1 : situation.egoVehicleState.dynamics);
432 : : }
433 : 9 : break;
434 : 22 : case situation::SituationType::IntersectionEgoHasPriority:
435 : : case situation::SituationType::IntersectionObjectHasPriority:
436 : : case situation::SituationType::IntersectionSamePriority:
437 [ + + ]: 22 : if (!situation.egoVehicleState.hasPriority)
438 : : {
439 : 18 : resultState.rssStateInformation.currentDistance = situation.egoVehicleState.distanceToEnterIntersection;
440 : : resultState.rssStateInformation.safeDistance
441 : 18 : = calculateLongitudinalStoppingDistance(situation.egoVehicleState.velocity.speedLon.maximum,
442 : 18 : situation.egoVehicleState.dynamics.maxSpeedOnAcceleration,
443 : 18 : situation.egoVehicleState.dynamics.alphaLon.accelMax,
444 : 18 : situation.egoVehicleState.dynamics.alphaLon.brakeMin,
445 : 18 : situation.egoVehicleState.dynamics.responseTime);
446 : 18 : resultState.rssStateInformation.evaluator = state::RssStateEvaluator::IntersectionOtherPriorityEgoAbleToStop;
447 [ + + ]: 18 : if (resultState.rssStateInformation.currentDistance > resultState.rssStateInformation.safeDistance)
448 : : {
449 : 2 : break;
450 : : }
451 : : }
452 [ + + ]: 20 : if (!situation.otherVehicleState.hasPriority)
453 : : {
454 : 12 : resultState.rssStateInformation.currentDistance = situation.otherVehicleState.distanceToEnterIntersection;
455 : : resultState.rssStateInformation.safeDistance
456 : 12 : = calculateLongitudinalStoppingDistance(situation.otherVehicleState.velocity.speedLon.maximum,
457 : 12 : situation.otherVehicleState.dynamics.maxSpeedOnAcceleration,
458 : 12 : situation.otherVehicleState.dynamics.alphaLon.accelMax,
459 : 12 : situation.otherVehicleState.dynamics.alphaLon.brakeMin,
460 : 12 : situation.otherVehicleState.dynamics.responseTime);
461 : 12 : resultState.rssStateInformation.evaluator = state::RssStateEvaluator::IntersectionEgoPriorityOtherAbleToStop;
462 [ + + ]: 12 : if (resultState.rssStateInformation.currentDistance > resultState.rssStateInformation.safeDistance)
463 : : {
464 : 5 : break;
465 : : }
466 : : }
467 : 15 : resultState.rssStateInformation.currentDistance = situation.relativePosition.longitudinalDistance;
468 [ + + ]: 15 : if (situation.relativePosition.longitudinalPosition == situation::LongitudinalRelativePosition::InFront)
469 : : {
470 : 9 : resultState.rssStateInformation.evaluator = state::RssStateEvaluator::IntersectionEgoInFront;
471 : : resultState.rssStateInformation.safeDistance
472 : 9 : = calculateLongitudinalMinSafeDistance(situation.otherVehicleState.velocity.speedLon.maximum,
473 : 9 : situation.otherVehicleState.dynamics,
474 : 9 : situation.egoVehicleState.velocity.speedLon.minimum,
475 : 9 : situation.egoVehicleState.dynamics);
476 : : }
477 [ + + ]: 6 : else if (situation.relativePosition.longitudinalPosition == situation::LongitudinalRelativePosition::AtBack)
478 : : {
479 : 5 : resultState.rssStateInformation.evaluator = state::RssStateEvaluator::IntersectionOtherInFront;
480 : : resultState.rssStateInformation.safeDistance
481 : 5 : = calculateLongitudinalMinSafeDistance(situation.egoVehicleState.velocity.speedLon.maximum,
482 : 5 : situation.egoVehicleState.dynamics,
483 : 5 : situation.otherVehicleState.velocity.speedLon.minimum,
484 : 5 : situation.otherVehicleState.dynamics);
485 : : }
486 [ + + ]: 15 : if (resultState.rssStateInformation.currentDistance > resultState.rssStateInformation.safeDistance)
487 : : {
488 : 6 : break;
489 : : }
490 : : else
491 : : {
492 : 9 : resultState.rssStateInformation.evaluator = state::RssStateEvaluator::IntersectionOverlap;
493 : 9 : resultState.rssStateInformation.currentDistance = Distance(0.);
494 : 9 : resultState.rssStateInformation.safeDistance = Distance(0.);
495 : : }
496 : 9 : break;
497 : 1 : case situation::SituationType::NotRelevant:
498 : 1 : resultState.rssStateInformation.evaluator = state::RssStateEvaluator::None;
499 : 1 : resultState.rssStateInformation.currentDistance = Distance::getMax();
500 : 1 : resultState.rssStateInformation.safeDistance = Distance::getMax();
501 : 1 : break;
502 : 0 : default:
503 : 0 : resultState.rssStateInformation.evaluator = state::RssStateEvaluator::None;
504 : 0 : resultState.rssStateInformation.currentDistance = Distance(-1.);
505 : 0 : resultState.rssStateInformation.safeDistance = Distance(-1.);
506 : 0 : break;
507 : : }
508 : :
509 : 74 : return resultState;
510 : : }
511 : :
512 : 8 : void getUnstructuredVehicle(unstructured::Point const ¢erPoint,
513 : : bool positiveDirection,
514 : : state::UnstructuredSceneStateInformation &stateInfo,
515 : : situation::VehicleState &vehicleState)
516 : : {
517 : 8 : vehicleState.objectState.centerPoint.x = ad::physics::Distance(centerPoint.x());
518 : 8 : vehicleState.objectState.centerPoint.y = ad::physics::Distance(centerPoint.y());
519 [ + + ]: 8 : if (positiveDirection)
520 : : {
521 : 6 : vehicleState.objectState.yaw = ad::physics::cPI_2;
522 : : }
523 : : else
524 : : {
525 [ + - ]: 2 : vehicleState.objectState.yaw = 3. * ad::physics::cPI_2;
526 : : }
527 : :
528 : 8 : stateInfo.brakeTrajectorySet.clear();
529 : 8 : stateInfo.continueForwardTrajectorySet.clear();
530 : : // brake
531 [ + + ]: 8 : if (positiveDirection)
532 : : {
533 [ + - ]: 6 : stateInfo.brakeTrajectorySet.push_back(
534 [ + - + - : 6 : unstructured::toDistance(unstructured::Point(centerPoint.x() - 0.5, centerPoint.y() - 0.5)));
+ - ]
535 [ + - ]: 6 : stateInfo.continueForwardTrajectorySet.push_back(
536 [ + - + - : 6 : unstructured::toDistance(unstructured::Point(centerPoint.x() - 0.5, centerPoint.y() - 0.5)));
+ - ]
537 : :
538 [ + - ]: 6 : stateInfo.brakeTrajectorySet.push_back(
539 [ + - + - : 6 : unstructured::toDistance(unstructured::Point(centerPoint.x() + 0.5, centerPoint.y() - 0.5)));
+ - ]
540 [ + - ]: 6 : stateInfo.continueForwardTrajectorySet.push_back(
541 [ + - + - : 6 : unstructured::toDistance(unstructured::Point(centerPoint.x() + 0.5, centerPoint.y() - 0.5)));
+ - ]
542 : :
543 [ + - ]: 6 : stateInfo.brakeTrajectorySet.push_back(
544 [ + - + - : 6 : unstructured::toDistance(unstructured::Point(centerPoint.x() + 0.5, centerPoint.y() + 1.5)));
+ - ]
545 [ + - ]: 6 : stateInfo.continueForwardTrajectorySet.push_back(
546 [ + - + - : 6 : unstructured::toDistance(unstructured::Point(centerPoint.x() + 0.5, centerPoint.y() + 2.5)));
+ - ]
547 : :
548 [ + - ]: 6 : stateInfo.brakeTrajectorySet.push_back(
549 [ + - + - : 6 : unstructured::toDistance(unstructured::Point(centerPoint.x() - 0.5, centerPoint.y() + 1.5)));
+ - ]
550 [ + - ]: 6 : stateInfo.continueForwardTrajectorySet.push_back(
551 [ + - + - : 12 : unstructured::toDistance(unstructured::Point(centerPoint.x() - 0.5, centerPoint.y() + 2.5)));
+ - ]
552 : : }
553 : : else
554 : : {
555 [ + - ]: 2 : stateInfo.brakeTrajectorySet.push_back(
556 [ + - + - : 2 : unstructured::toDistance(unstructured::Point(centerPoint.x() + 0.5, centerPoint.y() + 0.5)));
+ - ]
557 [ + - ]: 2 : stateInfo.continueForwardTrajectorySet.push_back(
558 [ + - + - : 2 : unstructured::toDistance(unstructured::Point(centerPoint.x() + 0.5, centerPoint.y() + 0.5)));
+ - ]
559 : :
560 [ + - ]: 2 : stateInfo.brakeTrajectorySet.push_back(
561 [ + - + - : 2 : unstructured::toDistance(unstructured::Point(centerPoint.x() - 0.5, centerPoint.y() + 0.5)));
+ - ]
562 [ + - ]: 2 : stateInfo.continueForwardTrajectorySet.push_back(
563 [ + - + - : 2 : unstructured::toDistance(unstructured::Point(centerPoint.x() - 0.5, centerPoint.y() + 0.5)));
+ - ]
564 : :
565 [ + - ]: 2 : stateInfo.brakeTrajectorySet.push_back(
566 [ + - + - : 2 : unstructured::toDistance(unstructured::Point(centerPoint.x() - 0.5, centerPoint.y() - 1.5)));
+ - ]
567 [ + - ]: 2 : stateInfo.continueForwardTrajectorySet.push_back(
568 [ + - + - : 2 : unstructured::toDistance(unstructured::Point(centerPoint.x() - 0.5, centerPoint.y() - 2.5)));
+ - ]
569 : :
570 [ + - ]: 2 : stateInfo.brakeTrajectorySet.push_back(
571 [ + - + - : 2 : unstructured::toDistance(unstructured::Point(centerPoint.x() + 0.5, centerPoint.y() - 1.5)));
+ - ]
572 [ + - ]: 2 : stateInfo.continueForwardTrajectorySet.push_back(
573 [ + - + - : 4 : unstructured::toDistance(unstructured::Point(centerPoint.x() + 0.5, centerPoint.y() - 2.5)));
+ - ]
574 : : }
575 : 8 : stateInfo.brakeTrajectorySet.push_back(stateInfo.brakeTrajectorySet.front());
576 : 8 : stateInfo.continueForwardTrajectorySet.push_back(stateInfo.continueForwardTrajectorySet.front());
577 : 8 : }
578 : :
579 : : } // namespace rss
580 : : } // namespace ad
|
