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 : :
9 : : #include "RssStructuredSceneIntersectionChecker.hpp"
10 : : #include <cmath>
11 : : #include <limits>
12 : : #include "ad/rss/situation/Physics.hpp"
13 : : #include "ad/rss/situation/RssFormulas.hpp"
14 : :
15 : : namespace ad {
16 : :
17 : : using physics::Duration;
18 : :
19 : : namespace rss {
20 : : namespace situation {
21 : :
22 : : using situation::calculateTimeToCoverDistance;
23 : :
24 : 241 : bool RssStructuredSceneIntersectionChecker::checkLateralIntersect(Situation const &situation, bool &isSafe)
25 : : {
26 : 241 : isSafe = false;
27 : :
28 : : /**
29 : : * Check if in any case the first vehicle has passed the intersection before the other
30 : : * vehicle arrives at that point
31 : : */
32 : :
33 : 241 : Duration timeToReachEgo;
34 : 241 : Duration timeToReachOther;
35 : 241 : Duration timeToLeaveEgo;
36 : 241 : Duration timeToLeaveOther;
37 : :
38 [ + - ]: 241 : bool result = calculateTimeToCoverDistance(situation.egoVehicleState.velocity.speedLon.maximum,
39 : : situation.egoVehicleState.dynamics.maxSpeedOnAcceleration,
40 : : situation.egoVehicleState.dynamics.responseTime,
41 : : situation.egoVehicleState.dynamics.alphaLon.accelMax,
42 : : situation.egoVehicleState.dynamics.alphaLon.brakeMin,
43 : : situation.egoVehicleState.distanceToEnterIntersection,
44 : : timeToReachEgo);
45 : :
46 : 241 : result = result
47 [ + - + - : 241 : && calculateTimeToCoverDistance(situation.otherVehicleState.velocity.speedLon.maximum,
+ - ]
48 : : situation.egoVehicleState.dynamics.maxSpeedOnAcceleration,
49 : : situation.otherVehicleState.dynamics.responseTime,
50 : : situation.otherVehicleState.dynamics.alphaLon.accelMax,
51 : : situation.otherVehicleState.dynamics.alphaLon.brakeMin,
52 : : situation.otherVehicleState.distanceToEnterIntersection,
53 : : timeToReachOther);
54 : :
55 : 241 : result = result
56 [ + - + - : 241 : && calculateTimeToCoverDistance(situation.egoVehicleState.velocity.speedLon.minimum,
+ - ]
57 : : situation.egoVehicleState.dynamics.maxSpeedOnAcceleration,
58 : : situation.egoVehicleState.dynamics.responseTime,
59 : : situation.egoVehicleState.dynamics.alphaLon.brakeMax,
60 : : situation.egoVehicleState.dynamics.alphaLon.brakeMax,
61 : : situation.egoVehicleState.distanceToLeaveIntersection,
62 : : timeToLeaveEgo);
63 : :
64 : 241 : result = result
65 [ + - + - : 241 : && calculateTimeToCoverDistance(situation.otherVehicleState.velocity.speedLon.minimum,
+ - ]
66 : : situation.egoVehicleState.dynamics.maxSpeedOnAcceleration,
67 : : situation.otherVehicleState.dynamics.responseTime,
68 : : situation.otherVehicleState.dynamics.alphaLon.brakeMax,
69 : : situation.otherVehicleState.dynamics.alphaLon.brakeMax,
70 : : situation.otherVehicleState.distanceToLeaveIntersection,
71 : : timeToLeaveOther);
72 : :
73 [ + - ]: 241 : if (result)
74 : : {
75 [ + - + - : 482 : if ((timeToReachEgo > timeToLeaveOther) || (timeToReachOther > timeToLeaveEgo)
+ + ]
76 [ + - + - : 571 : || ((timeToReachEgo == std::numeric_limits<Duration>::max())
+ + ]
77 [ + - - + : 330 : && (timeToReachOther == std::numeric_limits<Duration>::max())))
+ + ]
78 : : {
79 : : /**
80 : : * Currently we assume lateral overlap when there is a vehicle in an intersection
81 : : * @todo This could be relaxed by calculating lateral distance here as well
82 : : */
83 : 1 : isSafe = true;
84 : : }
85 : : }
86 : :
87 : 241 : return result;
88 : : }
89 : :
90 : 804 : bool RssStructuredSceneIntersectionChecker::checkIntersectionSafe(Situation const &situation,
91 : : state::RssStateInformation &rssStateInformation,
92 : : bool &isSafe,
93 : : IntersectionState &intersectionState)
94 : : {
95 : 804 : if ((situation.egoVehicleState.distanceToLeaveIntersection < situation.egoVehicleState.distanceToEnterIntersection)
96 [ + + + + ]: 1607 : || (situation.otherVehicleState.distanceToLeaveIntersection
97 [ + + ]: 803 : < situation.otherVehicleState.distanceToEnterIntersection))
98 : : {
99 : 2 : return false;
100 : : }
101 : :
102 : 802 : bool result = true;
103 : 802 : isSafe = false;
104 : :
105 : : /**
106 : : * Check if a non prio vehicle has safe distance to the intersection
107 : : */
108 [ + + ]: 802 : if (!situation.egoVehicleState.hasPriority)
109 : : {
110 : 417 : rssStateInformation.evaluator = state::RssStateEvaluator::IntersectionOtherPriorityEgoAbleToStop;
111 : 417 : rssStateInformation.currentDistance = situation.egoVehicleState.distanceToEnterIntersection;
112 : 417 : result = checkStopInFrontIntersection(situation.egoVehicleState, rssStateInformation.safeDistance, isSafe);
113 : : }
114 [ + - + + : 802 : if (result && !isSafe && !situation.otherVehicleState.hasPriority)
+ + ]
115 : : {
116 : 430 : rssStateInformation.evaluator = state::RssStateEvaluator::IntersectionEgoPriorityOtherAbleToStop;
117 : 430 : rssStateInformation.currentDistance = situation.otherVehicleState.distanceToEnterIntersection;
118 : 430 : result = checkStopInFrontIntersection(situation.otherVehicleState, rssStateInformation.safeDistance, isSafe);
119 : : }
120 : :
121 [ + + ]: 802 : if (isSafe)
122 : : {
123 : 333 : intersectionState = IntersectionState::NonPrioAbleToBreak;
124 : : }
125 [ + - ]: 469 : else if (result)
126 : : {
127 : 469 : rssStateInformation.currentDistance = situation.relativePosition.longitudinalDistance;
128 : : /**
129 : : * Check if there is a safe longitudinal distance between the vehicles
130 : : */
131 [ + + ]: 469 : if (situation.relativePosition.longitudinalPosition == LongitudinalRelativePosition::InFront)
132 : : {
133 : 114 : rssStateInformation.evaluator = state::RssStateEvaluator::IntersectionEgoInFront;
134 : 114 : result = checkSafeLongitudinalDistanceSameDirection(situation.egoVehicleState,
135 : 114 : situation.otherVehicleState,
136 : 114 : situation.relativePosition.longitudinalDistance,
137 : 114 : rssStateInformation.safeDistance,
138 : : isSafe);
139 : : }
140 : : else
141 : : {
142 : 355 : rssStateInformation.evaluator = state::RssStateEvaluator::IntersectionOtherInFront;
143 : 355 : result = checkSafeLongitudinalDistanceSameDirection(situation.otherVehicleState,
144 : 355 : situation.egoVehicleState,
145 : 355 : situation.relativePosition.longitudinalDistance,
146 : 355 : rssStateInformation.safeDistance,
147 : : isSafe);
148 : : }
149 [ + + ]: 469 : if (isSafe)
150 : : {
151 : 228 : intersectionState = IntersectionState::SafeLongitudinalDistance;
152 : : }
153 [ + - ]: 241 : else if (result)
154 : : {
155 : 241 : rssStateInformation.evaluator = state::RssStateEvaluator::IntersectionOverlap;
156 : 241 : rssStateInformation.currentDistance = physics::Distance(0.);
157 : 241 : rssStateInformation.safeDistance = physics::Distance(0.);
158 : 241 : result = checkLateralIntersect(situation, isSafe);
159 : :
160 [ + + ]: 241 : if (isSafe)
161 : : {
162 : 1 : intersectionState = IntersectionState::NoTimeOverlap;
163 : : }
164 : : }
165 : : }
166 : :
167 : 802 : return result;
168 : : }
169 : :
170 : 805 : bool RssStructuredSceneIntersectionChecker::calculateRssStateIntersection(world::TimeIndex const &timeIndex,
171 : : Situation const &situation,
172 : : state::RssState &rssState)
173 : : {
174 [ + + + + ]: 805 : if (situation.egoVehicleState.hasPriority && situation.otherVehicleState.hasPriority)
175 : : {
176 : : // both cannot have priority over the other at the same time
177 : 1 : return false;
178 : : }
179 : 804 : bool result = false;
180 : : try
181 : : {
182 [ + - ]: 804 : if (timeIndex != mCurrentTimeIndex)
183 : : {
184 : : /**
185 : : * next time step: current safe state map becomes last state now
186 : : */
187 : 804 : mLastSafeStateMap.swap(mNewSafeStateMap);
188 : 804 : mNewSafeStateMap.clear();
189 : 804 : mCurrentTimeIndex = timeIndex;
190 : : }
191 : :
192 : 804 : rssState.situationId = situation.situationId;
193 : 804 : rssState.situationType = situation.situationType;
194 : 804 : rssState.objectId = situation.objectId;
195 : :
196 : 804 : rssState.longitudinalState.isSafe = false;
197 : 804 : rssState.longitudinalState.response = state::LongitudinalResponse::BrakeMin;
198 : 804 : rssState.longitudinalState.alphaLon = situation.egoVehicleState.dynamics.alphaLon;
199 : :
200 : : /**
201 : : * An intersection situation is lateral unsafe but usually doesn't require a lateral brake
202 : : * @todo: if taking lateral intersection handling into account, this also has to be updated
203 : : */
204 : 804 : rssState.lateralStateLeft.isSafe = false;
205 : 804 : rssState.lateralStateLeft.response = state::LateralResponse::None;
206 : 804 : rssState.lateralStateLeft.alphaLat = situation.egoVehicleState.dynamics.alphaLat;
207 : 804 : rssState.lateralStateLeft.rssStateInformation.evaluator = state::RssStateEvaluator::LateralDistance;
208 : 804 : rssState.lateralStateLeft.rssStateInformation.currentDistance = physics::Distance(0);
209 : 804 : rssState.lateralStateLeft.rssStateInformation.safeDistance = physics::Distance(0);
210 : 804 : rssState.lateralStateRight.isSafe = false;
211 : 804 : rssState.lateralStateRight.response = state::LateralResponse::None;
212 : 804 : rssState.lateralStateRight.alphaLat = situation.egoVehicleState.dynamics.alphaLat;
213 : 804 : rssState.lateralStateRight.rssStateInformation.evaluator = state::RssStateEvaluator::LateralDistance;
214 : 804 : rssState.lateralStateRight.rssStateInformation.currentDistance = physics::Distance(0);
215 : 804 : rssState.lateralStateRight.rssStateInformation.safeDistance = physics::Distance(0);
216 : :
217 : 804 : bool isSafe = false;
218 : 804 : IntersectionState intersectionState = IntersectionState::NonPrioAbleToBreak;
219 : :
220 : : /**
221 : : * Check if the intersection is safe and determine the intersection state of the situation
222 : : */
223 : : result
224 [ + - ]: 804 : = checkIntersectionSafe(situation, rssState.longitudinalState.rssStateInformation, isSafe, intersectionState);
225 : :
226 [ + + ]: 804 : if (result)
227 : : {
228 : 802 : rssState.longitudinalState.isSafe = isSafe;
229 : :
230 [ + - ]: 802 : auto const previousIntersectionState = mLastSafeStateMap.find(situation.situationId);
231 : :
232 [ + + ]: 802 : if (!isSafe)
233 : : {
234 : : /**
235 : : * Situation is unsafe determine proper response
236 : : */
237 [ + + ]: 240 : if (previousIntersectionState != mLastSafeStateMap.end())
238 : : {
239 [ + + + - ]: 238 : switch (previousIntersectionState->second)
240 : : {
241 : 77 : case IntersectionState::NonPrioAbleToBreak:
242 : : {
243 [ + + ]: 77 : if (situation.egoVehicleState.hasPriority)
244 : : {
245 : 1 : rssState.longitudinalState.response = state::LongitudinalResponse::None;
246 : : }
247 : 77 : break;
248 : : }
249 : 160 : case IntersectionState::SafeLongitudinalDistance:
250 : : {
251 [ + + ]: 160 : if (situation.relativePosition.longitudinalPosition == LongitudinalRelativePosition::InFront)
252 : : {
253 : 52 : rssState.longitudinalState.response = state::LongitudinalResponse::None;
254 : : }
255 : 160 : break;
256 : : }
257 : 1 : case IntersectionState::NoTimeOverlap:
258 : : {
259 : : // @todo If we don't assume always lateral overlap we might need to brake laterally as well
260 : 1 : rssState.longitudinalState.response = state::LongitudinalResponse::BrakeMin;
261 : 1 : break;
262 : : }
263 : 0 : default:
264 : : {
265 : : // LCOV_EXCL_START: unreachable code, keep to be on the safe side
266 : : result = false;
267 : : break;
268 : : // LCOV_EXCL_STOP: unreachable code, keep to be on the safe side
269 : : }
270 : : }
271 : :
272 : : /**
273 : : * Store the last safe intersection state for next time step
274 : : */
275 : : mNewSafeStateMap.insert(
276 [ + - ]: 238 : RssIntersectionStateMap::value_type(situation.situationId, previousIntersectionState->second));
277 : : }
278 : : else
279 : : {
280 : : /**
281 : : * Situation is unsafe but there is no previous safe state. Therefore we can only decide to brake.
282 : : */
283 : 2 : rssState.longitudinalState.response = state::LongitudinalResponse::BrakeMin;
284 : : }
285 : : }
286 : : else
287 : : {
288 : : /**
289 : : * Situation is safe
290 : : */
291 : 562 : rssState.longitudinalState.response = state::LongitudinalResponse::None;
292 : :
293 : : // store the new safe state
294 [ + + ]: 562 : mNewSafeStateMap.insert(RssIntersectionStateMap::value_type(situation.situationId, intersectionState));
295 : : }
296 : : }
297 : : }
298 : 3 : catch (...)
299 : : {
300 : 3 : result = false;
301 : : }
302 : 804 : return result;
303 : : }
304 : :
305 : : } // namespace situation
306 : : } // namespace rss
307 : : } // namespace ad
|
