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/core/RssSituationExtraction.hpp"
10 : : #include <algorithm>
11 : : #include "../world/RssSituationCoordinateSystemConversion.hpp"
12 : : #include "../world/RssSituationIdProvider.hpp"
13 : : #include "ad/rss/world/WorldModelValidInputRange.hpp"
14 : : #include "spdlog/fmt/ostr.h"
15 : : #include "spdlog/spdlog.h"
16 : :
17 : : namespace ad {
18 : : namespace rss {
19 : : namespace core {
20 : :
21 : : using physics::Distance;
22 : : using physics::MetricRange;
23 : :
24 : 728 : RssSituationExtraction::RssSituationExtraction()
25 : : {
26 : : try
27 : : {
28 [ + + ]: 728 : mSituationIdProvider = std::unique_ptr<world::RssSituationIdProvider>(new world::RssSituationIdProvider());
29 : : }
30 [ + - ]: 26 : catch (...)
31 : : {
32 [ + - ]: 13 : spdlog::critical("RssSituationExtraction object initialization failed");
33 : 13 : mSituationIdProvider = nullptr;
34 : : }
35 : 728 : }
36 : :
37 : 728 : RssSituationExtraction::~RssSituationExtraction()
38 : : {
39 : 728 : }
40 : :
41 : 3981 : void RssSituationExtraction::calcluateRelativeLongitudinalPosition(
42 : : MetricRange const &egoMetricRange,
43 : : MetricRange const &otherMetricRange,
44 : : situation::LongitudinalRelativePosition &longitudinalPosition,
45 : : Distance &longitudinalDistance)
46 : : {
47 [ + + ]: 3981 : if (egoMetricRange.minimum > otherMetricRange.maximum)
48 : : {
49 : 714 : longitudinalPosition = situation::LongitudinalRelativePosition::InFront;
50 : 714 : longitudinalDistance = egoMetricRange.minimum - otherMetricRange.maximum;
51 : : }
52 [ + + ]: 3267 : else if (otherMetricRange.minimum > egoMetricRange.maximum)
53 : : {
54 : 2104 : longitudinalPosition = situation::LongitudinalRelativePosition::AtBack;
55 : 2104 : longitudinalDistance = otherMetricRange.minimum - egoMetricRange.maximum;
56 : : }
57 : : else
58 : : {
59 : 1163 : longitudinalDistance = Distance(0.);
60 [ + + + + : 1163 : if ((egoMetricRange.minimum > otherMetricRange.minimum) && (egoMetricRange.maximum > otherMetricRange.maximum))
+ + ]
61 : : {
62 : 105 : longitudinalPosition = situation::LongitudinalRelativePosition::OverlapFront;
63 : : }
64 [ + + + + : 1058 : else if ((egoMetricRange.minimum < otherMetricRange.minimum) && (egoMetricRange.maximum < otherMetricRange.maximum))
+ + ]
65 : : {
66 : 19 : longitudinalPosition = situation::LongitudinalRelativePosition::OverlapBack;
67 : : }
68 : : else
69 : : {
70 : 1039 : longitudinalPosition = situation::LongitudinalRelativePosition::Overlap;
71 : : }
72 : : }
73 : 3834 : }
74 : :
75 : 799 : void RssSituationExtraction::calcluateRelativeLongitudinalPositionIntersection(
76 : : MetricRange const &egoMetricRange,
77 : : MetricRange const &otherMetricRange,
78 : : situation::LongitudinalRelativePosition &longitudinalPosition,
79 : : Distance &longitudinalDistance)
80 : : {
81 [ + + ]: 799 : if (egoMetricRange.maximum < otherMetricRange.minimum)
82 : : {
83 : 109 : longitudinalPosition = situation::LongitudinalRelativePosition::InFront;
84 : 109 : longitudinalDistance = otherMetricRange.minimum - egoMetricRange.maximum;
85 : : }
86 [ + + ]: 690 : else if (otherMetricRange.maximum < egoMetricRange.minimum)
87 : : {
88 : 542 : longitudinalPosition = situation::LongitudinalRelativePosition::AtBack;
89 : 542 : longitudinalDistance = egoMetricRange.minimum - otherMetricRange.maximum;
90 : : }
91 : : else
92 : : {
93 : 148 : longitudinalDistance = Distance(0.);
94 [ + + + + : 148 : if ((egoMetricRange.minimum < otherMetricRange.minimum) && (egoMetricRange.maximum < otherMetricRange.maximum))
+ + ]
95 : : {
96 : 52 : longitudinalPosition = situation::LongitudinalRelativePosition::OverlapFront;
97 : : }
98 [ + + + - : 96 : else if ((egoMetricRange.minimum > otherMetricRange.minimum) && (egoMetricRange.maximum > otherMetricRange.maximum))
+ + ]
99 : : {
100 : 48 : longitudinalPosition = situation::LongitudinalRelativePosition::OverlapBack;
101 : : }
102 : : else
103 : : {
104 : 48 : longitudinalPosition = situation::LongitudinalRelativePosition::Overlap;
105 : : }
106 : : }
107 : 799 : }
108 : :
109 : 3834 : void RssSituationExtraction::calcluateRelativeLateralPosition(MetricRange const &egoMetricRange,
110 : : MetricRange const &otherMetricRange,
111 : : situation::LateralRelativePosition &lateralPosition,
112 : : Distance &lateralDistance)
113 : : {
114 [ + + ]: 3834 : if (egoMetricRange.minimum > otherMetricRange.maximum)
115 : : {
116 : 1150 : lateralPosition = situation::LateralRelativePosition::AtRight;
117 : 1150 : lateralDistance = egoMetricRange.minimum - otherMetricRange.maximum;
118 : : }
119 [ + + ]: 2684 : else if (otherMetricRange.minimum > egoMetricRange.maximum)
120 : : {
121 : 935 : lateralPosition = situation::LateralRelativePosition::AtLeft;
122 : 935 : lateralDistance = otherMetricRange.minimum - egoMetricRange.maximum;
123 : : }
124 : : else
125 : : {
126 : 1749 : lateralDistance = Distance(0.);
127 [ + + + + : 1749 : if ((egoMetricRange.minimum > otherMetricRange.minimum) && (egoMetricRange.maximum > otherMetricRange.maximum))
+ + ]
128 : : {
129 : 105 : lateralPosition = situation::LateralRelativePosition::OverlapRight;
130 : : }
131 [ + + + + : 1644 : else if ((egoMetricRange.minimum < otherMetricRange.minimum) && (egoMetricRange.maximum < otherMetricRange.maximum))
+ + ]
132 : : {
133 : 106 : lateralPosition = situation::LateralRelativePosition::OverlapLeft;
134 : : }
135 : : else
136 : : {
137 : 1538 : lateralPosition = situation::LateralRelativePosition::Overlap;
138 : : }
139 : : }
140 : 3834 : }
141 : :
142 : 3962 : bool RssSituationExtraction::convertObjectsNonIntersection(world::Scene const ¤tScene,
143 : : situation::Situation &situation)
144 : : {
145 [ + + ]: 3962 : if (!currentScene.intersectingRoad.empty())
146 : : {
147 [ + - ]: 1 : spdlog::error("RssSituationExtraction::convertObjectsNonIntersection>> Intersecting road not empty {}",
148 : : currentScene);
149 : 1 : return false;
150 : : }
151 : :
152 : 3961 : bool result = true;
153 : :
154 [ + - ]: 3961 : world::ObjectDimensions egoVehicleDimension;
155 [ + - ]: 3961 : world::ObjectDimensions objectToBeCheckedDimension;
156 [ + - ]: 3961 : result = calculateObjectDimensions(currentScene, egoVehicleDimension, objectToBeCheckedDimension);
157 : :
158 : : situation::LongitudinalRelativePosition longitudinalPosition;
159 : 3961 : Distance longitudinalDistance;
160 [ + + ]: 3961 : calcluateRelativeLongitudinalPosition(egoVehicleDimension.longitudinalDimensions,
161 : : objectToBeCheckedDimension.longitudinalDimensions,
162 : : longitudinalPosition,
163 : : longitudinalDistance);
164 : :
165 : 3814 : situation.relativePosition.longitudinalPosition = longitudinalPosition;
166 : 3814 : situation.relativePosition.longitudinalDistance = longitudinalDistance;
167 : :
168 : 3814 : situation.egoVehicleState.isInCorrectLane = !egoVehicleDimension.onNegativeLane;
169 : :
170 [ + + ]: 3814 : if (currentScene.situationType == ::ad::rss::situation::SituationType::OppositeDirection)
171 : : {
172 : 1071 : situation.otherVehicleState.isInCorrectLane = !objectToBeCheckedDimension.onPositiveLane;
173 : : }
174 : : else
175 : : {
176 : 2743 : situation.otherVehicleState.isInCorrectLane = !objectToBeCheckedDimension.onNegativeLane;
177 : : }
178 : :
179 : : /**
180 : : * Set lateral restrictions
181 : : */
182 [ + - ]: 3814 : if (result)
183 : : {
184 : : situation::LateralRelativePosition lateralPosition;
185 : 3814 : Distance lateralDistance;
186 [ + - ]: 3814 : calcluateRelativeLateralPosition(egoVehicleDimension.lateralDimensions,
187 : : objectToBeCheckedDimension.lateralDimensions,
188 : : lateralPosition,
189 : : lateralDistance);
190 : :
191 : 3814 : situation.relativePosition.lateralPosition = lateralPosition;
192 : 3814 : situation.relativePosition.lateralDistance = lateralDistance;
193 : : }
194 : 3814 : return result;
195 : : }
196 : :
197 : 1599 : void RssSituationExtraction::convertToIntersectionCentric(MetricRange const &objectDimension,
198 : : MetricRange const &intersectionPosition,
199 : : MetricRange &dimensionsIntersection)
200 : : {
201 : 1599 : dimensionsIntersection.maximum = intersectionPosition.minimum - objectDimension.minimum;
202 : 1599 : dimensionsIntersection.minimum = intersectionPosition.minimum - objectDimension.maximum;
203 : 1598 : }
204 : :
205 : 843 : bool RssSituationExtraction::convertObjectsIntersection(world::Scene const ¤tScene,
206 : : situation::Situation &situation)
207 : : {
208 [ + - ]: 843 : world::ObjectDimensions egoVehicleDimension;
209 [ + - ]: 843 : world::ObjectDimensions objectDimension;
210 : :
211 [ + - ]: 843 : bool result = calculateObjectDimensions(currentScene.egoVehicle, currentScene.egoVehicleRoad, egoVehicleDimension);
212 : :
213 [ + + + - : 843 : result = result && calculateObjectDimensions(currentScene.object, currentScene.intersectingRoad, objectDimension);
+ + ]
214 : :
215 [ + + ]: 843 : if (result)
216 : : {
217 : 800 : situation.egoVehicleState.isInCorrectLane = !egoVehicleDimension.onNegativeLane;
218 : 800 : situation.otherVehicleState.isInCorrectLane = !objectDimension.onNegativeLane;
219 : :
220 : : // For intersection the lanes don't have the same origin so the positions cannot be directly compared
221 : : // Intersection entry should be the common point so convert the positions to this reference point
222 [ + - ]: 800 : MetricRange egoDimensionsIntersection;
223 [ + + ]: 800 : convertToIntersectionCentric(
224 : : egoVehicleDimension.longitudinalDimensions, egoVehicleDimension.intersectionPosition, egoDimensionsIntersection);
225 : :
226 [ + - ]: 799 : MetricRange objectDimensionsIntersection;
227 : :
228 [ + - ]: 799 : convertToIntersectionCentric(
229 : : objectDimension.longitudinalDimensions, objectDimension.intersectionPosition, objectDimensionsIntersection);
230 : :
231 [ + - ]: 799 : situation.egoVehicleState.distanceToEnterIntersection = std::max(Distance(0.), egoDimensionsIntersection.minimum);
232 : : situation.egoVehicleState.distanceToLeaveIntersection
233 [ + - ]: 799 : = egoVehicleDimension.intersectionPosition.maximum - egoVehicleDimension.longitudinalDimensions.minimum;
234 : :
235 : : situation.otherVehicleState.distanceToEnterIntersection
236 [ + - ]: 799 : = std::max(Distance(0.), objectDimensionsIntersection.minimum);
237 : : situation.otherVehicleState.distanceToLeaveIntersection
238 [ + - ]: 799 : = objectDimension.intersectionPosition.maximum - objectDimension.longitudinalDimensions.minimum;
239 : :
240 : : situation::LongitudinalRelativePosition longitudinalPosition;
241 : 799 : Distance longitudinalDistance;
242 [ + - ]: 799 : calcluateRelativeLongitudinalPositionIntersection(
243 : : egoDimensionsIntersection, objectDimensionsIntersection, longitudinalPosition, longitudinalDistance);
244 : :
245 : 799 : situation.relativePosition.longitudinalPosition = longitudinalPosition;
246 : 799 : situation.relativePosition.longitudinalDistance = longitudinalDistance;
247 : :
248 : 799 : situation.relativePosition.lateralPosition = situation::LateralRelativePosition::Overlap;
249 : 799 : situation.relativePosition.lateralDistance = Distance(0.);
250 : : }
251 : :
252 [ + + ]: 842 : if (currentScene.situationType == situation::SituationType::IntersectionEgoHasPriority)
253 : : {
254 : 408 : situation.egoVehicleState.hasPriority = true;
255 : 408 : situation.otherVehicleState.hasPriority = false;
256 : : }
257 [ + + ]: 434 : else if (currentScene.situationType == situation::SituationType::IntersectionObjectHasPriority)
258 : : {
259 : 217 : situation.egoVehicleState.hasPriority = false;
260 : 217 : situation.otherVehicleState.hasPriority = true;
261 : : }
262 [ + - ]: 217 : else if (currentScene.situationType == situation::SituationType::IntersectionSamePriority)
263 : : {
264 : 217 : situation.egoVehicleState.hasPriority = false;
265 : 217 : situation.otherVehicleState.hasPriority = false;
266 : : }
267 : : else
268 : : {
269 : : // This function should never be called if we are not in intersection situation
270 [ # # ]: 0 : spdlog::error("RssSituationExtraction::convertObjectsIntersection>> Unexpected situationType {}", currentScene);
271 : : result = false; // LCOV_EXCL_LINE: unreachable code, keep to be on the safe side
272 : : }
273 : :
274 : 842 : return result;
275 : : }
276 : :
277 : 5021 : bool RssSituationExtraction::extractSituationInputRangeChecked(world::TimeIndex const &timeIndex,
278 : : world::Scene const ¤tScene,
279 : : situation::Situation &situation)
280 : : {
281 : : // ensure the object types are semantically correct
282 : : // @toDo: add this restriction to the data type model
283 : : // and extend generated withinValidInputRange by these
284 [ + + ]: 5021 : if (((currentScene.object.objectType != world::ObjectType::OtherVehicle)
285 [ + + ]: 6 : && (currentScene.object.objectType != world::ObjectType::ArtificialObject)
286 [ + + ]: 4 : && (currentScene.object.objectType != world::ObjectType::Pedestrian))
287 [ + + ]: 5018 : || (currentScene.egoVehicle.objectType != world::ObjectType::EgoVehicle))
288 : : {
289 [ + - ]: 8 : spdlog::error("RssSituationExtraction::extractSituationInputRangeChecked>> Invalid object type. Ego: {} Object: {}",
290 : 8 : currentScene.egoVehicle,
291 : 8 : currentScene.object);
292 : 8 : return false;
293 : : }
294 [ + + ]: 5013 : if (currentScene.object.objectId == currentScene.egoVehicle.objectId)
295 : : {
296 [ + - ]: 1 : spdlog::error("RssSituationExtraction::extractSituationInputRangeChecked>> Object and ego vehicle must not have "
297 : : "the same id. Ego: {} Object: {}",
298 : 1 : currentScene.egoVehicle,
299 : 1 : currentScene.object);
300 : 1 : return false;
301 : : }
302 [ + + ]: 5012 : if (!static_cast<bool>(mSituationIdProvider))
303 : : {
304 : 15 : return false;
305 : : }
306 : :
307 : 4997 : bool result = false;
308 : :
309 : : try
310 : : {
311 [ + + ]: 4997 : situation.situationId = mSituationIdProvider->getSituationId(timeIndex, currentScene);
312 : 4952 : situation.objectId = currentScene.object.objectId;
313 : 4952 : situation.situationType = currentScene.situationType;
314 : :
315 : 4952 : situation.egoVehicleState.objectType = currentScene.egoVehicle.objectType;
316 : 4952 : situation.otherVehicleState.objectType = currentScene.object.objectType;
317 : :
318 : 4952 : situation.egoVehicleState.objectState = currentScene.egoVehicle.state;
319 : 4952 : situation.otherVehicleState.objectState = currentScene.object.state;
320 : :
321 : 4952 : situation.egoVehicleState.hasPriority = false;
322 : 4952 : situation.otherVehicleState.hasPriority = false;
323 : :
324 : 4952 : situation.egoVehicleState.isInCorrectLane = true;
325 : 4952 : situation.otherVehicleState.isInCorrectLane = true;
326 : :
327 : 4952 : situation.egoVehicleState.distanceToEnterIntersection = Distance(0.);
328 : 4952 : situation.egoVehicleState.distanceToLeaveIntersection = Distance(1000.);
329 : :
330 : 4952 : situation.otherVehicleState.distanceToEnterIntersection = Distance(0.);
331 : 4952 : situation.otherVehicleState.distanceToLeaveIntersection = Distance(1000.);
332 : :
333 [ + - ]: 4952 : convertVehicleStateDynamics(currentScene.egoVehicle, currentScene.egoVehicleRssDynamics, situation.egoVehicleState);
334 [ + - ]: 4952 : convertVehicleStateDynamics(currentScene.object, currentScene.objectRssDynamics, situation.otherVehicleState);
335 : :
336 [ + + + + ]: 4952 : switch (currentScene.situationType)
337 : : {
338 : 3962 : case ad::rss::situation::SituationType::SameDirection:
339 : : case ad::rss::situation::SituationType::OppositeDirection:
340 : : {
341 [ + + ]: 3962 : result = convertObjectsNonIntersection(currentScene, situation);
342 : :
343 : 3815 : break;
344 : : }
345 : 843 : case ad::rss::situation::SituationType::IntersectionEgoHasPriority:
346 : : case ad::rss::situation::SituationType::IntersectionObjectHasPriority:
347 : : case ad::rss::situation::SituationType::IntersectionSamePriority:
348 : : {
349 [ + + ]: 843 : result = convertObjectsIntersection(currentScene, situation);
350 : 842 : break;
351 : : }
352 : 146 : case ad::rss::situation::SituationType::NotRelevant:
353 : : case ad::rss::situation::SituationType::Unstructured:
354 : : {
355 : 146 : result = true;
356 : 146 : break;
357 : : }
358 : 1 : default:
359 : : {
360 [ + - ]: 1 : spdlog::error("RssSituationExtraction::extractSituationInputRangeChecked>> Invalid situation type {}",
361 : : currentScene);
362 : 1 : result = false;
363 : 1 : break;
364 : : }
365 : : }
366 : : }
367 : 193 : catch (std::exception &e)
368 : : {
369 [ + - ]: 193 : spdlog::critical("RssSituationExtraction::extractSituationInputRangeChecked>> Exception caught '{}' {} {}",
370 : 193 : e.what(),
371 : : timeIndex,
372 : : currentScene);
373 : 193 : result = false;
374 : : }
375 : 0 : catch (...)
376 : : {
377 [ - - ]: 0 : spdlog::critical(
378 : : "RssSituationExtraction::extractSituationInputRangeChecked>> Exception caught {} {}", timeIndex, currentScene);
379 : 0 : result = false;
380 : : }
381 : :
382 : 4997 : return result;
383 : : }
384 : :
385 : 54 : bool RssSituationExtraction::mergeVehicleStates(MergeMode const &mergeMode,
386 : : situation::VehicleState const &otherVehicleState,
387 : : situation::VehicleState &mergedVehicleState)
388 : : {
389 : : // on vehicle states there are only differences in intersection distances allowed due to different road definitions
390 : 54 : if ((otherVehicleState.dynamics.alphaLat.accelMax != mergedVehicleState.dynamics.alphaLat.accelMax)
391 [ + + ]: 53 : || (otherVehicleState.dynamics.alphaLat.brakeMin != mergedVehicleState.dynamics.alphaLat.brakeMin)
392 [ + + ]: 52 : || (otherVehicleState.dynamics.alphaLon.accelMax != mergedVehicleState.dynamics.alphaLon.accelMax)
393 [ + + ]: 51 : || (otherVehicleState.dynamics.alphaLon.brakeMinCorrect != mergedVehicleState.dynamics.alphaLon.brakeMinCorrect)
394 [ + + ]: 50 : || (otherVehicleState.dynamics.alphaLon.brakeMin != mergedVehicleState.dynamics.alphaLon.brakeMin)
395 [ + + ]: 49 : || (otherVehicleState.dynamics.alphaLon.brakeMax != mergedVehicleState.dynamics.alphaLon.brakeMax)
396 [ + + ]: 48 : || (otherVehicleState.dynamics.lateralFluctuationMargin != mergedVehicleState.dynamics.lateralFluctuationMargin)
397 [ + + ]: 47 : || (otherVehicleState.dynamics.responseTime != mergedVehicleState.dynamics.responseTime)
398 [ + + - + : 107 : || (otherVehicleState.hasPriority != mergedVehicleState.hasPriority))
+ + ]
399 : : {
400 : 8 : return false;
401 : : }
402 : : // consider the worst cases
403 [ + + ]: 46 : if (mergeMode == MergeMode::EgoVehicle)
404 : : {
405 : : // worst case for ego vehicle is not driving in correct lane
406 [ + - + + ]: 27 : mergedVehicleState.isInCorrectLane = mergedVehicleState.isInCorrectLane && otherVehicleState.isInCorrectLane;
407 : : }
408 : : else
409 : : {
410 : : // worst case for other vehicle is driving in correct lane
411 [ - + - - ]: 19 : mergedVehicleState.isInCorrectLane = mergedVehicleState.isInCorrectLane || otherVehicleState.isInCorrectLane;
412 : : }
413 : :
414 : : mergedVehicleState.distanceToEnterIntersection
415 : 46 : = std::min(mergedVehicleState.distanceToEnterIntersection, otherVehicleState.distanceToEnterIntersection);
416 : : mergedVehicleState.distanceToLeaveIntersection
417 : 46 : = std::max(mergedVehicleState.distanceToLeaveIntersection, otherVehicleState.distanceToLeaveIntersection);
418 : : mergedVehicleState.velocity.speedLon.minimum
419 : 46 : = std::min(mergedVehicleState.velocity.speedLon.minimum, otherVehicleState.velocity.speedLon.minimum);
420 : : mergedVehicleState.velocity.speedLon.maximum
421 : 46 : = std::max(mergedVehicleState.velocity.speedLon.maximum, otherVehicleState.velocity.speedLon.maximum);
422 : : mergedVehicleState.velocity.speedLat.minimum
423 : 46 : = std::min(mergedVehicleState.velocity.speedLat.minimum, otherVehicleState.velocity.speedLat.minimum);
424 : : mergedVehicleState.velocity.speedLat.maximum
425 : 46 : = std::max(mergedVehicleState.velocity.speedLat.maximum, otherVehicleState.velocity.speedLat.maximum);
426 : :
427 : 46 : return true;
428 : : }
429 : :
430 : 27 : bool RssSituationExtraction::mergeSituations(situation::Situation const &otherSituation,
431 : : situation::Situation &mergedSituation)
432 : : {
433 : 27 : if ( // basic data has to match
434 [ + - ]: 27 : (otherSituation.situationId != mergedSituation.situationId) || (otherSituation.objectId != mergedSituation.objectId)
435 [ + - ]: 27 : || (otherSituation.situationType != mergedSituation.situationType)
436 [ + - + - ]: 27 : || !mergeVehicleStates(MergeMode::EgoVehicle, otherSituation.egoVehicleState, mergedSituation.egoVehicleState)
437 [ + - + + ]: 54 : || !mergeVehicleStates(
438 [ + - + + ]: 54 : MergeMode::OtherVehicle, otherSituation.otherVehicleState, mergedSituation.otherVehicleState))
439 : : {
440 : 8 : return false;
441 : : }
442 : :
443 : : // worst case
444 : 19 : mergedSituation.relativePosition.longitudinalDistance = std::min(
445 : 19 : mergedSituation.relativePosition.longitudinalDistance, otherSituation.relativePosition.longitudinalDistance);
446 [ + + ]: 19 : if (otherSituation.relativePosition.longitudinalPosition != mergedSituation.relativePosition.longitudinalPosition)
447 : : {
448 [ + + ]: 8 : if ((mergedSituation.relativePosition.longitudinalPosition == situation::LongitudinalRelativePosition::Overlap)
449 [ + + ]: 7 : || (otherSituation.relativePosition.longitudinalPosition == situation::LongitudinalRelativePosition::Overlap)
450 [ + + ]: 6 : || ((mergedSituation.relativePosition.longitudinalPosition < situation::LongitudinalRelativePosition::Overlap)
451 [ + + ]: 3 : && (otherSituation.relativePosition.longitudinalPosition
452 : : > situation::LongitudinalRelativePosition::Overlap))
453 [ + + ]: 5 : || ((mergedSituation.relativePosition.longitudinalPosition > situation::LongitudinalRelativePosition::Overlap)
454 [ + + ]: 3 : && (otherSituation.relativePosition.longitudinalPosition
455 : : < situation::LongitudinalRelativePosition::Overlap)))
456 : : {
457 : : // overlap is also worst case for contradicting input, ensure the longitudinal distance becomes 0.
458 : 4 : mergedSituation.relativePosition.longitudinalPosition = situation::LongitudinalRelativePosition::Overlap;
459 : 4 : mergedSituation.relativePosition.longitudinalDistance = Distance(0.);
460 : : }
461 [ + + ]: 4 : else if ((mergedSituation.relativePosition.longitudinalPosition
462 : : == situation::LongitudinalRelativePosition::OverlapFront)
463 [ + + ]: 3 : || (otherSituation.relativePosition.longitudinalPosition
464 : : == situation::LongitudinalRelativePosition::OverlapFront))
465 : : {
466 : : // one of the both is overlap font (the other then only can be InFront in here)
467 : 2 : mergedSituation.relativePosition.longitudinalPosition = situation::LongitudinalRelativePosition::OverlapFront;
468 : : }
469 [ + + ]: 2 : else if ((mergedSituation.relativePosition.longitudinalPosition
470 : : == situation::LongitudinalRelativePosition::OverlapBack)
471 [ + - ]: 1 : || (otherSituation.relativePosition.longitudinalPosition
472 : : == situation::LongitudinalRelativePosition::OverlapBack))
473 : : {
474 : : // one of the both is overlap back (the other then only can be AtBack in here)
475 : 2 : mergedSituation.relativePosition.longitudinalPosition = situation::LongitudinalRelativePosition::OverlapBack;
476 : : }
477 : : else
478 : : {
479 : : // LCOV_EXCL_START: unreachable code, keep to be on the safe side
480 : : // this is impossible to reach, set to overlap having longitudinal distance to 0. to be on the safe side
481 : : mergedSituation.relativePosition.longitudinalPosition = situation::LongitudinalRelativePosition::Overlap;
482 : : mergedSituation.relativePosition.longitudinalDistance = Distance(0.);
483 : : // LCOV_EXCL_STOP: unreachable code, keep to be on the safe side
484 : : }
485 : : }
486 : :
487 : : mergedSituation.relativePosition.lateralDistance
488 : 19 : = std::min(mergedSituation.relativePosition.lateralDistance, otherSituation.relativePosition.lateralDistance);
489 [ + + ]: 19 : if (otherSituation.relativePosition.lateralPosition != mergedSituation.relativePosition.lateralPosition)
490 : : {
491 [ + + ]: 8 : if ((mergedSituation.relativePosition.lateralPosition == situation::LateralRelativePosition::Overlap)
492 [ + + ]: 7 : || (otherSituation.relativePosition.lateralPosition == situation::LateralRelativePosition::Overlap)
493 [ + + ]: 6 : || ((mergedSituation.relativePosition.lateralPosition < situation::LateralRelativePosition::Overlap)
494 [ + + ]: 3 : && (otherSituation.relativePosition.lateralPosition > situation::LateralRelativePosition::Overlap))
495 [ + + ]: 5 : || ((mergedSituation.relativePosition.lateralPosition > situation::LateralRelativePosition::Overlap)
496 [ + + ]: 3 : && (otherSituation.relativePosition.lateralPosition < situation::LateralRelativePosition::Overlap)))
497 : : {
498 : : // overlap is also worst case for contradicting input, ensure the lateral distance becomes 0.
499 : 4 : mergedSituation.relativePosition.lateralPosition = situation::LateralRelativePosition::Overlap;
500 : 4 : mergedSituation.relativePosition.lateralDistance = Distance(0.);
501 : : }
502 [ + + ]: 4 : else if ((mergedSituation.relativePosition.lateralPosition == situation::LateralRelativePosition::OverlapLeft)
503 [ + + ]: 3 : || (otherSituation.relativePosition.lateralPosition == situation::LateralRelativePosition::OverlapLeft))
504 : : {
505 : : // one of the both is overlap left (the other then only can be AtLeft in here)
506 : 2 : mergedSituation.relativePosition.lateralPosition = situation::LateralRelativePosition::OverlapLeft;
507 : : }
508 [ + + ]: 2 : else if ((mergedSituation.relativePosition.lateralPosition == situation::LateralRelativePosition::OverlapRight)
509 [ + - ]: 1 : || (otherSituation.relativePosition.lateralPosition == situation::LateralRelativePosition::OverlapRight))
510 : : {
511 : : // one of the both is overlap right (the other then only can be AtRight in here)
512 : 2 : mergedSituation.relativePosition.lateralPosition = situation::LateralRelativePosition::OverlapRight;
513 : : }
514 : : else
515 : : {
516 : : // LCOV_EXCL_START: unreachable code, keep to be on the safe side
517 : : // this is impossible to reach, set to overlap having lateral distance to 0. to be on the safe side
518 : : mergedSituation.relativePosition.lateralPosition = situation::LateralRelativePosition::Overlap;
519 : : mergedSituation.relativePosition.lateralDistance = Distance(0.);
520 : : // LCOV_EXCL_STOP: unreachable code, keep to be on the safe side
521 : : }
522 : : }
523 : :
524 : 19 : return true;
525 : : }
526 : :
527 : 4263 : bool RssSituationExtraction::extractSituations(world::WorldModel const &worldModel,
528 : : situation::SituationSnapshot &situationSnapshot)
529 : : {
530 [ + + ]: 4263 : if (!withinValidInputRange(worldModel))
531 : : {
532 [ + - ]: 27 : spdlog::error("RssSituationExtraction::extractSituation>> Invalid input {}", worldModel);
533 : 27 : return false;
534 : : }
535 : :
536 : 4236 : bool result = true;
537 : : try
538 : : {
539 : 4236 : situationSnapshot.timeIndex = worldModel.timeIndex;
540 : 4236 : situationSnapshot.defaultEgoVehicleRssDynamics = worldModel.defaultEgoVehicleRssDynamics;
541 : 4236 : situationSnapshot.situations.clear();
542 [ + + ]: 9235 : for (auto const &scene : worldModel.scenes)
543 : : {
544 [ + - ]: 5014 : situation::Situation situation;
545 [ + - ]: 5014 : bool const extractResult = extractSituationInputRangeChecked(worldModel.timeIndex, scene, situation);
546 [ + + ]: 5014 : if (extractResult)
547 : : {
548 : : // situation id creation might detect that different scenes are representing identical situations
549 : : // ensure the situationSnapshot is unique while containing the worst-case situation
550 : : auto findResult = std::find_if(situationSnapshot.situations.begin(),
551 : : situationSnapshot.situations.end(),
552 : 5572 : [&situation](ad::rss::situation::Situation const &checkSituation) {
553 : 5572 : return checkSituation.situationId == situation.situationId;
554 [ + - ]: 4757 : });
555 [ + + ]: 4757 : if (findResult == situationSnapshot.situations.end())
556 : : {
557 [ + + ]: 4730 : situationSnapshot.situations.push_back(situation);
558 : : }
559 [ + - + + ]: 27 : else if (!mergeSituations(situation, *findResult))
560 : : {
561 : 8 : result = false;
562 : : }
563 : : }
564 : : else
565 : : {
566 [ + - ]: 257 : spdlog::error("RssSituationExtraction::extractSituations>> Extraction failed {}", scene);
567 : 257 : result = false;
568 : : }
569 : : }
570 : : }
571 : 15 : catch (std::exception &e)
572 : : {
573 [ + - ]: 30 : spdlog::critical("RssSituationExtraction::extractSituations>> Exception caught '{}' {}", e.what(), worldModel);
574 : 15 : result = false;
575 : : }
576 : 0 : catch (...)
577 : : {
578 [ - - ]: 0 : spdlog::critical("RssSituationExtraction::extractSituations>> Exception caught {}", worldModel);
579 : 0 : result = false;
580 : : }
581 : 4236 : return result;
582 : : }
583 : :
584 : : } // namespace core
585 : : } // namespace rss
586 : : } // namespace ad
|
