Branch data Line data Source code
1 : : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
2 : : //
3 : : // Copyright (C) 2020-2021 Intel Corporation
4 : : //
5 : : // SPDX-License-Identifier: LGPL-2.1-only
6 : : //
7 : : // ----------------- END LICENSE BLOCK -----------------------------------
8 : :
9 : : #include "RssUnstructuredSceneChecker.hpp"
10 : : #include <cmath>
11 : : #include <limits>
12 : : #include "../unstructured/TrajectoryPedestrian.hpp"
13 : : #include "../unstructured/TrajectoryVehicle.hpp"
14 : : #include "ad/rss/situation/Physics.hpp"
15 : : #include "ad/rss/situation/RssFormulas.hpp"
16 : : #include "ad/rss/unstructured/Geometry.hpp"
17 : :
18 : : namespace ad {
19 : :
20 : : namespace rss {
21 : : namespace situation {
22 : :
23 : : using situation::calculateTimeToCoverDistance;
24 : :
25 : 20 : bool RssUnstructuredSceneChecker::calculateUnstructuredSceneStateInfo(
26 : : situation::VehicleState const &vehicleState, state::UnstructuredSceneStateInformation &stateInfo) const
27 : : {
28 : 20 : bool result = true;
29 [ + - ]: 40 : unstructured::Polygon brake;
30 [ + - ]: 20 : unstructured::Polygon continueForward;
31 : :
32 [ - + - - : 20 : switch (vehicleState.objectType)
- ]
33 : : {
34 : 0 : case world::ObjectType::Invalid:
35 : 0 : result = false;
36 : 0 : break;
37 : 20 : case world::ObjectType::EgoVehicle:
38 : : case world::ObjectType::OtherVehicle:
39 : : {
40 : 20 : unstructured::TrajectoryVehicle trajectoryVehicle;
41 [ + - ]: 20 : result = trajectoryVehicle.calculateTrajectorySets(vehicleState, brake, continueForward);
42 : 20 : break;
43 : : }
44 : 0 : case world::ObjectType::Pedestrian:
45 : : {
46 : 0 : unstructured::TrajectoryPedestrian trajectoryPedestrian;
47 [ # # ]: 0 : result = trajectoryPedestrian.calculateTrajectorySets(vehicleState, brake, continueForward);
48 : 0 : break;
49 : : }
50 : 0 : case world::ObjectType::ArtificialObject:
51 : 0 : result = false;
52 : 0 : break;
53 : 0 : default:
54 : 0 : result = false;
55 : : throw std::runtime_error(
56 [ # # ]: 0 : "RssUnstructuredSceneChecker::calculateUnstructuredSceneStateInfo>> invalid object type");
57 : : break;
58 : : }
59 : :
60 [ + - ]: 20 : if (result)
61 : : {
62 [ + - ]: 20 : unstructured::toTrajectorySet(brake, stateInfo.brakeTrajectorySet);
63 [ + - ]: 20 : unstructured::toTrajectorySet(continueForward, stateInfo.continueForwardTrajectorySet);
64 : : }
65 : 40 : return result;
66 : : }
67 : :
68 : 10 : bool RssUnstructuredSceneChecker::calculateRssStateUnstructured(world::TimeIndex const &timeIndex,
69 : : Situation const &situation,
70 : : state::UnstructuredSceneStateInformation &egoStateInfo,
71 : : state::RssState &rssState)
72 : : {
73 : 10 : bool result = true;
74 : :
75 [ + - ]: 10 : if (timeIndex != mCurrentTimeIndex)
76 : : {
77 : 10 : mOtherMustBrakeStatesBeforeDangerThresholdTime.swap(mNewOtherMustBrakeStatesBeforeDangerThresholdTime);
78 : 10 : mNewOtherMustBrakeStatesBeforeDangerThresholdTime.clear();
79 : 10 : mCurrentTimeIndex = timeIndex;
80 : :
81 : : // ego state only has to be calculated once per time step
82 : 10 : result = calculateUnstructuredSceneStateInfo(situation.egoVehicleState, egoStateInfo);
83 : : }
84 : :
85 [ + - ]: 10 : if (result)
86 : : {
87 : 10 : result = calculateUnstructuredSceneStateInfo(situation.otherVehicleState,
88 : 10 : rssState.unstructuredSceneState.rssStateInformation);
89 : : }
90 : :
91 [ + - ]: 10 : if (result)
92 : : {
93 : 10 : result = calculateState(
94 : 10 : situation, egoStateInfo, rssState.unstructuredSceneState.rssStateInformation, rssState.unstructuredSceneState);
95 : : }
96 : :
97 [ + - ]: 10 : if (result)
98 : : {
99 [ + - ]: 10 : auto foundDriveAwayStateIt = mDriveAwayStateMap.find(situation.situationId);
100 : :
101 [ + + ]: 10 : if (foundDriveAwayStateIt != mDriveAwayStateMap.end())
102 : : {
103 : : auto steeringAngle = normalizeAngleSigned(situation.egoVehicleState.objectState.yaw
104 [ + - + - ]: 3 : + situation.egoVehicleState.objectState.steeringAngle);
105 : 3 : bool keepDriveAwayState = true;
106 : :
107 [ - + ]: 3 : if (rssState.unstructuredSceneState.isSafe)
108 : : {
109 [ # # ]: 0 : spdlog::trace("Situation {} Remove previous drive-away state as situation became safe again.",
110 : 0 : situation.situationId);
111 : 0 : keepDriveAwayState = false;
112 : : }
113 [ + - + + ]: 3 : else if (situation.otherVehicleState.objectState.centerPoint != foundDriveAwayStateIt->second.otherPosition)
114 : : {
115 [ + - ]: 1 : spdlog::trace("Situation {} Remove previous drive-away state as other object has moved.",
116 : 1 : situation.situationId);
117 : 1 : keepDriveAwayState = false;
118 : : }
119 [ + - + + ]: 2 : else if (!unstructured::isInsideHeadingRange(steeringAngle, foundDriveAwayStateIt->second.allowedHeadingRange))
120 : : {
121 [ + - ]: 1 : spdlog::trace("Situation {} Remove previous drive-away state as steering angle {} is not within range {}.",
122 : 1 : situation.situationId,
123 : : steeringAngle,
124 : 1 : foundDriveAwayStateIt->second.allowedHeadingRange);
125 : 1 : keepDriveAwayState = false;
126 : : }
127 : :
128 [ + + ]: 3 : if (keepDriveAwayState)
129 : : {
130 [ + - ]: 1 : spdlog::debug("Situation {} ego vehicle driving away with previously allowed heading {}.",
131 : 1 : situation.situationId,
132 : 1 : foundDriveAwayStateIt->second.allowedHeadingRange);
133 : 1 : rssState.unstructuredSceneState.response = state::UnstructuredSceneResponse::DriveAway;
134 : 1 : rssState.unstructuredSceneState.isSafe = false;
135 : 1 : rssState.unstructuredSceneState.headingRange = foundDriveAwayStateIt->second.allowedHeadingRange;
136 : : }
137 : : else
138 : : {
139 [ + - ]: 2 : mDriveAwayStateMap.erase(foundDriveAwayStateIt);
140 : : }
141 : : }
142 : : else
143 : : {
144 [ + + ]: 7 : if (rssState.unstructuredSceneState.response == state::UnstructuredSceneResponse::DriveAway)
145 : : {
146 [ + - ]: 3 : spdlog::debug("Situation {} store drive-away state with allowed heading range {}.",
147 : 3 : situation.situationId,
148 : 3 : rssState.unstructuredSceneState.headingRange);
149 [ + - ]: 3 : DriveAwayState driveAwayState;
150 : 3 : driveAwayState.allowedHeadingRange = rssState.unstructuredSceneState.headingRange;
151 : 3 : driveAwayState.otherPosition = situation.otherVehicleState.objectState.centerPoint;
152 [ + - ]: 3 : mDriveAwayStateMap[situation.situationId] = driveAwayState;
153 : : }
154 : : }
155 : : }
156 : 10 : return result;
157 : : }
158 : :
159 : 15 : bool RssUnstructuredSceneChecker::calculateState(Situation const &situation,
160 : : state::UnstructuredSceneStateInformation const &egoStateInfo,
161 : : state::UnstructuredSceneStateInformation const &otherStateInfo,
162 : : state::UnstructuredSceneRssState &rssState)
163 : : {
164 : 15 : bool result = true;
165 : 15 : auto const &egoBrake = egoStateInfo.brakeTrajectorySet;
166 : 15 : auto const &egoContinueForward = egoStateInfo.continueForwardTrajectorySet;
167 : 15 : auto const &otherBrake = otherStateInfo.brakeTrajectorySet;
168 : 15 : auto const &otherContinueForward = otherStateInfo.continueForwardTrajectorySet;
169 : :
170 [ + + + - : 15 : if (egoBrake.empty() || egoContinueForward.empty() || otherBrake.empty() || otherContinueForward.empty())
+ - - + +
+ ]
171 : : {
172 [ + - ]: 2 : spdlog::warn("Situation {} refers to empty trajectory sets", situation.situationId);
173 : 1 : return false;
174 : : }
175 : :
176 : : // check if distance is safe
177 [ + - ]: 14 : bool egoBrakeOtherContinueForwardOverlap = unstructured::collides(egoBrake, otherContinueForward);
178 [ + - ]: 14 : bool otherBrakeEgoContinueForwardOverlap = unstructured::collides(otherBrake, egoContinueForward);
179 [ + - ]: 14 : bool egoBrakeOtherBrakeOverlap = unstructured::collides(egoBrake, otherBrake);
180 : :
181 [ + + + + ]: 14 : bool isSafeEgoMustBrake = !egoBrakeOtherContinueForwardOverlap && otherBrakeEgoContinueForwardOverlap;
182 [ + + + + ]: 14 : bool isSafeOtherMustBrake = !otherBrakeEgoContinueForwardOverlap && egoBrakeOtherContinueForwardOverlap;
183 : 14 : bool isSafeBrakeBoth = !egoBrakeOtherBrakeOverlap;
184 : :
185 [ + + + + : 14 : auto isSafe = isSafeEgoMustBrake || isSafeOtherMustBrake || isSafeBrakeBoth;
+ + ]
186 : :
187 [ + - ]: 14 : spdlog::trace("Situation {} safe check: isSafeEgoMustBrake: {}, isSafeOtherMustBrake: {}, isSafeBrakeBoth: {}",
188 : 14 : situation.situationId,
189 : : isSafeEgoMustBrake,
190 : : isSafeOtherMustBrake,
191 : : isSafeBrakeBoth);
192 : 14 : DrivingMode mode{DrivingMode::Invalid};
193 : :
194 [ + + ]: 14 : if (isSafe)
195 : : {
196 [ + - ]: 5 : spdlog::debug("Situation {} safe. Store value otherMustBrake: {}",
197 : 5 : situation.situationId,
198 [ + + ]: 5 : isSafeOtherMustBrake ? "true" : "false");
199 [ + - ]: 5 : mNewOtherMustBrakeStatesBeforeDangerThresholdTime[situation.situationId] = isSafeOtherMustBrake;
200 : 5 : mode = DrivingMode::ContinueForward;
201 : : }
202 : : else
203 : : {
204 : : // not safe
205 : 9 : bool lastOtherMustBrake = false;
206 [ + - ]: 9 : auto foundStateIt = mOtherMustBrakeStatesBeforeDangerThresholdTime.find(situation.situationId);
207 [ + + ]: 9 : if (foundStateIt != mOtherMustBrakeStatesBeforeDangerThresholdTime.end())
208 : : {
209 : 7 : lastOtherMustBrake = foundStateIt->second;
210 [ + - ]: 7 : mNewOtherMustBrakeStatesBeforeDangerThresholdTime[situation.situationId] = lastOtherMustBrake;
211 : : }
212 : :
213 : : // Rule 1: If both cars were already at a full stop, then one can drive away from other vehicle
214 [ + - ]: 9 : if (((situation.egoVehicleState.objectState.speed == physics::Speed(0.))
215 [ + + + - : 9 : && (situation.otherVehicleState.objectState.speed == physics::Speed(0.))))
+ + + + ]
216 : : {
217 [ + - ]: 6 : spdlog::debug("Situation {} Rule 1: both stopped, unsafe distance -> drive away.", situation.situationId);
218 : 3 : mode = DrivingMode::DriveAway;
219 : : }
220 : :
221 : : // Rule 2: If:
222 : : // - ego-brake and other-continue-forward not overlap
223 : : // and
224 : : // - other-brake and ego-continue-forward overlap
225 [ + + + + ]: 9 : if ((mode == DrivingMode::Invalid) && lastOtherMustBrake)
226 : : {
227 [ + - + + ]: 2 : if (situation.egoVehicleState.objectState.speed > physics::Speed(0.))
228 : : {
229 [ + - ]: 2 : spdlog::debug("Situation {} Rule 2: opponent is moving -> continue forward", situation.situationId);
230 : 1 : mode = DrivingMode::ContinueForward;
231 : : }
232 : : else
233 : : {
234 [ + - ]: 2 : spdlog::debug("Situation {} Rule 2: opponent is stopped -> drive away", situation.situationId);
235 : 1 : mode = DrivingMode::DriveAway;
236 : : }
237 : : }
238 : :
239 : : // Rule 3: Brake
240 [ + + ]: 9 : if (mode == DrivingMode::Invalid)
241 : : {
242 [ + - ]: 8 : spdlog::debug("Situation {} Rule 3: brake (brake collides with other brake)", situation.situationId);
243 : 4 : mode = DrivingMode::Brake;
244 : : }
245 : : }
246 : :
247 [ + + + - : 14 : switch (mode)
- ]
248 : : {
249 : 6 : case DrivingMode::ContinueForward:
250 : 6 : rssState.response = state::UnstructuredSceneResponse::ContinueForward;
251 : 6 : rssState.isSafe = true;
252 : 6 : break;
253 : 4 : case DrivingMode::Brake:
254 : 4 : rssState.response = state::UnstructuredSceneResponse::Brake;
255 : 4 : rssState.isSafe = false;
256 : 4 : break;
257 : 4 : case DrivingMode::DriveAway:
258 : : {
259 [ + - + - ]: 4 : calculateDriveAwayAngle(unstructured::toPoint(situation.egoVehicleState.objectState.centerPoint),
260 : 0 : unstructured::toPoint(situation.otherVehicleState.objectState.centerPoint),
261 : 4 : situation.egoVehicleState.dynamics.unstructuredSettings.driveAwayMaxAngle,
262 [ + - ]: 4 : rssState.headingRange);
263 : 4 : rssState.response = state::UnstructuredSceneResponse::DriveAway;
264 : 4 : rssState.isSafe = false;
265 : 4 : break;
266 : : }
267 : 0 : case DrivingMode::Invalid:
268 : 0 : result = false;
269 : 0 : break;
270 : 0 : default:
271 : 0 : result = false;
272 [ # # ]: 0 : throw std::runtime_error("RssUnstructuredSceneChecker::calculateState>> invalid driving mode");
273 : : break;
274 : : }
275 : :
276 : 14 : return result;
277 : : }
278 : :
279 : 12 : bool RssUnstructuredSceneChecker::calculateDriveAwayAngle(unstructured::Point const &egoVehicleLocation,
280 : : unstructured::Point const &otherVehicleLocation,
281 : : physics::Angle const &maxAllowedAngleWhenBothStopped,
282 : : state::HeadingRange &range) const
283 : : {
284 [ + - ]: 12 : auto const substractedLocationVector = egoVehicleLocation - otherVehicleLocation;
285 : :
286 : : // get vector angle
287 : : auto const substractedLocationVectorAngle = physics::Angle(
288 [ + - + - ]: 12 : std::atan2(static_cast<double>(substractedLocationVector.y()), static_cast<double>(substractedLocationVector.x())));
289 : :
290 [ + - + - ]: 12 : range.begin = physics::normalizeAngleSigned(substractedLocationVectorAngle - maxAllowedAngleWhenBothStopped);
291 [ + - + - ]: 12 : range.end = physics::normalizeAngleSigned(substractedLocationVectorAngle + maxAllowedAngleWhenBothStopped);
292 : 12 : return true;
293 : : }
294 : :
295 : : } // namespace situation
296 : : } // namespace rss
297 : : } // namespace ad
|
