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