Branch data Line data Source code
1 : : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
2 : : //
3 : : // Copyright (C) 2020-2022 Intel Corporation
4 : : //
5 : : // SPDX-License-Identifier: LGPL-2.1-only
6 : : //
7 : : // ----------------- END LICENSE BLOCK -----------------------------------
8 : :
9 : : #include "ad/rss/unstructured/RssUnstructuredConstellationChecker.hpp"
10 : : #include <cmath>
11 : : #include <limits>
12 : : #include "ad/rss/core/Logging.hpp"
13 : : #include "ad/rss/core/Physics.hpp"
14 : : #include "ad/rss/unstructured/TrajectoryPedestrian.hpp"
15 : : #include "ad/rss/unstructured/TrajectoryVehicle.hpp"
16 : :
17 : : namespace ad {
18 : : namespace rss {
19 : : namespace unstructured {
20 : :
21 : : const ad::physics::Speed c_drive_away_stopped_max_speed(0.1);
22 : :
23 : 787 : RssUnstructuredConstellationChecker::RssUnstructuredConstellationChecker()
24 : : {
25 : : CalculateTrajectorySetsCallbackFunctionType trajectoryVehicleCallback
26 : 20 : = [](::ad::rss::core::RelativeConstellation const &constellation,
27 : : ::ad::rss::world::ObjectId const &objectId,
28 : : core::RelativeObjectState const &vehicleState,
29 : : ::ad::geometry::Polygon &brakePolygon,
30 : : ::ad::geometry::Polygon &continueForwardPolygon) -> bool {
31 : : (void)constellation;
32 : : (void)objectId;
33 : 20 : unstructured::TrajectoryVehicle trajectoryVehicle;
34 [ + - ]: 20 : bool result = trajectoryVehicle.calculateTrajectorySets(vehicleState, brakePolygon, continueForwardPolygon);
35 : 20 : return result;
36 : 787 : };
37 : : CalculateTrajectorySetsCallbackFunctionType trajectoryPedestrianCallback
38 : 0 : = [](::ad::rss::core::RelativeConstellation const &constellation,
39 : : ::ad::rss::world::ObjectId const &objectId,
40 : : core::RelativeObjectState const &vehicleState,
41 : : ::ad::geometry::Polygon &brakePolygon,
42 : : ::ad::geometry::Polygon &continueForwardPolygon) -> bool {
43 : : (void)constellation;
44 : : (void)objectId;
45 : 0 : unstructured::TrajectoryPedestrian trajectoryPedestrian;
46 [ # # ]: 0 : bool result = trajectoryPedestrian.calculateTrajectorySets(vehicleState, brakePolygon, continueForwardPolygon);
47 : 0 : return result;
48 : 787 : };
49 [ + - + - ]: 787 : mCalculateTrajectorySetsCallbackMap[world::ObjectType::EgoVehicle] = trajectoryVehicleCallback;
50 [ + - + - ]: 787 : mCalculateTrajectorySetsCallbackMap[world::ObjectType::Bicycle] = trajectoryVehicleCallback;
51 [ + - + - ]: 787 : mCalculateTrajectorySetsCallbackMap[world::ObjectType::OtherObject] = trajectoryVehicleCallback;
52 [ + - + - ]: 787 : mCalculateTrajectorySetsCallbackMap[world::ObjectType::OtherVehicle] = trajectoryVehicleCallback;
53 [ + - + - ]: 787 : mCalculateTrajectorySetsCallbackMap[world::ObjectType::ArtificialVehicle] = trajectoryVehicleCallback;
54 : :
55 [ + - + - ]: 787 : mCalculateTrajectorySetsCallbackMap[world::ObjectType::Pedestrian] = trajectoryPedestrianCallback;
56 [ + - + - ]: 787 : mCalculateTrajectorySetsCallbackMap[world::ObjectType::ArtificialPedestrian] = trajectoryPedestrianCallback;
57 : 787 : }
58 : :
59 : 3 : RssUnstructuredConstellationChecker::RssUnstructuredConstellationChecker(
60 : 3 : RssUnstructuredConstellationChecker const &other)
61 : 3 : : mOtherMustBrakeStatesBeforeDangerThresholdTime(other.mOtherMustBrakeStatesBeforeDangerThresholdTime)
62 [ + - ]: 3 : , mNewOtherMustBrakeStatesBeforeDangerThresholdTime(other.mNewOtherMustBrakeStatesBeforeDangerThresholdTime)
63 : 3 : , mCurrentTimeIndex(other.mCurrentTimeIndex)
64 [ + - ]: 3 : , mDriveAwayStateMap(other.mDriveAwayStateMap)
65 : : {
66 [ + - ]: 3 : core::shared_lock_guard const shared_lock(other.mCallbackRwLock);
67 [ + - ]: 3 : mCalculateTrajectorySetsCallbackMap = other.mCalculateTrajectorySetsCallbackMap;
68 : 3 : }
69 : :
70 : : RssUnstructuredConstellationChecker &
71 : 0 : RssUnstructuredConstellationChecker::operator=(RssUnstructuredConstellationChecker const &other)
72 : : {
73 [ # # ]: 0 : core::shared_lock_guard const shared_lock(other.mCallbackRwLock);
74 : :
75 [ # # ]: 0 : mOtherMustBrakeStatesBeforeDangerThresholdTime = other.mOtherMustBrakeStatesBeforeDangerThresholdTime;
76 [ # # ]: 0 : mNewOtherMustBrakeStatesBeforeDangerThresholdTime = other.mNewOtherMustBrakeStatesBeforeDangerThresholdTime;
77 : 0 : mCurrentTimeIndex = other.mCurrentTimeIndex;
78 [ # # ]: 0 : mDriveAwayStateMap = other.mDriveAwayStateMap;
79 [ # # ]: 0 : mCalculateTrajectorySetsCallbackMap = other.mCalculateTrajectorySetsCallbackMap;
80 : 0 : return *this;
81 : 0 : }
82 : :
83 : 24 : bool RssUnstructuredConstellationChecker::calculateUnstructuredConstellationStateInfo(
84 : : ::ad::rss::core::RelativeConstellation const &constellation,
85 : : ::ad::rss::world::ObjectId const &objectId,
86 : : core::RelativeObjectState const &vehicleState,
87 : : state::UnstructuredConstellationStateInformation &stateInfo) const
88 : : {
89 : 24 : bool result = true;
90 [ + - ]: 24 : ::ad::geometry::Polygon brake;
91 [ + - ]: 24 : ::ad::geometry::Polygon continueForward;
92 : :
93 [ + - ]: 24 : core::shared_lock_guard const shared_lock(mCallbackRwLock);
94 [ + - ]: 24 : auto callbackIter = mCalculateTrajectorySetsCallbackMap.find(vehicleState.object_type);
95 [ + - ]: 24 : if (callbackIter != mCalculateTrajectorySetsCallbackMap.end())
96 : : {
97 [ + - ]: 24 : result = callbackIter->second(constellation, objectId, vehicleState, brake, continueForward);
98 : : }
99 : : else
100 : : {
101 : 0 : result = false;
102 [ # # # # ]: 0 : core::getLogger()->error("RssUnstructuredConstellationChecker::calculateUnstructuredConstellationStateInfo>> "
103 : : "object type {} not yet supported for unstructured calculation.",
104 [ # # ]: 0 : std::to_string(vehicleState.object_type));
105 : : }
106 : :
107 [ + - ]: 24 : if (result)
108 : : {
109 [ + - ]: 24 : ::ad::geometry::toTrajectorySet(brake, stateInfo.brake_trajectory_set);
110 [ + - ]: 24 : ::ad::geometry::toTrajectorySet(continueForward, stateInfo.continue_forward_trajectory_set);
111 : : stateInfo.considered_drive_away_steering_angle
112 [ + - ]: 24 : = normalizeAngleSigned(constellation.ego_state.unstructured_object_state.yaw
113 [ + - ]: 48 : + constellation.ego_state.unstructured_object_state.steering_angle);
114 : : }
115 : 24 : return result;
116 : 24 : }
117 : :
118 : 12 : bool RssUnstructuredConstellationChecker::calculateRssStateUnstructured(
119 : : world::TimeIndex const &time_index,
120 : : core::RelativeConstellation const &constellation,
121 : : state::UnstructuredConstellationStateInformation &egoStateInfo,
122 : : state::RssState &rssState)
123 : : {
124 : 12 : bool result = true;
125 : :
126 [ + - ]: 12 : if (time_index != mCurrentTimeIndex)
127 : : {
128 : 12 : mOtherMustBrakeStatesBeforeDangerThresholdTime.swap(mNewOtherMustBrakeStatesBeforeDangerThresholdTime);
129 : 12 : mNewOtherMustBrakeStatesBeforeDangerThresholdTime.clear();
130 : 12 : mCurrentTimeIndex = time_index;
131 : :
132 : : // ego state only has to be calculated once per time step
133 : 12 : result = calculateUnstructuredConstellationStateInfo(
134 : 12 : constellation, constellation.ego_id, constellation.ego_state, egoStateInfo);
135 : : }
136 : :
137 [ + - ]: 12 : if (result)
138 : : {
139 : : result
140 : 12 : = calculateUnstructuredConstellationStateInfo(constellation,
141 : 12 : constellation.object_id,
142 : 12 : constellation.other_state,
143 : 12 : rssState.unstructured_constellation_state.rss_state_information);
144 : : }
145 : :
146 [ + - ]: 12 : if (result)
147 : : {
148 : 12 : result = calculateState(constellation,
149 : : egoStateInfo,
150 : 12 : rssState.unstructured_constellation_state.rss_state_information,
151 : 12 : rssState.unstructured_constellation_state);
152 : : }
153 : :
154 [ + - ]: 12 : if (result)
155 : : {
156 [ + + ]: 12 : if (rssState.unstructured_constellation_state.response == state::UnstructuredConstellationResponse::DriveAway)
157 : : {
158 [ + - + - ]: 6 : core::getLogger()->debug(
159 : : "Unstructured Constellation[{}->{}:{}] store drive-away state with allowed heading range {}.",
160 : 3 : constellation.ego_id,
161 : 3 : constellation.object_id,
162 : 3 : constellation.constellation_id,
163 [ + - ]: 3 : rssState.unstructured_constellation_state.heading_range);
164 [ + - ]: 3 : DriveAwayState driveAwayState;
165 : 3 : driveAwayState.allowedHeadingRange = rssState.unstructured_constellation_state.heading_range;
166 : 3 : driveAwayState.otherPosition = constellation.other_state.unstructured_object_state.center_point;
167 [ + - ]: 3 : mDriveAwayStateMap[constellation.constellation_id] = driveAwayState;
168 : : }
169 : : else
170 : : {
171 [ + - ]: 9 : auto foundDriveAwayStateIt = mDriveAwayStateMap.find(constellation.constellation_id);
172 : :
173 [ + + ]: 9 : if (foundDriveAwayStateIt != mDriveAwayStateMap.end())
174 : : {
175 : 3 : bool keepDriveAwayState = true;
176 : :
177 [ - + ]: 3 : if (rssState.unstructured_constellation_state.is_safe)
178 : : {
179 [ # # # # ]: 0 : core::getLogger()->trace("Unstructured Constellation[{}->{}:{}] Remove previous drive-away state as "
180 : : "constellation became safe again.",
181 : 0 : constellation.ego_id,
182 : 0 : constellation.object_id,
183 [ # # ]: 0 : constellation.constellation_id);
184 : 0 : keepDriveAwayState = false;
185 : : }
186 : 3 : else if (constellation.other_state.unstructured_object_state.center_point
187 [ + - + + ]: 3 : != foundDriveAwayStateIt->second.otherPosition)
188 : : {
189 [ + - + - ]: 2 : core::getLogger()->trace(
190 : : "Unstructured Constellation[{}->{}:{}] Remove previous drive-away state as other object has moved.",
191 : 1 : constellation.ego_id,
192 : 1 : constellation.object_id,
193 [ + - ]: 1 : constellation.constellation_id);
194 : 1 : keepDriveAwayState = false;
195 : : }
196 [ + + ]: 2 : else if (!::ad::geometry::isInsideHeadingRange(
197 : 2 : rssState.unstructured_constellation_state.rss_state_information.considered_drive_away_steering_angle,
198 [ + - ]: 2 : foundDriveAwayStateIt->second.allowedHeadingRange))
199 : : {
200 [ + - + - ]: 2 : core::getLogger()->trace(
201 : : "Unstructured Constellation[{}->{}:{}] Remove previous drive-away state as steering "
202 : : "angle {} is not within range {}.",
203 : 1 : constellation.ego_id,
204 : 1 : constellation.object_id,
205 : 1 : constellation.constellation_id,
206 : 1 : rssState.unstructured_constellation_state.rss_state_information.considered_drive_away_steering_angle,
207 [ + - ]: 1 : foundDriveAwayStateIt->second.allowedHeadingRange);
208 : 1 : keepDriveAwayState = false;
209 : : }
210 : :
211 [ + + ]: 3 : if (keepDriveAwayState)
212 : : {
213 [ + - + - ]: 2 : core::getLogger()->debug(
214 : : "Unstructured Constellation[{}->{}:{}] ego vehicle driving away with previously allowed heading {}.",
215 : 1 : constellation.ego_id,
216 : 1 : constellation.object_id,
217 : 1 : constellation.constellation_id,
218 [ + - ]: 1 : foundDriveAwayStateIt->second.allowedHeadingRange);
219 : 1 : rssState.unstructured_constellation_state.response = state::UnstructuredConstellationResponse::DriveAway;
220 : 1 : rssState.unstructured_constellation_state.is_safe = false;
221 : 1 : rssState.unstructured_constellation_state.heading_range = foundDriveAwayStateIt->second.allowedHeadingRange;
222 : : }
223 : : else
224 : : {
225 [ + - ]: 2 : mDriveAwayStateMap.erase(foundDriveAwayStateIt);
226 : : }
227 : : }
228 : : }
229 : : }
230 : 12 : return result;
231 : : }
232 : :
233 : 17 : bool RssUnstructuredConstellationChecker::calculateState(
234 : : core::RelativeConstellation const &constellation,
235 : : state::UnstructuredConstellationStateInformation const &egoStateInfo,
236 : : state::UnstructuredConstellationStateInformation const &otherStateInfo,
237 : : state::UnstructuredConstellationRssState &rssState)
238 : : {
239 : 17 : bool result = true;
240 : 17 : auto const &egoBrake = egoStateInfo.brake_trajectory_set;
241 : 17 : auto const &egoContinueForward = egoStateInfo.continue_forward_trajectory_set;
242 : 17 : auto const &otherBrake = otherStateInfo.brake_trajectory_set;
243 : 17 : auto const &otherContinueForward = otherStateInfo.continue_forward_trajectory_set;
244 : :
245 [ + + + - : 17 : if (egoBrake.empty() || egoContinueForward.empty() || otherBrake.empty() || otherContinueForward.empty())
+ - - + +
+ ]
246 : : {
247 [ + - + - ]: 2 : core::getLogger()->warn("Unstructured Constellation[{}->{}:{}] refers to empty trajectory sets",
248 : 1 : constellation.ego_id,
249 : 1 : constellation.object_id,
250 [ + - ]: 1 : constellation.constellation_id);
251 : 1 : return false;
252 : : }
253 : :
254 : : // check if distance is safe
255 [ + - ]: 16 : bool egoBrakeOtherContinueForwardOverlap = ::ad::geometry::collides(egoBrake, otherContinueForward);
256 [ + - ]: 16 : bool otherBrakeEgoContinueForwardOverlap = ::ad::geometry::collides(otherBrake, egoContinueForward);
257 [ + - ]: 16 : bool egoBrakeOtherBrakeOverlap = ::ad::geometry::collides(egoBrake, otherBrake);
258 : :
259 [ + + + + ]: 16 : bool isSafeEgoMustBrake = !egoBrakeOtherContinueForwardOverlap && otherBrakeEgoContinueForwardOverlap;
260 [ + + + + ]: 16 : bool isSafeOtherMustBrake = !otherBrakeEgoContinueForwardOverlap && egoBrakeOtherContinueForwardOverlap;
261 : 16 : bool isSafeBrakeBoth = !egoBrakeOtherBrakeOverlap;
262 : :
263 [ + + + + : 16 : auto is_safe = isSafeEgoMustBrake || isSafeOtherMustBrake || isSafeBrakeBoth;
+ + ]
264 : :
265 [ + - + - ]: 32 : core::getLogger()->trace("Unstructured Constellation[{}->{}:{}] safe check: isSafeEgoMustBrake: {}, "
266 : : "isSafeOtherMustBrake: {}, isSafeBrakeBoth: {}",
267 : 16 : constellation.ego_id,
268 : 16 : constellation.object_id,
269 [ + - ]: 16 : constellation.constellation_id,
270 : : isSafeEgoMustBrake,
271 : : isSafeOtherMustBrake,
272 : : isSafeBrakeBoth);
273 : 16 : DrivingMode mode{DrivingMode::Invalid};
274 : :
275 [ + + ]: 16 : if (is_safe)
276 : : {
277 [ + - + - ]: 10 : core::getLogger()->debug("Unstructured Constellation[{}->{}:{}] safe. Store value otherMustBrake: {}",
278 : 5 : constellation.ego_id,
279 : 5 : constellation.object_id,
280 : 5 : constellation.constellation_id,
281 [ + + + - ]: 10 : isSafeOtherMustBrake ? "true" : "false");
282 [ + - ]: 5 : mNewOtherMustBrakeStatesBeforeDangerThresholdTime[constellation.constellation_id] = isSafeOtherMustBrake;
283 : 5 : mode = DrivingMode::ContinueForward;
284 : : }
285 : : else
286 : : {
287 : : // not safe
288 : 11 : bool lastOtherMustBrake = false;
289 [ + - ]: 11 : auto foundStateIt = mOtherMustBrakeStatesBeforeDangerThresholdTime.find(constellation.constellation_id);
290 [ + + ]: 11 : if (foundStateIt != mOtherMustBrakeStatesBeforeDangerThresholdTime.end())
291 : : {
292 : 7 : lastOtherMustBrake = foundStateIt->second;
293 [ + - ]: 7 : mNewOtherMustBrakeStatesBeforeDangerThresholdTime[constellation.constellation_id] = lastOtherMustBrake;
294 : : }
295 : :
296 : : // Rule 1: If both cars were already at a full stop, then one can drive away from other vehicle
297 [ + - ]: 11 : if (((constellation.ego_state.unstructured_object_state.speed_range.maximum <= c_drive_away_stopped_max_speed)
298 [ + + + + ]: 15 : && (constellation.other_state.unstructured_object_state.speed_range.maximum
299 [ + - + + ]: 4 : <= c_drive_away_stopped_max_speed)))
300 : : {
301 [ + - + - ]: 6 : core::getLogger()->debug(
302 : : "Unstructured Constellation[{}->{}:{}] Rule 1: both stopped, unsafe distance -> drive away.",
303 : 3 : constellation.ego_id,
304 : 3 : constellation.object_id,
305 [ + - ]: 3 : constellation.constellation_id);
306 : 3 : mode = DrivingMode::DriveAway;
307 : : }
308 : :
309 : : // Rule 2: If:
310 : : // - ego-brake and other-continue-forward not overlap
311 : : // and
312 : : // - other-brake and ego-continue-forward overlap
313 [ + + + + ]: 11 : if ((mode == DrivingMode::Invalid) && lastOtherMustBrake)
314 : : {
315 [ + - + + ]: 2 : if (constellation.ego_state.unstructured_object_state.speed_range.maximum > c_drive_away_stopped_max_speed)
316 : : {
317 [ + - + - ]: 2 : core::getLogger()->debug("Unstructured Constellation[{}->{}:{}] Rule 2: opponent is moving -> continue forward",
318 : 1 : constellation.ego_id,
319 : 1 : constellation.object_id,
320 [ + - ]: 1 : constellation.constellation_id);
321 : 1 : mode = DrivingMode::ContinueForward;
322 : : }
323 : : else
324 : : {
325 [ + - + - ]: 2 : core::getLogger()->debug("Unstructured Constellation[{}->{}:{}] Rule 2: opponent is stopped -> drive away",
326 : 1 : constellation.ego_id,
327 : 1 : constellation.object_id,
328 [ + - ]: 1 : constellation.constellation_id);
329 : 1 : mode = DrivingMode::DriveAway;
330 : : }
331 : : }
332 : :
333 : : // Rule 3: Brake
334 [ + + ]: 11 : if (mode == DrivingMode::Invalid)
335 : : {
336 [ + - + - ]: 12 : core::getLogger()->debug("Unstructured Constellation[{}->{}:{}] Rule 3: brake (brake collides with other brake)",
337 : 6 : constellation.ego_id,
338 : 6 : constellation.object_id,
339 [ + - ]: 6 : constellation.constellation_id);
340 : 6 : mode = DrivingMode::Brake;
341 : : }
342 : : }
343 : :
344 [ + + + - : 16 : switch (mode)
- ]
345 : : {
346 : 6 : case DrivingMode::ContinueForward:
347 : 6 : rssState.response = state::UnstructuredConstellationResponse::ContinueForward;
348 : 6 : rssState.is_safe = true;
349 : 6 : break;
350 : 6 : case DrivingMode::Brake:
351 : 6 : rssState.response = state::UnstructuredConstellationResponse::Brake;
352 : 6 : rssState.is_safe = false;
353 : 6 : break;
354 : 4 : case DrivingMode::DriveAway:
355 : : {
356 [ + - + - ]: 4 : calculateDriveAwayAngle(::ad::geometry::toPoint(constellation.ego_state.unstructured_object_state.center_point),
357 : 0 : ::ad::geometry::toPoint(constellation.other_state.unstructured_object_state.center_point),
358 : 4 : constellation.ego_state.dynamics.unstructured_settings.drive_away_max_angle,
359 [ + - ]: 4 : rssState.heading_range);
360 : 4 : rssState.response = state::UnstructuredConstellationResponse::DriveAway;
361 : 4 : rssState.is_safe = false;
362 : 4 : break;
363 : : }
364 : 0 : case DrivingMode::Invalid:
365 : 0 : result = false;
366 : 0 : break;
367 : 0 : default:
368 : 0 : result = false;
369 [ # # ]: 0 : throw std::runtime_error("RssUnstructuredConstellationChecker::calculateState>> invalid driving mode");
370 : : break;
371 : : }
372 : :
373 : 16 : return result;
374 : : }
375 : :
376 : 12 : bool RssUnstructuredConstellationChecker::calculateDriveAwayAngle(::ad::geometry::Point const &egoVehicleLocation,
377 : : ::ad::geometry::Point const &otherVehicleLocation,
378 : : physics::Angle const &maxAllowedAngleWhenBothStopped,
379 : : ::ad::geometry::HeadingRange &range) const
380 : : {
381 [ + - ]: 12 : auto const substractedLocationVector = egoVehicleLocation - otherVehicleLocation;
382 : :
383 : : // get vector angle
384 : : auto const substractedLocationVectorAngle = physics::Angle(
385 [ + - + - ]: 12 : std::atan2(static_cast<double>(substractedLocationVector.y()), static_cast<double>(substractedLocationVector.x())));
386 : :
387 [ + - + - ]: 12 : range.begin = physics::normalizeAngleSigned(substractedLocationVectorAngle - maxAllowedAngleWhenBothStopped);
388 [ + - + - ]: 12 : range.end = physics::normalizeAngleSigned(substractedLocationVectorAngle + maxAllowedAngleWhenBothStopped);
389 : 12 : return true;
390 : : }
391 : :
392 : : } // namespace unstructured
393 : : } // namespace rss
394 : : } // namespace ad
|
