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 "ad/rss/core/Logging.hpp"
12 : : #include "ad/rss/structured/RssConstellationIdProvider.hpp"
13 : : #include "ad/rss/structured/RssLaneCoordinateSystemConversion.hpp"
14 : : #include "ad/rss/world/WorldModelValidInputRange.hpp"
15 : :
16 : : namespace ad {
17 : : namespace rss {
18 : : namespace core {
19 : :
20 : : using physics::Distance;
21 : : using physics::MetricRange;
22 : :
23 : 0 : void RssSituationExtraction::dropObjectHistory(world::ObjectId const &object_id)
24 : : {
25 : 0 : mConstellationIdProvider.dropConstellationIds(object_id);
26 : 0 : }
27 : :
28 : 3981 : void RssSituationExtraction::calcluateRelativeLongitudinalPosition(MetricRange const &egoMetricRange,
29 : : MetricRange const &otherMetricRange,
30 : : LongitudinalRelativePosition &longitudinal_position,
31 : : Distance &longitudinal_distance)
32 : : {
33 [ + + ]: 3981 : if (egoMetricRange.minimum > otherMetricRange.maximum)
34 : : {
35 : 723 : longitudinal_position = LongitudinalRelativePosition::InFront;
36 : 723 : longitudinal_distance = egoMetricRange.minimum - otherMetricRange.maximum;
37 : : }
38 [ + + ]: 3257 : else if (otherMetricRange.minimum > egoMetricRange.maximum)
39 : : {
40 : 2112 : longitudinal_position = LongitudinalRelativePosition::AtBack;
41 : 2112 : longitudinal_distance = otherMetricRange.minimum - egoMetricRange.maximum;
42 : : }
43 : : else
44 : : {
45 : 1146 : longitudinal_distance = Distance(0.);
46 [ + + + + : 1146 : if ((egoMetricRange.minimum > otherMetricRange.minimum) && (egoMetricRange.maximum > otherMetricRange.maximum))
+ + ]
47 : : {
48 : 105 : longitudinal_position = LongitudinalRelativePosition::OverlapFront;
49 : : }
50 [ + + + + : 1041 : else if ((egoMetricRange.minimum < otherMetricRange.minimum) && (egoMetricRange.maximum < otherMetricRange.maximum))
+ + ]
51 : : {
52 : 19 : longitudinal_position = LongitudinalRelativePosition::OverlapBack;
53 : : }
54 : : else
55 : : {
56 : 1022 : longitudinal_position = LongitudinalRelativePosition::Overlap;
57 : : }
58 : : }
59 : 3981 : }
60 : :
61 : 802 : void RssSituationExtraction::calcluateRelativeLongitudinalPositionIntersection(
62 : : MetricRange const &egoMetricRange,
63 : : MetricRange const &otherMetricRange,
64 : : LongitudinalRelativePosition &longitudinal_position,
65 : : Distance &longitudinal_distance)
66 : : {
67 [ + + ]: 802 : if (egoMetricRange.maximum < otherMetricRange.minimum)
68 : : {
69 : 109 : longitudinal_position = LongitudinalRelativePosition::InFront;
70 : 109 : longitudinal_distance = otherMetricRange.minimum - egoMetricRange.maximum;
71 : : }
72 [ + + ]: 693 : else if (otherMetricRange.maximum < egoMetricRange.minimum)
73 : : {
74 : 545 : longitudinal_position = LongitudinalRelativePosition::AtBack;
75 : 545 : longitudinal_distance = egoMetricRange.minimum - otherMetricRange.maximum;
76 : : }
77 : : else
78 : : {
79 : 148 : longitudinal_distance = Distance(0.);
80 [ + + + + : 148 : if ((egoMetricRange.minimum < otherMetricRange.minimum) && (egoMetricRange.maximum < otherMetricRange.maximum))
+ + ]
81 : : {
82 : 52 : longitudinal_position = LongitudinalRelativePosition::OverlapFront;
83 : : }
84 [ + + + - : 96 : else if ((egoMetricRange.minimum > otherMetricRange.minimum) && (egoMetricRange.maximum > otherMetricRange.maximum))
+ + ]
85 : : {
86 : 48 : longitudinal_position = LongitudinalRelativePosition::OverlapBack;
87 : : }
88 : : else
89 : : {
90 : 48 : longitudinal_position = LongitudinalRelativePosition::Overlap;
91 : : }
92 : : }
93 : 802 : }
94 : :
95 : 3834 : void RssSituationExtraction::calcluateRelativeLateralPosition(MetricRange const &egoMetricRange,
96 : : MetricRange const &otherMetricRange,
97 : : LateralRelativePosition &lateral_position,
98 : : Distance &lateral_distance)
99 : : {
100 [ + + ]: 3834 : if (egoMetricRange.minimum > otherMetricRange.maximum)
101 : : {
102 : 1145 : lateral_position = LateralRelativePosition::AtRight;
103 : 1145 : lateral_distance = egoMetricRange.minimum - otherMetricRange.maximum;
104 : : }
105 [ + + ]: 2689 : else if (otherMetricRange.minimum > egoMetricRange.maximum)
106 : : {
107 : 923 : lateral_position = LateralRelativePosition::AtLeft;
108 : 923 : lateral_distance = otherMetricRange.minimum - egoMetricRange.maximum;
109 : : }
110 : : else
111 : : {
112 : 1766 : lateral_distance = Distance(0.);
113 [ + + + + : 1766 : if ((egoMetricRange.minimum > otherMetricRange.minimum) && (egoMetricRange.maximum > otherMetricRange.maximum))
+ + ]
114 : : {
115 : 117 : lateral_position = LateralRelativePosition::OverlapRight;
116 : : }
117 [ + + + + : 1649 : else if ((egoMetricRange.minimum < otherMetricRange.minimum) && (egoMetricRange.maximum < otherMetricRange.maximum))
+ + ]
118 : : {
119 : 133 : lateral_position = LateralRelativePosition::OverlapLeft;
120 : : }
121 : : else
122 : : {
123 : 1516 : lateral_position = LateralRelativePosition::Overlap;
124 : : }
125 : : }
126 : 3834 : }
127 : :
128 : 3961 : bool RssSituationExtraction::convertObjectsNonIntersection(world::Constellation const ¤tConstellation,
129 : : core::RelativeConstellation &constellation)
130 : : {
131 [ + + ]: 3961 : if (!currentConstellation.intersecting_road.empty())
132 : : {
133 [ + - + - ]: 2 : core::getLogger()->error(
134 : : "RssSituationExtraction::convertObjectsNonIntersection[{}->{}]>> Intersecting road not empty {}",
135 : 1 : currentConstellation.ego_vehicle.object_id,
136 [ + - ]: 1 : currentConstellation.object.object_id,
137 : : currentConstellation);
138 : 1 : return false;
139 : : }
140 : :
141 : 3959 : bool result = true;
142 : :
143 [ + - ]: 3959 : structured::ObjectDimensions egoVehicleDimension;
144 [ + - ]: 3961 : structured::ObjectDimensions objectToBeCheckedDimension;
145 [ + - ]: 3960 : result = structured::calculateObjectDimensions(currentConstellation, egoVehicleDimension, objectToBeCheckedDimension);
146 : :
147 : : LongitudinalRelativePosition longitudinal_position;
148 : 3961 : Distance longitudinal_distance;
149 [ + - ]: 3960 : calcluateRelativeLongitudinalPosition(egoVehicleDimension.longitudinalDimensions,
150 : : objectToBeCheckedDimension.longitudinalDimensions,
151 : : longitudinal_position,
152 : : longitudinal_distance);
153 : :
154 : 3959 : constellation.relative_position.longitudinal_position = longitudinal_position;
155 : 3959 : constellation.relative_position.longitudinal_distance = longitudinal_distance;
156 : :
157 : 3959 : constellation.ego_state.structured_object_state.is_in_correct_lane = !egoVehicleDimension.onNegativeLane;
158 : :
159 [ + + ]: 3959 : if (currentConstellation.constellation_type == ::ad::rss::world::ConstellationType::OppositeDirection)
160 : : {
161 : 1101 : constellation.other_state.structured_object_state.is_in_correct_lane = !objectToBeCheckedDimension.onPositiveLane;
162 : : }
163 : : else
164 : : {
165 : 2858 : constellation.other_state.structured_object_state.is_in_correct_lane = !objectToBeCheckedDimension.onNegativeLane;
166 : : }
167 : :
168 : : /**
169 : : * Set lateral restrictions
170 : : */
171 [ + + ]: 3959 : if (result)
172 : : {
173 : : LateralRelativePosition lateral_position;
174 : 3812 : Distance lateral_distance;
175 [ + - ]: 3812 : calcluateRelativeLateralPosition(egoVehicleDimension.lateralDimensions,
176 : : objectToBeCheckedDimension.lateralDimensions,
177 : : lateral_position,
178 : : lateral_distance);
179 : :
180 : 3814 : constellation.relative_position.lateral_position = lateral_position;
181 : 3814 : constellation.relative_position.lateral_distance = lateral_distance;
182 : : }
183 : 3961 : return result;
184 : : }
185 : :
186 : 1604 : void RssSituationExtraction::convertToIntersectionCentric(MetricRange const &objectDimension,
187 : : MetricRange const &intersectionPosition,
188 : : MetricRange &dimensionsIntersection)
189 : : {
190 : 1604 : dimensionsIntersection.maximum = intersectionPosition.minimum - objectDimension.minimum;
191 : 1604 : dimensionsIntersection.minimum = intersectionPosition.minimum - objectDimension.maximum;
192 : 1604 : }
193 : :
194 : 845 : bool RssSituationExtraction::convertObjectsIntersection(world::Constellation const ¤tConstellation,
195 : : core::RelativeConstellation &constellation)
196 : : {
197 [ + - ]: 845 : structured::ObjectDimensions egoVehicleDimension;
198 [ + - ]: 845 : structured::ObjectDimensions objectDimension;
199 : :
200 : 1690 : bool result = structured::calculateObjectDimensions(
201 [ + - ]: 845 : currentConstellation.ego_vehicle, currentConstellation.ego_vehicle_road, egoVehicleDimension);
202 : :
203 : 845 : result = result
204 [ + + + + ]: 1669 : && structured::calculateObjectDimensions(
205 [ + - ]: 824 : currentConstellation.object, currentConstellation.intersecting_road, objectDimension);
206 : :
207 [ + + ]: 845 : if (result)
208 : : {
209 : 802 : constellation.ego_state.structured_object_state.is_in_correct_lane = !egoVehicleDimension.onNegativeLane;
210 : 802 : constellation.other_state.structured_object_state.is_in_correct_lane = !objectDimension.onNegativeLane;
211 : :
212 : : // For intersection the lanes don't have the same origin so the positions cannot be directly compared
213 : : // Intersection entry should be the common point so convert the positions to this reference point
214 [ + - ]: 802 : MetricRange egoDimensionsIntersection;
215 [ + - ]: 802 : convertToIntersectionCentric(
216 : : egoVehicleDimension.longitudinalDimensions, egoVehicleDimension.intersectionPosition, egoDimensionsIntersection);
217 : :
218 [ + - ]: 802 : MetricRange objectDimensionsIntersection;
219 : :
220 [ + - ]: 802 : convertToIntersectionCentric(
221 : : objectDimension.longitudinalDimensions, objectDimension.intersectionPosition, objectDimensionsIntersection);
222 : :
223 : : constellation.ego_state.structured_object_state.distance_to_enter_intersection
224 [ + - ]: 802 : = std::max(Distance(0.), egoDimensionsIntersection.minimum);
225 : : constellation.ego_state.structured_object_state.distance_to_leave_intersection
226 [ + - ]: 802 : = std::max(Distance(0.),
227 [ + - ]: 802 : egoVehicleDimension.intersectionPosition.maximum - egoVehicleDimension.longitudinalDimensions.minimum);
228 : :
229 : : constellation.other_state.structured_object_state.distance_to_enter_intersection
230 [ + - ]: 802 : = std::max(Distance(0.), objectDimensionsIntersection.minimum);
231 [ + - ]: 802 : constellation.other_state.structured_object_state.distance_to_leave_intersection = std::max(
232 [ + - ]: 802 : Distance(0.), objectDimension.intersectionPosition.maximum - objectDimension.longitudinalDimensions.minimum);
233 : :
234 : : LongitudinalRelativePosition longitudinal_position;
235 : 802 : Distance longitudinal_distance;
236 [ + - ]: 802 : calcluateRelativeLongitudinalPositionIntersection(
237 : : egoDimensionsIntersection, objectDimensionsIntersection, longitudinal_position, longitudinal_distance);
238 : :
239 : 802 : constellation.relative_position.longitudinal_position = longitudinal_position;
240 : 802 : constellation.relative_position.longitudinal_distance = longitudinal_distance;
241 : :
242 : 802 : constellation.relative_position.lateral_position = LateralRelativePosition::Overlap;
243 : 802 : constellation.relative_position.lateral_distance = Distance(0.);
244 : : }
245 : :
246 [ + + ]: 845 : if (currentConstellation.constellation_type == world::ConstellationType::IntersectionEgoHasPriority)
247 : : {
248 : 409 : constellation.ego_state.structured_object_state.has_priority = true;
249 : 409 : constellation.other_state.structured_object_state.has_priority = false;
250 : : }
251 [ + + ]: 436 : else if (currentConstellation.constellation_type == world::ConstellationType::IntersectionObjectHasPriority)
252 : : {
253 : 218 : constellation.ego_state.structured_object_state.has_priority = false;
254 : 218 : constellation.other_state.structured_object_state.has_priority = true;
255 : : }
256 [ + - ]: 218 : else if (currentConstellation.constellation_type == world::ConstellationType::IntersectionSamePriority)
257 : : {
258 : 218 : constellation.ego_state.structured_object_state.has_priority = false;
259 : 218 : constellation.other_state.structured_object_state.has_priority = false;
260 : : }
261 : : else
262 : : {
263 : : // This function should never be called if we are not in intersection constellation
264 [ # # # # ]: 0 : core::getLogger()->error(
265 : : "RssSituationExtraction::convertObjectsIntersection[{}->{}]>> Unexpected constellation_type {}",
266 : 0 : currentConstellation.ego_vehicle.object_id,
267 [ # # ]: 0 : currentConstellation.object.object_id,
268 : : currentConstellation);
269 : : result = false; // LCOV_EXCL_LINE: unreachable code, keep to be on the safe side
270 : : }
271 : :
272 : 845 : return result;
273 : : }
274 : :
275 : 5001 : bool RssSituationExtraction::extractConstellationInputRangeChecked(world::TimeIndex const &time_index,
276 : : world::Constellation const &worldConstellation,
277 : : core::RelativeConstellation &relativeConstellation)
278 : : {
279 : : // ensure the object types are semantically correct
280 : : // @toDo: add this restriction to the data type model
281 : : // and extend generated withinValidInputRange by these
282 [ + + ]: 5001 : if (((worldConstellation.object.object_type != world::ObjectType::OtherVehicle)
283 [ + + ]: 14 : && (worldConstellation.object.object_type != world::ObjectType::ArtificialObject)
284 [ + + ]: 12 : && (worldConstellation.object.object_type != world::ObjectType::ArtificialPedestrian)
285 [ + + ]: 9 : && (worldConstellation.object.object_type != world::ObjectType::ArtificialVehicle)
286 [ + + ]: 6 : && (worldConstellation.object.object_type != world::ObjectType::Pedestrian)
287 [ + + ]: 5 : && (worldConstellation.object.object_type != world::ObjectType::Bicycle)
288 [ + + ]: 4 : && (worldConstellation.object.object_type != world::ObjectType::OtherObject))
289 [ + + ]: 4998 : || (worldConstellation.ego_vehicle.object_type != world::ObjectType::EgoVehicle))
290 : : {
291 [ + - ]: 24 : core::getLogger()->error(
292 : : "RssSituationExtraction::extractConstellationInputRangeChecked[{}->{}]>> Invalid object type. Ego: "
293 : : "{} Object: {}",
294 : 12 : worldConstellation.ego_vehicle.object_id,
295 : 12 : worldConstellation.object.object_id,
296 : 12 : worldConstellation.ego_vehicle,
297 [ + - ]: 12 : worldConstellation.object);
298 : 12 : return false;
299 : : }
300 [ + + ]: 4989 : if (worldConstellation.object.object_id == worldConstellation.ego_vehicle.object_id)
301 : : {
302 [ + - ]: 2 : core::getLogger()->error(
303 : : "RssSituationExtraction::extractConstellationInputRangeChecked[{}->{}]>> Object and ego vehicle must not have "
304 : : "the same id. Ego: {} Object: {}",
305 : 1 : worldConstellation.ego_vehicle.object_id,
306 : 1 : worldConstellation.object.object_id,
307 : 1 : worldConstellation.ego_vehicle,
308 [ + - ]: 1 : worldConstellation.object);
309 : 1 : return false;
310 : : }
311 : 4988 : bool result = false;
312 : :
313 : : try
314 : : {
315 : : relativeConstellation.constellation_id
316 [ + + ]: 4988 : = mConstellationIdProvider.getConstellationId(time_index, worldConstellation);
317 : 4968 : relativeConstellation.ego_id = worldConstellation.ego_vehicle.object_id;
318 : 4968 : relativeConstellation.object_id = worldConstellation.object.object_id;
319 : 4968 : relativeConstellation.constellation_type = worldConstellation.constellation_type;
320 : :
321 : 4968 : relativeConstellation.ego_state.object_type = worldConstellation.ego_vehicle.object_type;
322 : 4968 : relativeConstellation.other_state.object_type = worldConstellation.object.object_type;
323 : :
324 : 4968 : relativeConstellation.ego_state.unstructured_object_state = worldConstellation.ego_vehicle.state;
325 : 4968 : relativeConstellation.other_state.unstructured_object_state = worldConstellation.object.state;
326 : :
327 : 4968 : relativeConstellation.ego_state.structured_object_state.has_priority = false;
328 : 4968 : relativeConstellation.other_state.structured_object_state.has_priority = false;
329 : :
330 : 4968 : relativeConstellation.ego_state.structured_object_state.is_in_correct_lane = true;
331 : 4968 : relativeConstellation.other_state.structured_object_state.is_in_correct_lane = true;
332 : :
333 : 4968 : relativeConstellation.ego_state.structured_object_state.distance_to_enter_intersection = Distance(0.);
334 : 4968 : relativeConstellation.ego_state.structured_object_state.distance_to_leave_intersection = Distance(1000.);
335 : :
336 : 4966 : relativeConstellation.other_state.structured_object_state.distance_to_enter_intersection = Distance(0.);
337 : 4966 : relativeConstellation.other_state.structured_object_state.distance_to_leave_intersection = Distance(1000.);
338 : :
339 : 4968 : structured::convertVehicleStateDynamics(
340 [ + - ]: 4968 : worldConstellation.ego_vehicle, worldConstellation.ego_vehicle_rss_dynamics, relativeConstellation.ego_state);
341 : 4964 : structured::convertVehicleStateDynamics(
342 [ + - ]: 4964 : worldConstellation.object, worldConstellation.object_rss_dynamics, relativeConstellation.other_state);
343 : :
344 [ + + + - ]: 4967 : switch (worldConstellation.constellation_type)
345 : : {
346 : 3962 : case ad::rss::world::ConstellationType::SameDirection:
347 : : case ad::rss::world::ConstellationType::OppositeDirection:
348 : : {
349 [ + - ]: 3962 : result = convertObjectsNonIntersection(worldConstellation, relativeConstellation);
350 : :
351 : 3962 : break;
352 : : }
353 : 845 : case ad::rss::world::ConstellationType::IntersectionEgoHasPriority:
354 : : case ad::rss::world::ConstellationType::IntersectionObjectHasPriority:
355 : : case ad::rss::world::ConstellationType::IntersectionSamePriority:
356 : : {
357 [ + - ]: 845 : result = convertObjectsIntersection(worldConstellation, relativeConstellation);
358 : 845 : break;
359 : : }
360 : 160 : case ad::rss::world::ConstellationType::NotRelevant:
361 : : case ad::rss::world::ConstellationType::Unstructured:
362 : : {
363 : 160 : result = true;
364 : 160 : break;
365 : : }
366 : 0 : default:
367 : : {
368 [ + - + - ]: 1 : core::getLogger()->error(
369 : : "RssSituationExtraction::extractConstellationInputRangeChecked[{}->{}]>> Invalid constellation type {}",
370 : 1 : worldConstellation.ego_vehicle.object_id,
371 [ + - ]: 1 : worldConstellation.object.object_id,
372 : : worldConstellation);
373 : 1 : result = false;
374 : 1 : break;
375 : : }
376 : : }
377 : : }
378 [ + - ]: 21 : catch (std::exception &e)
379 : : {
380 [ + - + - ]: 42 : core::getLogger()->critical(
381 : : "RssSituationExtraction::extractConstellationInputRangeChecked[{}->{}]>> Exception caught '{}' {} {}",
382 : 21 : worldConstellation.ego_vehicle.object_id,
383 : 21 : worldConstellation.object.object_id,
384 [ + - ]: 42 : e.what(),
385 : : time_index,
386 : : worldConstellation);
387 : 21 : result = false;
388 : 21 : }
389 : 0 : catch (...)
390 : : {
391 [ - - - - ]: 0 : core::getLogger()->critical(
392 : : "RssSituationExtraction::extractConstellationInputRangeChecked[{}->{}]>> Exception caught {} {}",
393 : 0 : worldConstellation.ego_vehicle.object_id,
394 [ - - ]: 0 : worldConstellation.object.object_id,
395 : : time_index,
396 : : worldConstellation);
397 : 0 : result = false;
398 : 0 : }
399 : :
400 : 4988 : return result;
401 : : }
402 : :
403 : 70 : bool RssSituationExtraction::mergeVehicleStates(MergeMode const &mergeMode,
404 : : core::RelativeObjectState const &other_state,
405 : : core::RelativeObjectState &mergedVehicleState)
406 : : {
407 : : // on vehicle states there are only differences in intersection distances allowed due to different road definitions
408 : 70 : if ((other_state.dynamics.alpha_lat.accel_max != mergedVehicleState.dynamics.alpha_lat.accel_max)
409 [ + + ]: 68 : || (other_state.dynamics.alpha_lat.brake_min != mergedVehicleState.dynamics.alpha_lat.brake_min)
410 [ + + ]: 67 : || (other_state.dynamics.alpha_lon.accel_max != mergedVehicleState.dynamics.alpha_lon.accel_max)
411 [ + + ]: 67 : || (other_state.dynamics.alpha_lon.brake_min_correct != mergedVehicleState.dynamics.alpha_lon.brake_min_correct)
412 [ + + ]: 65 : || (other_state.dynamics.alpha_lon.brake_min != mergedVehicleState.dynamics.alpha_lon.brake_min)
413 [ + + ]: 65 : || (other_state.dynamics.alpha_lon.brake_max != mergedVehicleState.dynamics.alpha_lon.brake_max)
414 [ + - ]: 64 : || (other_state.dynamics.lateral_fluctuation_margin != mergedVehicleState.dynamics.lateral_fluctuation_margin)
415 [ + + ]: 63 : || (other_state.dynamics.response_time != mergedVehicleState.dynamics.response_time)
416 [ + - + + : 136 : || (other_state.structured_object_state.has_priority != mergedVehicleState.structured_object_state.has_priority))
+ + ]
417 : : {
418 : 8 : return false;
419 : : }
420 : : // consider the worst cases
421 [ + + ]: 60 : if (mergeMode == MergeMode::EgoVehicle)
422 : : {
423 : : // worst case for ego vehicle is not driving in correct lane
424 : : mergedVehicleState.structured_object_state.is_in_correct_lane
425 : 34 : = mergedVehicleState.structured_object_state.is_in_correct_lane
426 [ + - + + ]: 34 : && other_state.structured_object_state.is_in_correct_lane;
427 : : }
428 : : else
429 : : {
430 : : // worst case for other vehicle is driving in correct lane
431 : : mergedVehicleState.structured_object_state.is_in_correct_lane
432 : 26 : = mergedVehicleState.structured_object_state.is_in_correct_lane
433 [ - + - - ]: 26 : || other_state.structured_object_state.is_in_correct_lane;
434 : : }
435 : :
436 : : mergedVehicleState.structured_object_state.distance_to_enter_intersection
437 : 121 : = std::min(mergedVehicleState.structured_object_state.distance_to_enter_intersection,
438 : 60 : other_state.structured_object_state.distance_to_enter_intersection);
439 : : mergedVehicleState.structured_object_state.distance_to_leave_intersection
440 : 121 : = std::max(mergedVehicleState.structured_object_state.distance_to_leave_intersection,
441 : 61 : other_state.structured_object_state.distance_to_leave_intersection);
442 : : mergedVehicleState.structured_object_state.velocity.speed_lon_min
443 : 122 : = std::min(mergedVehicleState.structured_object_state.velocity.speed_lon_min,
444 : 60 : other_state.structured_object_state.velocity.speed_lon_min);
445 : : mergedVehicleState.structured_object_state.velocity.speed_lon_max
446 : 123 : = std::max(mergedVehicleState.structured_object_state.velocity.speed_lon_max,
447 : 62 : other_state.structured_object_state.velocity.speed_lon_max);
448 : : mergedVehicleState.structured_object_state.velocity.speed_lat_min
449 : 120 : = std::min(mergedVehicleState.structured_object_state.velocity.speed_lat_min,
450 : 61 : other_state.structured_object_state.velocity.speed_lat_min);
451 : : mergedVehicleState.structured_object_state.velocity.speed_lat_max
452 : 121 : = std::max(mergedVehicleState.structured_object_state.velocity.speed_lat_max,
453 : 59 : other_state.structured_object_state.velocity.speed_lat_max);
454 : :
455 : 62 : return true;
456 : : }
457 : :
458 : 35 : bool RssSituationExtraction::mergeConstellations(core::RelativeConstellation const &otherConstellation,
459 : : core::RelativeConstellation &mergedConstellation)
460 : : {
461 : 34 : if ( // basic data has to match
462 : 35 : (otherConstellation.constellation_id != mergedConstellation.constellation_id)
463 [ + - ]: 35 : || (otherConstellation.ego_id != mergedConstellation.ego_id)
464 [ + - ]: 35 : || (otherConstellation.object_id != mergedConstellation.object_id)
465 [ + - ]: 35 : || (otherConstellation.constellation_type != mergedConstellation.constellation_type)
466 [ + - + - ]: 35 : || !mergeVehicleStates(MergeMode::EgoVehicle, otherConstellation.ego_state, mergedConstellation.ego_state)
467 [ + - + - : 70 : || !mergeVehicleStates(MergeMode::OtherVehicle, otherConstellation.other_state, mergedConstellation.other_state))
+ + + + ]
468 : : {
469 : 8 : return false;
470 : : }
471 : :
472 : : // worst case
473 : : mergedConstellation.relative_position.longitudinal_distance
474 : 52 : = std::min(mergedConstellation.relative_position.longitudinal_distance,
475 : 26 : otherConstellation.relative_position.longitudinal_distance);
476 : 26 : if (otherConstellation.relative_position.longitudinal_position
477 [ + + ]: 26 : != mergedConstellation.relative_position.longitudinal_position)
478 : : {
479 [ + + ]: 8 : if ((mergedConstellation.relative_position.longitudinal_position == LongitudinalRelativePosition::Overlap)
480 [ + + ]: 7 : || (otherConstellation.relative_position.longitudinal_position == LongitudinalRelativePosition::Overlap)
481 [ + + ]: 6 : || ((mergedConstellation.relative_position.longitudinal_position < LongitudinalRelativePosition::Overlap)
482 [ + + ]: 3 : && (otherConstellation.relative_position.longitudinal_position > LongitudinalRelativePosition::Overlap))
483 [ + + ]: 5 : || ((mergedConstellation.relative_position.longitudinal_position > LongitudinalRelativePosition::Overlap)
484 [ + + ]: 3 : && (otherConstellation.relative_position.longitudinal_position < LongitudinalRelativePosition::Overlap)))
485 : : {
486 : : // overlap is also worst case for contradicting input, ensure the longitudinal distance becomes 0.
487 : 4 : mergedConstellation.relative_position.longitudinal_position = LongitudinalRelativePosition::Overlap;
488 : 4 : mergedConstellation.relative_position.longitudinal_distance = Distance(0.);
489 : : }
490 [ + + ]: 4 : else if ((mergedConstellation.relative_position.longitudinal_position == LongitudinalRelativePosition::OverlapFront)
491 [ + + ]: 3 : || (otherConstellation.relative_position.longitudinal_position
492 : : == LongitudinalRelativePosition::OverlapFront))
493 : : {
494 : : // one of the both is overlap font (the other then only can be InFront in here)
495 : 2 : mergedConstellation.relative_position.longitudinal_position = LongitudinalRelativePosition::OverlapFront;
496 : : }
497 [ + + ]: 2 : else if ((mergedConstellation.relative_position.longitudinal_position == LongitudinalRelativePosition::OverlapBack)
498 [ + - ]: 1 : || (otherConstellation.relative_position.longitudinal_position
499 : : == LongitudinalRelativePosition::OverlapBack))
500 : : {
501 : : // one of the both is overlap back (the other then only can be AtBack in here)
502 : 2 : mergedConstellation.relative_position.longitudinal_position = LongitudinalRelativePosition::OverlapBack;
503 : : }
504 : : else
505 : : {
506 : : // LCOV_EXCL_START: unreachable code, keep to be on the safe side
507 : : // this is impossible to reach, set to overlap having longitudinal distance to 0. to be on the safe side
508 : : mergedConstellation.relative_position.longitudinal_position = LongitudinalRelativePosition::Overlap;
509 : : mergedConstellation.relative_position.longitudinal_distance = Distance(0.);
510 : : // LCOV_EXCL_STOP: unreachable code, keep to be on the safe side
511 : : }
512 : : }
513 : :
514 : 53 : mergedConstellation.relative_position.lateral_distance = std::min(
515 : 26 : mergedConstellation.relative_position.lateral_distance, otherConstellation.relative_position.lateral_distance);
516 [ + + ]: 27 : if (otherConstellation.relative_position.lateral_position != mergedConstellation.relative_position.lateral_position)
517 : : {
518 [ + + ]: 8 : if ((mergedConstellation.relative_position.lateral_position == LateralRelativePosition::Overlap)
519 [ + + ]: 7 : || (otherConstellation.relative_position.lateral_position == LateralRelativePosition::Overlap)
520 [ + + ]: 6 : || ((mergedConstellation.relative_position.lateral_position < LateralRelativePosition::Overlap)
521 [ + + ]: 3 : && (otherConstellation.relative_position.lateral_position > LateralRelativePosition::Overlap))
522 [ + + ]: 5 : || ((mergedConstellation.relative_position.lateral_position > LateralRelativePosition::Overlap)
523 [ + + ]: 3 : && (otherConstellation.relative_position.lateral_position < LateralRelativePosition::Overlap)))
524 : : {
525 : : // overlap is also worst case for contradicting input, ensure the lateral distance becomes 0.
526 : 4 : mergedConstellation.relative_position.lateral_position = LateralRelativePosition::Overlap;
527 : 4 : mergedConstellation.relative_position.lateral_distance = Distance(0.);
528 : : }
529 [ + + ]: 4 : else if ((mergedConstellation.relative_position.lateral_position == LateralRelativePosition::OverlapLeft)
530 [ + + ]: 3 : || (otherConstellation.relative_position.lateral_position == LateralRelativePosition::OverlapLeft))
531 : : {
532 : : // one of the both is overlap left (the other then only can be AtLeft in here)
533 : 2 : mergedConstellation.relative_position.lateral_position = LateralRelativePosition::OverlapLeft;
534 : : }
535 [ + + ]: 2 : else if ((mergedConstellation.relative_position.lateral_position == LateralRelativePosition::OverlapRight)
536 [ + - ]: 1 : || (otherConstellation.relative_position.lateral_position == LateralRelativePosition::OverlapRight))
537 : : {
538 : : // one of the both is overlap right (the other then only can be AtRight in here)
539 : 2 : mergedConstellation.relative_position.lateral_position = LateralRelativePosition::OverlapRight;
540 : : }
541 : : else
542 : : {
543 : : // LCOV_EXCL_START: unreachable code, keep to be on the safe side
544 : : // this is impossible to reach, set to overlap having lateral distance to 0. to be on the safe side
545 : : mergedConstellation.relative_position.lateral_position = LateralRelativePosition::Overlap;
546 : : mergedConstellation.relative_position.lateral_distance = Distance(0.);
547 : : // LCOV_EXCL_STOP: unreachable code, keep to be on the safe side
548 : : }
549 : : }
550 : :
551 [ + - ]: 27 : mergedConstellation.world_model_indices.insert(mergedConstellation.world_model_indices.end(),
552 : : otherConstellation.world_model_indices.begin(),
553 : : otherConstellation.world_model_indices.end());
554 : :
555 : 25 : return true;
556 : : }
557 : :
558 : 4329 : bool RssSituationExtraction::extractSituation(world::WorldModel const &worldModel,
559 : : core::RssSituationSnapshot &situationSnapshot)
560 : : {
561 [ + + ]: 4329 : if (!withinValidInputRange(worldModel))
562 : : {
563 [ + - ]: 38 : core::getLogger()->error("RssSituationExtraction::extractSituation>> Invalid input {}", worldModel);
564 : 19 : return false;
565 : : }
566 : :
567 : 4310 : bool result = true;
568 : : try
569 : : {
570 : 4310 : situationSnapshot.time_index = worldModel.time_index;
571 : 4310 : situationSnapshot.default_ego_vehicle_rss_dynamics = worldModel.default_ego_vehicle_rss_dynamics;
572 : 4310 : situationSnapshot.constellations.clear();
573 [ + + ]: 9275 : for (uint64_t world_model_index = 0u; world_model_index < worldModel.constellations.size(); world_model_index++)
574 : : {
575 [ + + ]: 5108 : auto const worldConstellation = worldModel.constellations[world_model_index];
576 [ + - ]: 5009 : core::RelativeConstellation relativeConstellation;
577 [ + + ]: 5010 : relativeConstellation.world_model_indices.push_back(world_model_index);
578 : : bool const extractResult
579 [ + - ]: 4993 : = extractConstellationInputRangeChecked(worldModel.time_index, worldConstellation, relativeConstellation);
580 [ + + ]: 4995 : if (extractResult)
581 : : {
582 : : // constellation id creation might detect that different world constellations are representing identical
583 : : // relative constellations
584 : : // ensure the situationSnapshot is unique while containing the merged constellation
585 : : auto findResult
586 [ + - ]: 4774 : = std::find_if(situationSnapshot.constellations.begin(),
587 : : situationSnapshot.constellations.end(),
588 : 5580 : [&relativeConstellation](core::RelativeConstellation const &checkConstellation) {
589 : 5580 : return checkConstellation.constellation_id == relativeConstellation.constellation_id;
590 : : });
591 [ + + ]: 4770 : if (findResult == situationSnapshot.constellations.end())
592 : : {
593 [ + + ]: 4735 : situationSnapshot.constellations.push_back(relativeConstellation);
594 : : }
595 [ + - + + ]: 35 : else if (!mergeConstellations(relativeConstellation, *findResult))
596 : : {
597 : 8 : result = false;
598 : : }
599 : : }
600 : : else
601 : : {
602 [ + - + - ]: 442 : core::getLogger()->error("RssSituationExtraction::extractSituation>> Extraction failed {}", worldConstellation);
603 : 221 : result = false;
604 : : }
605 : 5052 : }
606 : : }
607 [ + - ]: 143 : catch (std::exception &e)
608 : : {
609 [ + - + - ]: 286 : core::getLogger()->critical(
610 [ + - ]: 286 : "RssSituationExtraction::extractSituation>> Exception caught '{}' {}", e.what(), worldModel);
611 : 143 : result = false;
612 : 143 : }
613 : 0 : catch (...)
614 : : {
615 [ - - - - ]: 0 : core::getLogger()->critical("RssSituationExtraction::extractSituation>> Exception caught {}", worldModel);
616 : 0 : result = false;
617 : 0 : }
618 : 4310 : return result;
619 : : }
620 : :
621 : : } // namespace core
622 : : } // namespace rss
623 : : } // namespace ad
|
