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 "ad/rss/structured/RssNonIntersectionConstellationChecker.hpp"
10 : : #include "ad/rss/core/Logging.hpp"
11 : : #include "ad/rss/state/RssStateOperation.hpp"
12 : : #include "ad/rss/structured/RssFormulas.hpp"
13 : :
14 : : namespace ad {
15 : : namespace rss {
16 : : namespace structured {
17 : :
18 : 66191 : bool RssNonIntersectionConstellationChecker::calculateRssStateNonIntersection(
19 : : world::TimeIndex const &time_index, core::RelativeConstellation const &constellation, state::RssState &rssState)
20 : : {
21 [ + + ]: 66191 : if (time_index != mCurrentTimeIndex)
22 : : {
23 : 3109 : mLastStatesBeforeDangerThresholdTime.swap(mNewStatesBeforeDangerThresholdTime);
24 : 3109 : mNewStatesBeforeDangerThresholdTime.clear();
25 : 3109 : mCurrentTimeIndex = time_index;
26 : : }
27 : :
28 : 66191 : rssState.constellation_id = constellation.constellation_id;
29 : 66191 : rssState.constellation_type = constellation.constellation_type;
30 : 66191 : rssState.ego_id = constellation.ego_id;
31 : 66191 : rssState.object_id = constellation.object_id;
32 : :
33 : 66191 : rssState.longitudinal_state.is_safe = false;
34 : 66191 : rssState.longitudinal_state.response = state::LongitudinalResponse::BrakeMin;
35 : 66191 : rssState.longitudinal_state.alpha_lon = constellation.ego_state.dynamics.alpha_lon;
36 : :
37 : 66191 : rssState.lateral_state_left.is_safe = false;
38 : 66191 : rssState.lateral_state_left.response = state::LateralResponse::BrakeMin;
39 : 66191 : rssState.lateral_state_left.alpha_lat = constellation.ego_state.dynamics.alpha_lat;
40 : :
41 : 66191 : rssState.lateral_state_right.is_safe = false;
42 : 66191 : rssState.lateral_state_right.response = state::LateralResponse::BrakeMin;
43 : 66191 : rssState.lateral_state_right.alpha_lat = constellation.ego_state.dynamics.alpha_lat;
44 : :
45 : 66191 : bool result = false;
46 : : // first calculate the current state
47 [ + + ]: 66191 : if (constellation.constellation_type == world::ConstellationType::SameDirection)
48 : : {
49 [ + - ]: 65123 : result = calculateRssStateSameDirection(constellation, rssState);
50 : : }
51 [ + - ]: 1068 : else if (constellation.constellation_type == world::ConstellationType::OppositeDirection)
52 : : {
53 [ + - ]: 1068 : result = calculateRssStateOppositeDirection(constellation, rssState);
54 : : }
55 : : else
56 : : {
57 [ # # # # ]: 0 : core::getLogger()->error("RssNonIntersectionConstellationChecker::calculateRssStateNonIntersection[{}->{}]>> "
58 : : "constellation type invalid {}",
59 : 0 : rssState.ego_id,
60 [ # # ]: 0 : rssState.object_id,
61 : : constellation);
62 : : }
63 : :
64 : : // second calculate proper response in respect to the state before danger threshold according to definition 10 of the
65 : : // RSS paper v6
66 : 66191 : RssSafeState lastStateBeforeDangerThresholdTimeToRemember;
67 [ + + ]: 66191 : if (isDangerous(rssState))
68 : : {
69 : : auto const previousLastStateBeforeDangerThresholdTime
70 [ + - ]: 13314 : = mLastStatesBeforeDangerThresholdTime.find(rssState.constellation_id);
71 [ + + ]: 13314 : if (previousLastStateBeforeDangerThresholdTime != mLastStatesBeforeDangerThresholdTime.end())
72 : : {
73 : 1278 : lastStateBeforeDangerThresholdTimeToRemember = previousLastStateBeforeDangerThresholdTime->second;
74 : : }
75 : : else
76 : : {
77 : : // This is the first time the constellation appears and it is immediately in dangerous state
78 : : // in same direction use-case we are able to deduce the non dangereous state:
79 : : // if the vehicles are actually overlapping in their lateral dimension, the vehicles are following each other
80 : : // Therefore, we don't require to break in lateral direction
81 : : // and can store a previous longitudinal safe state
82 : : // Example: something dropping from a vehicle driving in front or let the tracker of the perception system
83 : : // just loose the track of a vehicle in front/at back and assign it a new object id
84 : : // In such cases, a lateral proper response with breakMin would even prevent from an evasive maneuver
85 : : // which might want to evade laterally
86 [ + + ]: 12036 : if ((constellation.constellation_type == world::ConstellationType::SameDirection)
87 [ + + ]: 12028 : && ((constellation.relative_position.lateral_position == core::LateralRelativePosition::OverlapLeft)
88 [ + + ]: 12026 : || (constellation.relative_position.lateral_position == core::LateralRelativePosition::Overlap)
89 [ + + ]: 20 : || (constellation.relative_position.lateral_position == core::LateralRelativePosition::OverlapRight)))
90 : : {
91 : 12009 : lastStateBeforeDangerThresholdTimeToRemember.longitudinalSafe = true;
92 : 12009 : lastStateBeforeDangerThresholdTimeToRemember.lateralSafe = false;
93 [ + - + - ]: 24018 : core::getLogger()->info(
94 : : "RssNonIntersectionConstellationChecker[{}->{}]>> State is dangerous spontaneously and relative position "
95 : : "already overlaps laterally. Asuming longitudinal safe at danger threshold time. {} {}",
96 : 12009 : rssState.ego_id,
97 [ + - ]: 12009 : rssState.object_id,
98 : : rssState,
99 : : constellation);
100 : : }
101 : : }
102 : : }
103 : : else
104 : : {
105 : 52877 : lastStateBeforeDangerThresholdTimeToRemember.longitudinalSafe = isLongitudinalSafe(rssState);
106 : 52877 : lastStateBeforeDangerThresholdTimeToRemember.lateralSafe = isLateralSafe(rssState);
107 : : }
108 : :
109 [ + + ]: 66191 : if ((lastStateBeforeDangerThresholdTimeToRemember.lateralSafe)
110 [ + + ]: 1537 : && (lastStateBeforeDangerThresholdTimeToRemember.longitudinalSafe))
111 : : {
112 : : // Both longitudinal and lateral distances became dangerous at the same time
113 [ - + ]: 321 : if (isDangerous(rssState))
114 : : {
115 [ # # # # ]: 0 : core::getLogger()->info(
116 : : "RssNonIntersectionConstellationChecker[{}->{}]>> State is dangerous (t_b == t_b,lon == t_b,lat) {}",
117 : 0 : rssState.ego_id,
118 [ # # ]: 0 : rssState.object_id,
119 : : rssState);
120 : : }
121 : : }
122 [ + + ]: 65870 : else if (lastStateBeforeDangerThresholdTimeToRemember.lateralSafe)
123 : : {
124 : : // @todo: Handling of a cut-in by a leading vehicle as stated in definitions 11-13 of the RSS paper v6
125 : : // will be handled outside of this function. As a consequence.
126 : : // There is currently no response for a cut-in of a leading vehicle
127 : 1216 : rssState.longitudinal_state.response = state::LongitudinalResponse::None;
128 [ + + ]: 1216 : if (isDangerous(rssState))
129 : : {
130 [ + - + - ]: 396 : core::getLogger()->info("RssNonIntersectionConstellationChecker[{}->{}]>> State is dangerous (t_b == t_b,lat) No "
131 : : "longitudinal response: {}",
132 : 198 : rssState.ego_id,
133 [ + - ]: 198 : rssState.object_id,
134 : : rssState);
135 : : }
136 : : }
137 : 129308 : else if (lastStateBeforeDangerThresholdTimeToRemember.longitudinalSafe
138 [ + + + + : 64654 : && (getRightBorderObjectId() != rssState.object_id) && (getLeftBorderObjectId() != rssState.object_id))
+ - + + ]
139 : : {
140 : 64588 : rssState.lateral_state_left.response = state::LateralResponse::None;
141 : 64588 : rssState.lateral_state_right.response = state::LateralResponse::None;
142 [ + + ]: 64588 : if (isDangerous(rssState))
143 : : {
144 [ + - + - ]: 26102 : core::getLogger()->info(
145 : : "RssNonIntersectionConstellationChecker[{}->{}]>> State is dangerous (t_b == t_b,lon) No lateral response: {}",
146 : 13051 : rssState.ego_id,
147 [ + - ]: 13051 : rssState.object_id,
148 : : rssState);
149 : : }
150 : : }
151 : : else
152 : : {
153 : : // no non dangerous state available: the constellation appeared spontaneously being unsafe immediately
154 [ + + ]: 66 : if (isDangerous(rssState))
155 : : {
156 [ + - + - ]: 130 : core::getLogger()->info(
157 : : "RssNonIntersectionConstellationChecker[{}->{}]>> State is dangerous, no non dangerous state available {}",
158 : 65 : rssState.ego_id,
159 [ + - ]: 65 : rssState.object_id,
160 : : rssState);
161 : : }
162 : : }
163 : :
164 : : // store state for the next time step
165 : : auto const insertResult
166 [ + + ]: 66191 : = mNewStatesBeforeDangerThresholdTime.insert(RssSafeStateBeforeDangerThresholdTimeMap::value_type(
167 : 66191 : constellation.constellation_id, lastStateBeforeDangerThresholdTimeToRemember));
168 : :
169 [ + + ]: 66180 : if (!insertResult.second)
170 : : {
171 : 11976 : result = false;
172 [ + - + - ]: 23952 : core::getLogger()->error("RssNonIntersectionConstellationChecker[{}->{}]>> map insertion failed unexpectedly",
173 : 11976 : rssState.ego_id,
174 [ + - ]: 11976 : rssState.object_id);
175 : : }
176 : :
177 : 66180 : return result;
178 : : }
179 : :
180 : 65123 : bool RssNonIntersectionConstellationChecker::calculateRssStateSameDirection(
181 : : core::RelativeConstellation const &constellation, state::RssState &rssState)
182 : : {
183 : 65123 : bool result = calculateLongitudinalRssStateSameDirection(constellation, rssState.longitudinal_state);
184 [ + + ]: 65123 : if (result)
185 : : {
186 : 53122 : result = calculateLateralRssState(constellation, rssState.lateral_state_left, rssState.lateral_state_right);
187 : : }
188 : 65123 : return result;
189 : : }
190 : :
191 : 1068 : bool RssNonIntersectionConstellationChecker::calculateRssStateOppositeDirection(
192 : : core::RelativeConstellation const &constellation, state::RssState &rssState)
193 : : {
194 : 1068 : bool result = calculateLongitudinalRssStateOppositeDirection(constellation, rssState.longitudinal_state);
195 [ + + ]: 1068 : if (result)
196 : : {
197 : 1066 : result = calculateLateralRssState(constellation, rssState.lateral_state_left, rssState.lateral_state_right);
198 : : }
199 : 1068 : return result;
200 : : }
201 : :
202 : 65123 : bool RssNonIntersectionConstellationChecker::calculateLongitudinalRssStateSameDirection(
203 : : core::RelativeConstellation const &constellation, state::LongitudinalRssState &rssState)
204 : : {
205 : 65123 : bool result = false;
206 : :
207 : 65123 : rssState.response = state::LongitudinalResponse::BrakeMin;
208 : 65123 : rssState.rss_state_information.current_distance = constellation.relative_position.longitudinal_distance;
209 : :
210 : 65123 : bool is_safe = false;
211 : :
212 [ + + ]: 65123 : if ((core::LongitudinalRelativePosition::InFront == constellation.relative_position.longitudinal_position)
213 [ + + ]: 2083 : || (core::LongitudinalRelativePosition::OverlapFront == constellation.relative_position.longitudinal_position))
214 : : {
215 : 63140 : rssState.rss_state_information.evaluator = state::RssStateEvaluator::LongitudinalDistanceSameDirectionEgoFront;
216 : :
217 : : // The ego vehicle is leading in this constellation so we don't need to break longitudinal
218 : 63140 : rssState.response = state::LongitudinalResponse::None;
219 : :
220 : 63140 : result = checkSafeLongitudinalDistanceSameDirection(constellation.ego_state,
221 : 63140 : constellation.other_state,
222 : 63140 : constellation.relative_position.longitudinal_distance,
223 [ + - ]: 63140 : rssState.rss_state_information.safe_distance,
224 : : is_safe);
225 : : }
226 : : else
227 : : {
228 : 1983 : rssState.rss_state_information.evaluator = state::RssStateEvaluator::LongitudinalDistanceSameDirectionOtherInFront;
229 : :
230 : 1983 : result = checkSafeLongitudinalDistanceSameDirection(constellation.other_state,
231 : 1983 : constellation.ego_state,
232 : 1983 : constellation.relative_position.longitudinal_distance,
233 [ + - ]: 1983 : rssState.rss_state_information.safe_distance,
234 : : is_safe);
235 : : }
236 : :
237 : 65123 : rssState.is_safe = is_safe;
238 [ + + ]: 65123 : if (is_safe)
239 : : {
240 : 51470 : rssState.response = state::LongitudinalResponse::None;
241 : : }
242 : :
243 : 65123 : return result;
244 : : }
245 : :
246 : 1068 : bool RssNonIntersectionConstellationChecker::calculateLongitudinalRssStateOppositeDirection(
247 : : core::RelativeConstellation const &constellation, state::LongitudinalRssState &rssState)
248 : : {
249 : 1068 : bool result = false;
250 : :
251 : 1068 : bool is_safe = false;
252 : 1068 : rssState.response = state::LongitudinalResponse::BrakeMin;
253 : 1068 : rssState.rss_state_information.current_distance = constellation.relative_position.longitudinal_distance;
254 : :
255 [ + + ]: 1068 : if (constellation.ego_state.structured_object_state.is_in_correct_lane)
256 : : {
257 : : rssState.rss_state_information.evaluator
258 : 749 : = state::RssStateEvaluator::LongitudinalDistanceOppositeDirectionEgoCorrectLane;
259 : :
260 : 1498 : result = checkSafeLongitudinalDistanceOppositeDirection(constellation.ego_state,
261 : 749 : constellation.other_state,
262 : 749 : constellation.relative_position.longitudinal_distance,
263 [ + - ]: 749 : rssState.rss_state_information.safe_distance,
264 : : is_safe);
265 : 749 : rssState.response = state::LongitudinalResponse::BrakeMinCorrect;
266 : : }
267 : : else
268 : : {
269 : 319 : rssState.rss_state_information.evaluator = state::RssStateEvaluator::LongitudinalDistanceOppositeDirection;
270 : :
271 : 319 : result = checkSafeLongitudinalDistanceOppositeDirection(constellation.other_state,
272 : 319 : constellation.ego_state,
273 : 319 : constellation.relative_position.longitudinal_distance,
274 [ + - ]: 319 : rssState.rss_state_information.safe_distance,
275 : : is_safe);
276 : : }
277 : :
278 : 1068 : rssState.is_safe = is_safe;
279 [ + + ]: 1068 : if (rssState.is_safe)
280 : : {
281 : 389 : rssState.response = state::LongitudinalResponse::None;
282 : : }
283 : :
284 : 1068 : return result;
285 : : }
286 : :
287 : 54188 : bool RssNonIntersectionConstellationChecker::calculateLateralRssState(core::RelativeConstellation const &constellation,
288 : : state::LateralRssState &rssStateLeft,
289 : : state::LateralRssState &rssStateRight)
290 : : {
291 : 54188 : rssStateLeft.is_safe = false;
292 : 54188 : rssStateLeft.response = state::LateralResponse::BrakeMin;
293 : 54188 : rssStateRight.is_safe = false;
294 : 54188 : rssStateRight.response = state::LateralResponse::BrakeMin;
295 : :
296 : 54188 : bool isDistanceSafe = false;
297 : :
298 : 54188 : bool result = false;
299 : 108376 : if ((core::LateralRelativePosition::AtLeft == constellation.relative_position.lateral_position)
300 [ + + + + : 54188 : || (getRightBorderObjectId() == constellation.object_id))
+ + ]
301 : : {
302 : 906 : rssStateLeft.rss_state_information.evaluator = state::RssStateEvaluator::None;
303 : 906 : rssStateLeft.rss_state_information.current_distance = std::numeric_limits<physics::Distance>::max();
304 : 906 : rssStateLeft.rss_state_information.safe_distance = std::numeric_limits<physics::Distance>::max();
305 : :
306 : : // ego is the left vehicle, so right side has to be checked
307 : 906 : rssStateRight.rss_state_information.evaluator = state::RssStateEvaluator::LateralDistance;
308 : 906 : rssStateRight.rss_state_information.current_distance = constellation.relative_position.lateral_distance;
309 : 906 : result = checkSafeLateralDistance(constellation.ego_state,
310 : 906 : constellation.other_state,
311 : 906 : constellation.relative_position.lateral_distance,
312 [ + - ]: 906 : rssStateRight.rss_state_information.safe_distance,
313 : : isDistanceSafe);
314 : : }
315 : 106564 : else if ((core::LateralRelativePosition::AtRight == constellation.relative_position.lateral_position)
316 [ + + - + : 53282 : || (getLeftBorderObjectId() == constellation.object_id))
+ + ]
317 : : {
318 : 1086 : rssStateRight.rss_state_information.evaluator = state::RssStateEvaluator::None;
319 : 1086 : rssStateRight.rss_state_information.current_distance = std::numeric_limits<physics::Distance>::max();
320 : 1086 : rssStateRight.rss_state_information.safe_distance = std::numeric_limits<physics::Distance>::max();
321 : :
322 : : // ego is the right vehicle, so left side has to be checked
323 : 1086 : rssStateLeft.rss_state_information.evaluator = state::RssStateEvaluator::LateralDistance;
324 : 1086 : rssStateLeft.rss_state_information.current_distance = constellation.relative_position.lateral_distance;
325 : 1086 : result = checkSafeLateralDistance(constellation.other_state,
326 : 1086 : constellation.ego_state,
327 : 1086 : constellation.relative_position.lateral_distance,
328 [ + - ]: 1086 : rssStateLeft.rss_state_information.safe_distance,
329 : : isDistanceSafe);
330 : : }
331 : : else
332 : : {
333 : 52196 : rssStateLeft.rss_state_information.evaluator = state::RssStateEvaluator::LateralDistance;
334 : 52196 : rssStateLeft.rss_state_information.current_distance = physics::Distance(0);
335 : 52196 : rssStateLeft.rss_state_information.safe_distance = physics::Distance(0);
336 : 52196 : rssStateRight.rss_state_information.evaluator = state::RssStateEvaluator::LateralDistance;
337 : 52196 : rssStateRight.rss_state_information.current_distance = physics::Distance(0);
338 : 52196 : rssStateRight.rss_state_information.safe_distance = physics::Distance(0);
339 : :
340 : : // lateral distance is zero, never safe
341 : 52196 : result = true;
342 : : }
343 : :
344 [ + + ]: 54188 : if (isDistanceSafe)
345 : : {
346 : 1339 : rssStateLeft.is_safe = true;
347 : 1339 : rssStateLeft.response = state::LateralResponse::None;
348 : 1339 : rssStateRight.is_safe = true;
349 : 1339 : rssStateRight.response = state::LateralResponse::None;
350 : : }
351 : 105698 : else if ((core::LateralRelativePosition::AtLeft == constellation.relative_position.lateral_position)
352 [ + + ]: 52692 : || (core::LateralRelativePosition::OverlapLeft == constellation.relative_position.lateral_position)
353 [ + + + + : 105541 : || (getRightBorderObjectId() == constellation.object_id))
+ + ]
354 : : {
355 : : // ego is the left vehicle, so the collision is on the right side
356 : 284 : rssStateLeft.is_safe = true;
357 : 284 : rssStateLeft.response = state::LateralResponse::None;
358 : : }
359 : 105130 : else if ((core::LateralRelativePosition::AtRight == constellation.relative_position.lateral_position)
360 [ + + ]: 52070 : || (core::LateralRelativePosition::OverlapRight == constellation.relative_position.lateral_position)
361 [ + + - + : 104635 : || (getLeftBorderObjectId() == constellation.object_id))
+ + ]
362 : : {
363 : : // ego is the right vehicle, so the collision is on the left side
364 : 605 : rssStateRight.is_safe = true;
365 : 605 : rssStateRight.response = state::LateralResponse::None;
366 : : }
367 : :
368 : 54188 : return result;
369 : : }
370 : :
371 : : } // namespace structured
372 : : } // namespace rss
373 : : } // namespace ad
|
