Branch data Line data Source code
1 : : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
2 : : //
3 : : // Copyright (C) 2019-2022 Intel Corporation
4 : : //
5 : : // SPDX-License-Identifier: LGPL-2.1-only
6 : : //
7 : : // ----------------- END LICENSE BLOCK -----------------------------------
8 : :
9 : : #include "ad/rss/map/RssWorldModelCreator.hpp"
10 : : #include <ad/map/lane/LaneOperation.hpp>
11 : : #include <ad/rss/world/ConstellationValidInputRange.hpp>
12 : : #include "ad/rss/map/Logging.hpp"
13 : :
14 : : /*!
15 : : * @brief namespace rss
16 : : */
17 : : namespace ad {
18 : : /*!
19 : : * @brief namespace rss
20 : : */
21 : : namespace rss {
22 : : /*!
23 : : * @brief namespace map
24 : : */
25 : : namespace map {
26 : :
27 : 261 : RssWorldModelCreator::RssWorldModelCreator(RssRestrictSpeedLimitMode const &restrict_speed_limit_mode,
28 : : ::ad::map::landmark::LandmarkIdSet const &greenTrafficLights,
29 : 261 : RssWorldModelCreation &worldModelCreation)
30 : 261 : : mGreenTrafficLights(greenTrafficLights)
31 : 261 : , mWorldModelCreation(worldModelCreation)
32 : : {
33 [ - - + + ]: 261 : switch (restrict_speed_limit_mode)
34 : : {
35 : 0 : case RssRestrictSpeedLimitMode::ExactSpeedLimit:
36 : 0 : mSpeedLimitFactor = 1.;
37 : 0 : break;
38 : 0 : case RssRestrictSpeedLimitMode::IncreasedSpeedLimit5:
39 : 0 : mSpeedLimitFactor = 1.05;
40 : 0 : break;
41 : 151 : case RssRestrictSpeedLimitMode::IncreasedSpeedLimit10:
42 : 151 : mSpeedLimitFactor = 1.1;
43 : 151 : break;
44 : 110 : case RssRestrictSpeedLimitMode::None:
45 : : default:
46 : 110 : mSpeedLimitFactor = 0.;
47 : 110 : break;
48 : : }
49 : 261 : }
50 : :
51 : 3 : RssWorldModelCreator::RssWorldModelCreator(RssWorldModelCreation &worldModelCreation)
52 : 3 : : mSpeedLimitFactor(0.)
53 : 3 : , mGreenTrafficLights()
54 : 3 : , mWorldModelCreation(worldModelCreation)
55 : : {
56 : 3 : }
57 : :
58 : 62 : void RssWorldModelCreator::extractLaneIntervalsAndPolygon(
59 : : std::vector<ad::map::route::RoadSegment> const &intersectionArea,
60 : : std::vector<ad::map::route::LaneInterval> &intersectionLaneIntervals,
61 : : ::ad::geometry::Polygon &polygon)
62 : : {
63 : 62 : ::ad::map::lane::ENUBorderList enuBorders;
64 [ + + ]: 136 : for (auto const &roadSegment : intersectionArea)
65 : : {
66 [ + - + - ]: 74 : enuBorders.push_back(ad::map::route::getENUBorderOfRoadSegment(roadSegment));
67 [ + + ]: 148 : for (auto const &laneSegment : roadSegment.drivable_lane_segments)
68 : : {
69 [ + - ]: 74 : intersectionLaneIntervals.push_back(laneSegment.lane_interval);
70 : : }
71 : : }
72 [ + - ]: 62 : polygon = ad::map::geometry::toPolygon(enuBorders);
73 : 62 : }
74 : :
75 : 62 : bool RssWorldModelCreator::updateExactIntersectionArea(
76 : : world::RoadArea &roadArea,
77 : : std::vector<ad::map::route::LaneInterval> const &originalLaneIntervals,
78 : : ad::map::match::LaneOccupiedRegionList const &exactIntersectingRegions)
79 : : {
80 : 62 : bool firstActualIntersectionSegmentProcessed = false;
81 : 62 : bool firstAfterIntersectionSegmentProcessed = false;
82 [ + + ]: 187 : for (auto &rssRoadSegment : roadArea)
83 : : {
84 [ + + ]: 125 : if (rssRoadSegment.type == world::RoadSegmentType::Intersection)
85 : : {
86 : 74 : physics::Distance min_length_before_intersecting_area = ad::physics::Distance(-1.);
87 : 74 : physics::Distance min_length_after_intersecting_area = ad::physics::Distance(-1.);
88 [ + + ]: 148 : for (auto const &rssLaneSegment : rssRoadSegment.lane_segments)
89 : : {
90 [ + + ]: 164 : for (auto const ®ion : exactIntersectingRegions)
91 : : {
92 [ + + ]: 90 : if (region.lane_id.mLaneId == rssLaneSegment.id)
93 : : {
94 [ + + ]: 160 : for (auto const &laneInterval : originalLaneIntervals)
95 : : {
96 [ + - + + ]: 90 : if (region.lane_id == laneInterval.lane_id)
97 : : {
98 [ - + ]: 70 : if (firstAfterIntersectionSegmentProcessed)
99 : : {
100 [ # # # # ]: 0 : getLogger()->warn(
101 : : "RssWorldModelCreator::updateExactIntersectionArea[:{}]>> additional intersection road segment {} "
102 : : "with matching exact intersection region {} while previously road segment already shortened. {}",
103 [ # # ]: 0 : mWorldModelCreation.mRouteId,
104 : : laneInterval,
105 : : region,
106 : : roadArea);
107 : : }
108 [ + - ]: 70 : auto laneIntervalRange = ad::map::route::toParametricRange(laneInterval);
109 [ + - ]: 70 : auto nonIntersectionRanges = ad::physics::subtract(laneIntervalRange, region.longitudinal_range);
110 : 70 : bool updateBefore = false;
111 : 70 : bool updateAfter = false;
112 [ + + ]: 144 : for (auto const &nonIntersectionRange : nonIntersectionRanges)
113 : : {
114 [ + - ]: 74 : if ((nonIntersectionRange.minimum == laneInterval.start)
115 [ + + + - : 74 : || (nonIntersectionRange.maximum == laneInterval.start))
+ + + + ]
116 : : {
117 : 58 : ad::map::route::LaneInterval laneIntervalBeforeIntersection = laneInterval;
118 [ + - + + ]: 58 : if (nonIntersectionRange.minimum == laneInterval.start)
119 : : {
120 : 33 : laneIntervalBeforeIntersection.end = nonIntersectionRange.maximum;
121 : : }
122 : : else
123 : : {
124 : 25 : laneIntervalBeforeIntersection.end = nonIntersectionRange.minimum;
125 : : }
126 [ + - ]: 58 : physics::MetricRange lengthBeforeIntersectingArea;
127 [ + - ]: 58 : physics::MetricRange width;
128 [ + - ]: 58 : getMetricRanges(laneIntervalBeforeIntersection, lengthBeforeIntersectingArea, width);
129 [ + - + - ]: 58 : if (min_length_before_intersecting_area < ad::physics::Distance(0.))
130 : : {
131 : 58 : min_length_before_intersecting_area = lengthBeforeIntersectingArea.minimum;
132 : : }
133 : : else
134 : : {
135 : : min_length_before_intersecting_area
136 [ # # ]: 0 : = std::min(min_length_before_intersecting_area, lengthBeforeIntersectingArea.minimum);
137 : : }
138 : 58 : updateBefore = true;
139 : : }
140 [ + - ]: 16 : else if ((nonIntersectionRange.minimum == laneInterval.end)
141 [ + + + - : 16 : || (nonIntersectionRange.maximum == laneInterval.end))
+ - + - ]
142 : : {
143 : 16 : ad::map::route::LaneInterval laneIntervalAfterIntersection = laneInterval;
144 [ + - + + ]: 16 : if (nonIntersectionRange.minimum == laneInterval.end)
145 : : {
146 : 8 : laneIntervalAfterIntersection.start = nonIntersectionRange.maximum;
147 : : }
148 : : else
149 : : {
150 : 8 : laneIntervalAfterIntersection.start = nonIntersectionRange.minimum;
151 : : }
152 [ + - ]: 16 : physics::MetricRange lengthAfterIntersectingArea;
153 [ + - ]: 16 : physics::MetricRange width;
154 [ + - ]: 16 : getMetricRanges(laneIntervalAfterIntersection, lengthAfterIntersectingArea, width);
155 [ + - + - ]: 16 : if (min_length_after_intersecting_area < ad::physics::Distance(0.))
156 : : {
157 : 16 : min_length_after_intersecting_area = lengthAfterIntersectingArea.minimum;
158 : : }
159 : : else
160 : : {
161 : : min_length_after_intersecting_area
162 [ # # ]: 0 : = std::min(min_length_after_intersecting_area, lengthAfterIntersectingArea.minimum);
163 : : }
164 : 16 : updateAfter = true;
165 : : }
166 : : }
167 [ + + ]: 70 : if (!updateBefore)
168 : : {
169 : : // nothing before is outside intersecting area for this lane, have to reset
170 : 12 : min_length_before_intersecting_area = ad::physics::Distance(0.);
171 : : }
172 [ + + ]: 70 : if (!updateAfter)
173 : : {
174 : : // nothing after is outside intersecting area for this lane, have to reset
175 : 54 : min_length_after_intersecting_area = ad::physics::Distance(0.);
176 : : }
177 : 70 : }
178 : : }
179 : : }
180 : : }
181 : : }
182 [ + + ]: 74 : if (!firstActualIntersectionSegmentProcessed)
183 : : {
184 : : // only update before if not yet processed
185 [ + - + + ]: 66 : if (min_length_before_intersecting_area < ad::physics::Distance(0.))
186 : : {
187 : : // road segment marked as intersection, but no lane segment of this seems actually be part of the area
188 : : // intersecting with the other route reset type to normal
189 : 4 : rssRoadSegment.type = world::RoadSegmentType::Normal;
190 : : }
191 : : else
192 : : {
193 : 62 : rssRoadSegment.minimum_length_before_intersecting_area = min_length_before_intersecting_area;
194 : 62 : firstActualIntersectionSegmentProcessed = true;
195 : : }
196 : : }
197 [ + + ]: 74 : if (firstActualIntersectionSegmentProcessed)
198 : : {
199 : : // only update "after" if already within intersection area processing
200 [ + - ]: 70 : if (!firstAfterIntersectionSegmentProcessed)
201 : : {
202 [ + - + + ]: 70 : if (min_length_after_intersecting_area > ad::physics::Distance(0.))
203 : : {
204 : 12 : rssRoadSegment.minimum_length_after_intersecting_area = min_length_after_intersecting_area;
205 : 12 : firstAfterIntersectionSegmentProcessed = true;
206 : : }
207 : : }
208 : : else
209 : : {
210 [ # # # # ]: 0 : if (min_length_after_intersecting_area >= ad::physics::Distance(0.))
211 : : {
212 : : // still some parts of this road segment seem to be part of the intersecting area, note it for correct
213 : : // handling of maximum intersection length
214 : 0 : rssRoadSegment.minimum_length_after_intersecting_area = min_length_after_intersecting_area;
215 : : }
216 : : else
217 : : {
218 : : // road segment marked as intersection, but no lane segment of this seems actually be part of the area
219 : : // intersecting with the other route reset type to normal
220 : 0 : rssRoadSegment.type = world::RoadSegmentType::Normal;
221 : : }
222 : : }
223 : : }
224 : : }
225 : : }
226 : 62 : return firstActualIntersectionSegmentProcessed;
227 : : }
228 : :
229 : 31 : bool RssWorldModelCreator::updateExactIntersectionArea(
230 : : world::RoadArea &roadAreaA,
231 : : std::vector<ad::map::route::RoadSegment> const &intersectionAreaA,
232 : : world::RoadArea &roadAreaB,
233 : : std::vector<ad::map::route::RoadSegment> const &intersectionAreaB)
234 : : {
235 : 31 : std::vector<ad::map::route::LaneInterval> intersectionLaneIntervalsA;
236 [ + - ]: 31 : ::ad::geometry::Polygon polygonA;
237 [ + - ]: 31 : extractLaneIntervalsAndPolygon(intersectionAreaA, intersectionLaneIntervalsA, polygonA);
238 : :
239 : 31 : std::vector<ad::map::route::LaneInterval> intersectionLaneIntervalsB;
240 [ + - ]: 31 : ::ad::geometry::Polygon polygonB;
241 [ + - ]: 31 : extractLaneIntervalsAndPolygon(intersectionAreaB, intersectionLaneIntervalsB, polygonB);
242 : :
243 : : ad::map::match::LaneOccupiedRegionList intersectingRegionA
244 [ + - ]: 31 : = ad::map::geometry::extractInnerRegions(polygonB, intersectionLaneIntervalsA);
245 : :
246 : : auto actualIntersectingAreaFound
247 [ + - ]: 31 : = updateExactIntersectionArea(roadAreaA, intersectionLaneIntervalsA, intersectingRegionA);
248 : :
249 [ + - ]: 31 : if (actualIntersectingAreaFound)
250 : : {
251 : : ad::map::match::LaneOccupiedRegionList intersectingRegionB
252 [ + - ]: 31 : = ad::map::geometry::extractInnerRegions(polygonA, intersectionLaneIntervalsB);
253 : :
254 : : actualIntersectingAreaFound
255 [ + - ]: 31 : = updateExactIntersectionArea(roadAreaB, intersectionLaneIntervalsB, intersectingRegionB);
256 : 31 : }
257 : :
258 : 31 : return actualIntersectingAreaFound;
259 : 31 : }
260 : :
261 : 455 : bool RssWorldModelCreator::createRoadArea(Route const &route,
262 : : ::ad::map::lane::LaneIdSet const &intersectionLanes,
263 : : std::vector<RssObjectConversion::Ptr> objects,
264 : : world::RoadArea &area,
265 : : std::vector<ad::map::route::RoadSegment> *intersectingArea)
266 : : {
267 : 455 : area.clear();
268 : 455 : bool intersectionEntered = false;
269 [ + + ]: 1305 : for (auto roadSegmentIter = route.route.road_segments.begin(); roadSegmentIter != route.route.road_segments.end();
270 : 850 : roadSegmentIter++)
271 : : {
272 [ + - ]: 912 : world::RoadSegment rssRoadSegment;
273 : 912 : rssRoadSegment.minimum_length_before_intersecting_area = ad::physics::Distance(0.);
274 : 912 : rssRoadSegment.minimum_length_after_intersecting_area = ad::physics::Distance(0.);
275 : 912 : rssRoadSegment.type = world::RoadSegmentType::Normal;
276 : :
277 [ + - ]: 912 : ad::map::route::RoadSegment intersectingRoadSegment;
278 : : // RoadSegments are in strict order from right to left
279 : : // RSS order is vice-versa
280 [ + - ]: 2439 : for (auto laneSegmentIter = roadSegmentIter->drivable_lane_segments.crbegin();
281 [ + - + + ]: 2439 : laneSegmentIter != roadSegmentIter->drivable_lane_segments.crend();
282 : : laneSegmentIter++)
283 : : {
284 [ + - ]: 1527 : if ((laneSegmentIter->route_lane_offset >= route.min_lane_offset)
285 [ + - + - : 1527 : && (laneSegmentIter->route_lane_offset <= route.max_lane_offset))
+ + + + ]
286 : : {
287 [ + - ]: 1192 : world::LaneSegment rssLaneSegment;
288 [ + - ]: 1192 : rssLaneSegment.id = world::LaneSegmentId(laneSegmentIter->lane_interval.lane_id.mLaneId);
289 [ + - + - : 1192 : if (intersectionLanes.count(laneSegmentIter->lane_interval.lane_id) > 0)
+ + ]
290 : : {
291 : 74 : rssRoadSegment.type = world::RoadSegmentType::Intersection;
292 : : }
293 [ + - + + ]: 1192 : if (laneSegmentIter->lane_interval.wrong_way)
294 : : {
295 [ - + ]: 280 : if (rssRoadSegment.type == world::RoadSegmentType::Intersection)
296 : : {
297 : : // within intersections only positive direction lanes are considered to be more exact
298 : : // on the actual intersecting region (input should not contain such already)
299 : 0 : continue;
300 : : }
301 : 280 : rssLaneSegment.driving_direction = world::LaneDrivingDirection::Negative;
302 : : }
303 : : else
304 : : {
305 : 912 : rssLaneSegment.driving_direction = world::LaneDrivingDirection::Positive;
306 [ + + ]: 912 : if (rssRoadSegment.type == world::RoadSegmentType::Intersection)
307 : : {
308 [ + - ]: 74 : intersectingRoadSegment.drivable_lane_segments.insert(
309 : 148 : intersectingRoadSegment.drivable_lane_segments.begin(), *laneSegmentIter);
310 : : }
311 : : }
312 [ + - + - ]: 1192 : getMetricRanges(laneSegmentIter->lane_interval, rssLaneSegment.length, rssLaneSegment.width);
313 : :
314 [ + - + - ]: 1192 : auto const speedLimitRestrictions = getSpeedLimits(laneSegmentIter->lane_interval);
315 [ + + ]: 3189 : for (auto object : objects)
316 : : {
317 [ + + ]: 3994 : for (auto const &speedLimitRestriction : speedLimitRestrictions)
318 : : {
319 [ + - + - ]: 1997 : object->updateSpeedLimit(speedLimitRestriction.speed_limit * mSpeedLimitFactor);
320 : : }
321 [ + - + - ]: 1997 : object->laneIntervalAdded(laneSegmentIter->lane_interval);
322 : 1997 : }
323 : :
324 [ + - ]: 1192 : rssRoadSegment.lane_segments.push_back(rssLaneSegment);
325 : 1192 : }
326 : : }
327 [ + + ]: 912 : if (rssRoadSegment.type == world::RoadSegmentType::Intersection)
328 : : {
329 : 74 : intersectionEntered = true;
330 [ + - ]: 74 : if (intersectingArea != nullptr)
331 : : {
332 [ + - ]: 74 : intersectingArea->push_back(intersectingRoadSegment);
333 : : }
334 : : }
335 [ + + ]: 838 : else if (intersectionEntered)
336 : : {
337 : : // intersection left, stop processing route
338 : 62 : break;
339 : : }
340 [ - + ]: 850 : if (rssRoadSegment.lane_segments.empty())
341 : : {
342 : : // empty road segment not allowed, restriction on lane offsets too large
343 : 0 : area.clear();
344 [ # # # # ]: 0 : getLogger()->debug("RssWorldModelCreator::createRoadArea[:{}]>> invalid road segment",
345 [ # # ]: 0 : mWorldModelCreation.mRouteId);
346 : 0 : return false;
347 : : }
348 [ + - ]: 850 : area.push_back(rssRoadSegment);
349 [ + + - + : 974 : }
+ - ]
350 [ + + ]: 455 : if (route.route_heading_available)
351 : : {
352 [ - + ]: 328 : if (objects.size() == 0u)
353 : : {
354 [ # # ]: 0 : getLogger()->error("RssWorldModelCreator::createRoadArea[:{}]>> no object to update provided. "
355 : : "Results might be incomplete.",
356 [ # # ]: 0 : mWorldModelCreation.mRouteId);
357 : 0 : return false;
358 : : }
359 : 328 : objects.front()->updateVelocityOnRoute(route.route_heading_start);
360 [ + - ]: 328 : if (objects.size() == 2u)
361 : : {
362 : 328 : objects.back()->updateVelocityOnRoute(route.route_heading_end);
363 : : }
364 [ - + ]: 328 : if (objects.size() > 2u)
365 : : {
366 [ # # ]: 0 : getLogger()->error("RssWorldModelCreator::createRoadArea[{}->{}:{}]>> more than two objects provided. "
367 : : "Results might be incomplete.",
368 [ # # # # ]: 0 : objects.front()->getId(),
369 [ # # ]: 0 : objects.back()->getId(),
370 : 0 : mWorldModelCreation.mRouteId);
371 : 0 : return false;
372 : : }
373 : : }
374 : : else
375 : : {
376 [ + + ]: 254 : for (auto object : objects)
377 : : {
378 [ + - ]: 127 : auto updateResult = object->updateVelocityOnRoute(route.route);
379 [ - + ]: 127 : if (!updateResult)
380 : : {
381 [ # # # # ]: 0 : getLogger()->error(
382 : : "RssWorldModelCreator::createRoadArea[{}->{}:{}]>> updateVelocityOnRoute for object {} failed.",
383 [ # # # # ]: 0 : objects.front()->getId(),
384 [ # # ]: 0 : objects.back()->getId(),
385 : 0 : mWorldModelCreation.mRouteId,
386 [ # # ]: 0 : object->getId());
387 : 0 : return false;
388 : : }
389 [ + - ]: 127 : }
390 : : }
391 : :
392 [ - + ]: 455 : if (!withinValidInputRange(area, false))
393 : : {
394 : : // this mainly happens for ego long routes in NonRelevant scenes
395 [ # # ]: 0 : if (area.size() > 50u)
396 : : {
397 : 0 : area.resize(50u);
398 : : }
399 : 0 : return false;
400 : : }
401 : :
402 : 455 : return true;
403 : : }
404 : :
405 : 68 : bool RssWorldModelCreator::appendNotRelevantConstellation(::ad::map::route::FullRoute const &route,
406 : : RssObjectConversion::ConstPtr iEgoObject,
407 : : RssObjectConversion::ConstPtr iOtherObject)
408 : : {
409 [ + - - + : 68 : if (!bool(iEgoObject) || !bool(iOtherObject))
- + ]
410 : : {
411 [ # # # # ]: 0 : getLogger()->error("RssWorldModelCreator::appendNotRelevantConstellation[:{}]>> ego/other object input is NULL",
412 [ # # ]: 0 : mWorldModelCreation.mRouteId);
413 : 0 : return false;
414 : : }
415 [ + - ]: 68 : auto egoObject = std::make_shared<RssObjectConversion>(*iEgoObject);
416 [ - + ]: 68 : if (!bool(egoObject))
417 : : {
418 [ # # # # ]: 0 : getLogger()->error("RssWorldModelCreator::appendNotRelevantConstellation[{}->{}:{}]>> failed to create copy of ego "
419 : : "RssObjectConversion",
420 [ # # # # ]: 0 : iEgoObject->getId(),
421 [ # # ]: 0 : iOtherObject->getId(),
422 : 0 : mWorldModelCreation.mRouteId);
423 : 0 : return false;
424 : : }
425 [ + - ]: 68 : auto otherObject = std::make_shared<RssObjectConversion>(*iOtherObject);
426 [ - + ]: 68 : if (!bool(otherObject))
427 : : {
428 [ # # # # ]: 0 : getLogger()->error(
429 : : "RssWorldModelCreator::appendNotRelevantConstellation[{}->{}:{}]>> failed to create copy of other "
430 : : "RssObjectConversion",
431 [ # # # # ]: 0 : iEgoObject->getId(),
432 [ # # ]: 0 : iOtherObject->getId(),
433 : 0 : mWorldModelCreation.mRouteId);
434 : 0 : return false;
435 : : }
436 : :
437 : : // in the end this constellation is only for convenience to get to know that we considered it and e.g. visualize it
438 : : // It could be left out completely
439 : 68 : world::RoadArea ego_vehicle_road;
440 [ + + ]: 68 : if (!route.road_segments.empty())
441 : : {
442 : : // in case the ego route is actually available, we can try to fill the route as usual to have some hint where the
443 : : // ego is driving
444 : : bool createRoadAreaResult
445 [ + - + - : 124 : = createRoadArea(Route(route), ::ad::map::lane::LaneIdSet(), {egoObject}, ego_vehicle_road);
+ + - - ]
446 [ - + ]: 62 : if (!createRoadAreaResult)
447 : : {
448 [ # # # # ]: 0 : getLogger()->info("RssWorldModelCreator::appendNotRelevantConstellation[{}->{}:{}]>> failed to create ego "
449 : : "vehicle road area from route {}",
450 [ # # # # ]: 0 : egoObject->getId(),
451 [ # # ]: 0 : otherObject->getId(),
452 : 0 : mWorldModelCreation.mRouteId,
453 : : route);
454 : : }
455 : : }
456 : :
457 [ + - + - ]: 136 : getLogger()->debug("RssWorldModelCreator::appendNotRelevantConstellation[{}->{}:{}]>>constellation {}",
458 [ + - + - ]: 68 : egoObject->getId(),
459 [ + - ]: 68 : otherObject->getId(),
460 : 68 : mWorldModelCreation.mRouteId,
461 : 136 : world::ConstellationType::NotRelevant);
462 : :
463 : 136 : return appendConstellation(
464 [ + - ]: 204 : world::ConstellationType::NotRelevant, egoObject, ego_vehicle_road, otherObject, world::RoadArea());
465 : 68 : }
466 : :
467 : 328 : bool RssWorldModelCreator::createNonIntersectionRoadArea(::ad::map::route::FullRoute const &route,
468 : : ::ad::map::point::ENUHeading const &route_heading_start,
469 : : ::ad::map::point::ENUHeading const &route_heading_end,
470 : : std::vector<RssObjectConversion::Ptr> objects,
471 : : world::RoadArea &road_area)
472 : : {
473 : : // take only the relevant part of the route, the two vehicles are driving on
474 : 328 : ::ad::map::route::RouteLaneOffset const min_lane_offset = std::min(0, route.destination_lane_offset);
475 : 328 : ::ad::map::route::RouteLaneOffset const max_lane_offset = std::max(0, route.destination_lane_offset);
476 : :
477 : : // no intersections on route
478 [ + - ]: 656 : bool result = createRoadArea(Route(route, min_lane_offset, max_lane_offset, route_heading_start, route_heading_end),
479 : 656 : ::ad::map::lane::LaneIdSet(),
480 : : objects,
481 : : road_area);
482 [ - + ]: 328 : if (!result)
483 : : {
484 : : // creation leads to invalid road_area; maybe there are some narrowing parts in between
485 : : // retry taking all lane segments
486 [ # # ]: 0 : result = createRoadArea(
487 : 0 : Route(route, route_heading_start, route_heading_end), ::ad::map::lane::LaneIdSet(), objects, road_area);
488 : : }
489 : 328 : return result;
490 : : }
491 : :
492 : 328 : bool RssWorldModelCreator::appendNonIntersectionConstellation(::ad::map::route::ConnectingRoute const &connectingRoute,
493 : : world::ConstellationType const &constellation_type,
494 : : RssObjectConversion::ConstPtr iEgoObject,
495 : : RssObjectConversion::ConstPtr iOtherObject)
496 : : {
497 [ + - - + : 328 : if (!bool(iEgoObject) || !bool(iOtherObject))
- + ]
498 : : {
499 [ # # # # ]: 0 : getLogger()->error("RssWorldModelCreator::appendNonIntersectionConstellation[:{}]>> ego/other object input is NULL",
500 [ # # ]: 0 : mWorldModelCreation.mRouteId);
501 : 0 : return false;
502 : : }
503 [ + - ]: 328 : auto egoObject = std::make_shared<RssObjectConversion>(*iEgoObject);
504 [ - + ]: 328 : if (!bool(egoObject))
505 : : {
506 [ # # # # ]: 0 : getLogger()->error(
507 : : "RssWorldModelCreator::appendNonIntersectionConstellation[{}->{}:{}]>> failed to create copy of ego "
508 : : "RssObjectConversion",
509 [ # # # # ]: 0 : iEgoObject->getId(),
510 [ # # ]: 0 : iOtherObject->getId(),
511 : 0 : mWorldModelCreation.mRouteId);
512 : 0 : return false;
513 : : }
514 [ + - ]: 328 : auto otherObject = std::make_shared<RssObjectConversion>(*iOtherObject);
515 [ - + ]: 328 : if (!bool(otherObject))
516 : : {
517 [ # # # # ]: 0 : getLogger()->error(
518 : : "RssWorldModelCreator::appendNonIntersectionConstellation[{}->{}:{}]>> failed to create copy of other "
519 : : "RssObjectConversion",
520 [ # # # # ]: 0 : iEgoObject->getId(),
521 [ # # ]: 0 : iOtherObject->getId(),
522 : 0 : mWorldModelCreation.mRouteId);
523 : 0 : return false;
524 : : }
525 : :
526 : 328 : world::RoadArea ego_vehicle_road;
527 [ + + ]: 328 : if (!connectingRoute.route_a.road_segments.empty())
528 : : {
529 [ + - + + : 960 : bool const createRoadAreaResult = createNonIntersectionRoadArea(connectingRoute.route_a,
- - ]
530 : 240 : connectingRoute.route_a_heading_start,
531 [ + - ]: 240 : connectingRoute.route_a_heading_end,
532 : : {egoObject, otherObject},
533 : : ego_vehicle_road);
534 [ - + ]: 240 : if (!createRoadAreaResult)
535 : : {
536 [ # # # # ]: 0 : getLogger()->error("RssWorldModelCreator::appendNonIntersectionConstellation[{}->{}:{}]>> error creating ego "
537 : : "vehicle road area from route_a {}",
538 [ # # # # ]: 0 : egoObject->getId(),
539 [ # # ]: 0 : otherObject->getId(),
540 : 0 : mWorldModelCreation.mRouteId,
541 : 0 : connectingRoute.route_a);
542 : 0 : return false;
543 : : }
544 : : }
545 [ + - ]: 88 : else if (!connectingRoute.route_b.road_segments.empty())
546 : : {
547 [ + - + + : 352 : bool const createRoadAreaResult = createNonIntersectionRoadArea(connectingRoute.route_b,
- - ]
548 : 88 : connectingRoute.route_b_heading_start,
549 [ + - ]: 88 : connectingRoute.route_b_heading_end,
550 : : {egoObject, otherObject},
551 : : ego_vehicle_road);
552 [ - + ]: 88 : if (!createRoadAreaResult)
553 : : {
554 [ # # # # ]: 0 : getLogger()->error("RssWorldModelCreator::appendNonIntersectionConstellation[{}->{}:{}]>> error creating ego "
555 : : "vehicle road area from route_b {}",
556 [ # # # # ]: 0 : egoObject->getId(),
557 [ # # ]: 0 : otherObject->getId(),
558 : 0 : mWorldModelCreation.mRouteId,
559 : 0 : connectingRoute.route_b);
560 : 0 : return false;
561 : : }
562 : : }
563 : : else
564 : : {
565 [ # # # # ]: 0 : getLogger()->error("RssWorldModelCreator::appendNonIntersectionConstellation[{}->{}:{}]>> constellation {} "
566 : : "expected either route_a or "
567 : : "route_b not to be empty {}",
568 [ # # # # ]: 0 : egoObject->getId(),
569 [ # # ]: 0 : otherObject->getId(),
570 : 0 : mWorldModelCreation.mRouteId,
571 : : constellation_type,
572 : : connectingRoute);
573 : 0 : return false;
574 : : }
575 : :
576 [ + - + - ]: 656 : getLogger()->debug(
577 : : "RssWorldModelCreator::appendNonIntersectionConstellation[{}->{}:{}]>> constellation {}\n road area {}",
578 [ + - + - ]: 328 : egoObject->getId(),
579 [ + - ]: 656 : otherObject->getId(),
580 : 328 : mWorldModelCreation.mRouteId,
581 : : constellation_type,
582 : : ego_vehicle_road);
583 : :
584 [ + - ]: 328 : return appendConstellation(constellation_type, egoObject, ego_vehicle_road, otherObject, world::RoadArea());
585 : 328 : }
586 : :
587 : 0 : bool RssWorldModelCreator::createMergingRoadArea(::ad::map::route::FullRoute const &route,
588 : : ::ad::map::point::ENUHeading const &route_heading_start,
589 : : ::ad::map::point::ENUHeading const &route_heading_end,
590 : : RssObjectConversion::Ptr object,
591 : : world::RoadArea &road_area)
592 : : {
593 : : // the last road segment of the merging road is the intersecting one
594 : 0 : ::ad::map::lane::LaneIdSet intersectionLanes;
595 : 0 : for (auto laneSegmentIter = route.road_segments.back().drivable_lane_segments.begin();
596 [ # # ]: 0 : laneSegmentIter != route.road_segments.back().drivable_lane_segments.end();
597 : 0 : laneSegmentIter++)
598 : : {
599 [ # # ]: 0 : intersectionLanes.insert(laneSegmentIter->lane_interval.lane_id);
600 : : }
601 : : // @todo: there might be more lanes in the merging roads that overlap, so add these also to the intersection lanes!
602 : :
603 : : // take the whole route
604 [ # # # # : 0 : return createRoadArea(Route(route, route_heading_start, route_heading_end), intersectionLanes, {object}, road_area);
# # # # ]
605 : 0 : }
606 : :
607 : 0 : bool RssWorldModelCreator::appendMergingConstellation(::ad::map::route::ConnectingRoute const &connectingRoute,
608 : : world::ConstellationType const &constellation_type,
609 : : RssObjectConversion::ConstPtr iEgoObject,
610 : : RssObjectConversion::ConstPtr iOtherObject)
611 : : {
612 [ # # # # : 0 : if (!bool(iEgoObject) || !bool(iOtherObject))
# # ]
613 : : {
614 [ # # # # ]: 0 : getLogger()->error("RssWorldModelCreator::appendMergingConstellation[:{}]>> ego/other object input is NULL",
615 [ # # ]: 0 : mWorldModelCreation.mRouteId);
616 : 0 : return false;
617 : : }
618 [ # # # # : 0 : if (connectingRoute.route_a.road_segments.empty() || connectingRoute.route_b.road_segments.empty())
# # ]
619 : : {
620 [ # # # # ]: 0 : getLogger()->error(
621 : : "RssWorldModelCreator::appendMergingConstellation[{}->{}:{}]>> constellation {} connecting route empty {}",
622 [ # # # # ]: 0 : iEgoObject->getId(),
623 [ # # ]: 0 : iOtherObject->getId(),
624 : 0 : mWorldModelCreation.mRouteId,
625 : : constellation_type,
626 : : connectingRoute);
627 : 0 : return false;
628 : : }
629 : :
630 [ # # ]: 0 : auto egoObject = std::make_shared<RssObjectConversion>(*iEgoObject);
631 [ # # ]: 0 : if (!bool(egoObject))
632 : : {
633 [ # # # # ]: 0 : getLogger()->error(
634 : : "RssWorldModelCreator::appendMergingConstellation[{}->{}:{}]>> failed to create copy of ego RssObjectConversion",
635 [ # # # # ]: 0 : iEgoObject->getId(),
636 [ # # ]: 0 : iOtherObject->getId(),
637 : 0 : mWorldModelCreation.mRouteId);
638 : 0 : return false;
639 : : }
640 [ # # ]: 0 : auto otherObject = std::make_shared<RssObjectConversion>(*iOtherObject);
641 [ # # ]: 0 : if (!bool(otherObject))
642 : : {
643 [ # # # # ]: 0 : getLogger()->error("RssWorldModelCreator::appendMergingConstellation[{}->{}:{}]>> failed to create copy of other "
644 : : "RssObjectConversion",
645 [ # # # # ]: 0 : iEgoObject->getId(),
646 [ # # ]: 0 : iOtherObject->getId(),
647 : 0 : mWorldModelCreation.mRouteId);
648 : 0 : return false;
649 : : }
650 : :
651 : 0 : world::RoadArea ego_vehicle_road;
652 : 0 : bool createMergingRoadAreaResult = createMergingRoadArea(connectingRoute.route_a,
653 : 0 : connectingRoute.route_a_heading_start,
654 [ # # ]: 0 : connectingRoute.route_a_heading_end,
655 : : egoObject,
656 : : ego_vehicle_road);
657 [ # # ]: 0 : if (!createMergingRoadAreaResult)
658 : : {
659 [ # # # # ]: 0 : getLogger()->error("RssWorldModelCreator::appendMergingConstellation[{}->{}:{}]>> failed to create ego vehicle "
660 : : "road area from route_a {}",
661 [ # # # # ]: 0 : egoObject->getId(),
662 [ # # ]: 0 : otherObject->getId(),
663 : 0 : mWorldModelCreation.mRouteId,
664 : 0 : connectingRoute.route_a);
665 : 0 : return false;
666 : : }
667 : 0 : world::RoadArea intersecting_road;
668 : 0 : createMergingRoadAreaResult = createMergingRoadArea(connectingRoute.route_b,
669 : 0 : connectingRoute.route_b_heading_start,
670 [ # # ]: 0 : connectingRoute.route_b_heading_end,
671 : : otherObject,
672 : : intersecting_road);
673 [ # # ]: 0 : if (!createMergingRoadAreaResult)
674 : : {
675 [ # # # # ]: 0 : getLogger()->error("RssWorldModelCreator::appendMergingConstellation[{}->{}:{}]>> failed to create intersecting "
676 : : "road area from route_b {}",
677 [ # # # # ]: 0 : egoObject->getId(),
678 [ # # ]: 0 : otherObject->getId(),
679 : 0 : mWorldModelCreation.mRouteId,
680 : 0 : connectingRoute.route_b);
681 : 0 : return false;
682 : : }
683 : :
684 [ # # # # ]: 0 : getLogger()->debug("RssWorldModelCreator::appendMergingConstellation[{}->{}:{}]>> constellation {}\n"
685 : : " ego road area {}\n intersection road area {}",
686 [ # # # # ]: 0 : egoObject->getId(),
687 [ # # ]: 0 : otherObject->getId(),
688 : 0 : mWorldModelCreation.mRouteId,
689 : : constellation_type,
690 : : ego_vehicle_road,
691 : : intersecting_road);
692 [ # # ]: 0 : return appendConstellation(constellation_type, egoObject, ego_vehicle_road, otherObject, intersecting_road);
693 : 0 : }
694 : :
695 : 62 : bool RssWorldModelCreator::createIntersectionRoadArea(::ad::map::route::FullRoute const &route,
696 : : ::ad::map::intersection::IntersectionConstPtr intersection,
697 : : RssObjectConversion::Ptr object,
698 : : world::RoadArea &road_area,
699 : : std::vector<ad::map::route::RoadSegment> &intersectingArea)
700 : : {
701 : : // only the lanes of the intersection under consideration are marked as intersection;
702 [ + - + - : 124 : return createRoadArea(Route(route), intersection->internalLanes(), {object}, road_area, &intersectingArea);
+ - + + -
- ]
703 : : }
704 : :
705 : 31 : bool RssWorldModelCreator::appendIntersectionConstellation(::ad::map::intersection::IntersectionPtr intersection,
706 : : ::ad::map::route::FullRoute const &egoRoute,
707 : : ::ad::map::route::FullRoute const &objectRoute,
708 : : ::ad::map::route::FullRoute const &intersectionOtherRoute,
709 : : RssObjectConversion::ConstPtr iEgoObject,
710 : : RssObjectConversion::ConstPtr iOtherObject)
711 : : {
712 [ + - - + : 31 : if (!bool(iEgoObject) || !bool(iOtherObject))
- + ]
713 : : {
714 [ # # # # ]: 0 : getLogger()->error("RssWorldModelCreator::appendIntersectionConstellation[:{}]>> ego/other object input is NULL",
715 [ # # ]: 0 : mWorldModelCreation.mRouteId);
716 : 0 : return false;
717 : : }
718 [ + - ]: 31 : auto egoObject = std::make_shared<RssObjectConversion>(*iEgoObject);
719 [ - + ]: 31 : if (!bool(egoObject))
720 : : {
721 [ # # # # ]: 0 : getLogger()->error("RssWorldModelCreator::appendIntersectionConstellation[{}->{}:{}]>> failed to create copy of "
722 : : "ego RssObjectConversion",
723 [ # # # # ]: 0 : iEgoObject->getId(),
724 [ # # ]: 0 : iOtherObject->getId(),
725 : 0 : mWorldModelCreation.mRouteId);
726 : 0 : return false;
727 : : }
728 [ + - ]: 31 : auto otherObject = std::make_shared<RssObjectConversion>(*iOtherObject);
729 [ - + ]: 31 : if (!bool(otherObject))
730 : : {
731 [ # # # # ]: 0 : getLogger()->error(
732 : : "RssWorldModelCreator::appendIntersectionConstellation[{}->{}:{}]>> failed to create copy of other "
733 : : "RssObjectConversion",
734 [ # # # # ]: 0 : iEgoObject->getId(),
735 [ # # ]: 0 : iOtherObject->getId(),
736 : 0 : mWorldModelCreation.mRouteId);
737 : 0 : return false;
738 : : }
739 : :
740 : 31 : auto constellation_type = world::ConstellationType::IntersectionObjectHasPriority;
741 [ + - - + ]: 31 : if (intersection->intersectionType() == ::ad::map::intersection::IntersectionType::TrafficLight)
742 : : {
743 : : // in case no traffic light actually modeled (map error), we assume others have always priority
744 : 0 : bool intersectionRouteHasGreen = true;
745 [ # # # # ]: 0 : if (intersection->applicableTrafficLights().empty())
746 : : {
747 [ # # # # ]: 0 : getLogger()->warn(
748 : : "RssWorldModelCreation::appendIntersectionConstellation[{}->{}:{}]>> traffic light intersection has no "
749 : : "applicable traffic lights {}",
750 [ # # # # ]: 0 : egoObject->getId(),
751 [ # # ]: 0 : otherObject->getId(),
752 : 0 : mWorldModelCreation.mRouteId,
753 [ # # # # ]: 0 : std::to_string(intersection->incomingLanesOnRoute()));
754 : 0 : intersectionRouteHasGreen = false;
755 : : }
756 [ # # # # ]: 0 : for (auto relevantTrafficLight : intersection->applicableTrafficLights())
757 : : {
758 [ # # # # ]: 0 : if (mGreenTrafficLights.find(relevantTrafficLight) == mGreenTrafficLights.end())
759 : : {
760 : : // if any relevant traffic light is not green, intersection route has no priority
761 : 0 : intersectionRouteHasGreen = false;
762 : 0 : break;
763 : : }
764 : : }
765 : :
766 [ # # # # : 0 : if (!intersectionRouteHasGreen || intersection->objectRouteCrossesLanesWithHigherPriority(intersectionOtherRoute))
# # # # ]
767 : : {
768 [ # # ]: 0 : if (intersectionOtherRoute.route_planning_counter == objectRoute.route_planning_counter)
769 : : {
770 : 0 : constellation_type = world::ConstellationType::IntersectionObjectHasPriority;
771 : : }
772 : : else
773 : : {
774 : 0 : constellation_type = world::ConstellationType::IntersectionEgoHasPriority;
775 : : }
776 : : }
777 : : else
778 : : {
779 [ # # ]: 0 : if (intersectionOtherRoute.route_planning_counter == objectRoute.route_planning_counter)
780 : : {
781 : 0 : constellation_type = world::ConstellationType::IntersectionEgoHasPriority;
782 : : }
783 : : else
784 : : {
785 : 0 : constellation_type = world::ConstellationType::IntersectionObjectHasPriority;
786 : : }
787 : : }
788 [ # # # # ]: 0 : getLogger()->debug(
789 : : "RssWorldModelCreation::appendIntersectionConstellation[{}->{}:{}]>> traffic light intersection intersection "
790 : : "route has green: {} constellation: {}",
791 [ # # # # ]: 0 : egoObject->getId(),
792 [ # # ]: 0 : otherObject->getId(),
793 : 0 : mWorldModelCreation.mRouteId,
794 : : intersectionRouteHasGreen,
795 : : constellation_type);
796 : : }
797 [ + - + + ]: 31 : else if (intersection->objectRouteCrossesLanesWithHigherPriority(intersectionOtherRoute))
798 : : {
799 [ + + ]: 8 : if (intersectionOtherRoute.route_planning_counter == objectRoute.route_planning_counter)
800 : : {
801 : 4 : constellation_type = world::ConstellationType::IntersectionObjectHasPriority;
802 : : }
803 : : else
804 : : {
805 : 4 : constellation_type = world::ConstellationType::IntersectionEgoHasPriority;
806 : : }
807 : : }
808 : : else
809 : : {
810 [ + + ]: 23 : if (intersectionOtherRoute.route_planning_counter == objectRoute.route_planning_counter)
811 : : {
812 : 19 : constellation_type = world::ConstellationType::IntersectionEgoHasPriority;
813 : : }
814 : : else
815 : : {
816 : 4 : constellation_type = world::ConstellationType::IntersectionObjectHasPriority;
817 : : }
818 : : }
819 : :
820 : 31 : world::RoadArea ego_vehicle_road;
821 : 31 : std::vector<ad::map::route::RoadSegment> egoIntersectingArea;
822 : : bool createIntersectionRoadAreaResult
823 [ + - ]: 31 : = createIntersectionRoadArea(egoRoute, intersection, egoObject, ego_vehicle_road, egoIntersectingArea);
824 [ - + ]: 31 : if (!createIntersectionRoadAreaResult)
825 : : {
826 [ # # # # ]: 0 : getLogger()->error("RssWorldModelCreator::appendIntersectionConstellation[{}->{}:{}]>> failed to create ego "
827 : : "vehicle road area from ego route {}",
828 [ # # # # ]: 0 : egoObject->getId(),
829 [ # # ]: 0 : otherObject->getId(),
830 : 0 : mWorldModelCreation.mRouteId,
831 : : egoRoute);
832 : 0 : return false;
833 : : }
834 : :
835 : 31 : world::RoadArea intersecting_road;
836 : 31 : std::vector<ad::map::route::RoadSegment> intersectingIntersectingArea;
837 [ + - ]: 31 : createIntersectionRoadAreaResult = createIntersectionRoadArea(
838 : : objectRoute, intersection, otherObject, intersecting_road, intersectingIntersectingArea);
839 [ - + ]: 31 : if (!createIntersectionRoadAreaResult)
840 : : {
841 [ # # # # ]: 0 : getLogger()->error("RssWorldModelCreator::appendIntersectionConstellation[{}->{}:{}]>> failed to create "
842 : : "intersecting road area from object route {}",
843 [ # # # # ]: 0 : egoObject->getId(),
844 [ # # ]: 0 : otherObject->getId(),
845 : 0 : mWorldModelCreation.mRouteId,
846 : : objectRoute);
847 : 0 : return false;
848 : : }
849 : :
850 [ + - + - ]: 62 : getLogger()->trace("RssWorldModelCreation::appendIntersectionConstellation[{}->{}:{}]>> constellation {} Before "
851 : : "exact intersection area\n"
852 : : " ego route {} -> ego road area {} -> ego intersecting area {}\n"
853 : : " other route {} -> intersection road area {} -> intersecting intersecting area",
854 [ + - + - ]: 31 : egoObject->getId(),
855 [ + - ]: 62 : otherObject->getId(),
856 : 31 : mWorldModelCreation.mRouteId,
857 : : constellation_type,
858 : : egoRoute,
859 : : ego_vehicle_road,
860 : : egoIntersectingArea,
861 : : objectRoute,
862 : : intersecting_road,
863 : : intersectingIntersectingArea);
864 : :
865 [ + - ]: 31 : auto const intersectionAreaActuallyExists = updateExactIntersectionArea(
866 : : ego_vehicle_road, egoIntersectingArea, intersecting_road, intersectingIntersectingArea);
867 [ + - ]: 31 : if (intersectionAreaActuallyExists)
868 : : {
869 [ + - + - ]: 62 : getLogger()->debug("RssWorldModelCreation::appendIntersectionConstellation[{}->{}:{}]>> constellation {}\n"
870 : : " ego road area {}\n intersection road area {}",
871 [ + - + - ]: 31 : egoObject->getId(),
872 [ + - ]: 62 : otherObject->getId(),
873 : 31 : mWorldModelCreation.mRouteId,
874 : : constellation_type,
875 : : ego_vehicle_road,
876 : : intersecting_road);
877 : :
878 [ + - ]: 31 : return appendConstellation(constellation_type, egoObject, ego_vehicle_road, otherObject, intersecting_road);
879 : : }
880 : : else
881 : : {
882 [ # # # # ]: 0 : getLogger()->debug(
883 : : "RssWorldModelCreation::appendIntersectionConstellation[{}->{}:{}]>> constellation {}"
884 : : " Detailed road areas dodn't actually intersect with each other. Appending NotRelevant constellation",
885 [ # # # # ]: 0 : egoObject->getId(),
886 [ # # ]: 0 : otherObject->getId(),
887 : 0 : mWorldModelCreation.mRouteId,
888 : : constellation_type);
889 : :
890 [ # # ]: 0 : return appendNotRelevantConstellation(egoRoute, egoObject, otherObject);
891 : : }
892 : 31 : }
893 : :
894 : 3 : bool RssWorldModelCreator::appendRoadBoundaryConstellations(::ad::map::route::FullRoute const &route,
895 : : RssObjectConversion::ConstPtr iEgoObject)
896 : : {
897 [ - + ]: 3 : if (!bool(iEgoObject))
898 : : {
899 [ # # # # ]: 0 : getLogger()->error("RssWorldModelCreator::appendRoadBoundaryConstellations[:{}]>> ego object input is NULL",
900 [ # # ]: 0 : mWorldModelCreation.mRouteId);
901 : 0 : return false;
902 : : }
903 [ + - ]: 3 : auto egoObject = std::make_shared<RssObjectConversion>(*iEgoObject);
904 [ - + ]: 3 : if (!bool(egoObject))
905 : : {
906 [ # # # # ]: 0 : getLogger()->error(
907 : : "RssWorldModelCreator::appendRoadBoundaryConstellations[{}:{}]>> failed to create copy of RssObjectConversion",
908 [ # # # # ]: 0 : iEgoObject->getId(),
909 : 0 : mWorldModelCreation.mRouteId);
910 : 0 : return false;
911 : : }
912 : :
913 : 3 : world::RoadArea ego_vehicle_road;
914 [ + - + - : 6 : bool createRoadAreaResult = createRoadArea(Route(route), ::ad::map::lane::LaneIdSet(), {egoObject}, ego_vehicle_road);
+ + - - ]
915 [ - + ]: 3 : if (!createRoadAreaResult)
916 : : {
917 [ # # # # ]: 0 : getLogger()->error("RssWorldModelCreator::appendRoadBoundaryConstellations[{}:{}]>> failed to create ego vehicle "
918 : : "road area from route {}",
919 [ # # # # ]: 0 : egoObject->getId(),
920 : 0 : mWorldModelCreation.mRouteId,
921 : : route);
922 : 0 : return false;
923 : : }
924 : :
925 [ + - + - ]: 6 : getLogger()->debug("RssWorldModelCreator::appendRoadBoundaryConstellations[{}:{}]>> ego road area {}",
926 [ + - + - ]: 6 : egoObject->getId(),
927 : 3 : mWorldModelCreation.mRouteId,
928 : : ego_vehicle_road);
929 : :
930 [ + - ]: 3 : world::RssDynamics staticDynamics;
931 : 3 : staticDynamics.alpha_lat.accel_max = ::ad::physics::Acceleration(0.);
932 : 3 : staticDynamics.alpha_lat.brake_min = ::ad::physics::Acceleration(-0.01);
933 : 3 : staticDynamics.alpha_lon.accel_max = ::ad::physics::Acceleration(0.);
934 : 3 : staticDynamics.alpha_lon.brake_max = ::ad::physics::Acceleration(-0.01);
935 : 3 : staticDynamics.alpha_lon.brake_min = ::ad::physics::Acceleration(-0.01);
936 : 3 : staticDynamics.alpha_lon.brake_min_correct = ::ad::physics::Acceleration(-0.01);
937 : 3 : staticDynamics.lateral_fluctuation_margin = ::ad::physics::Distance(0.0);
938 : 3 : staticDynamics.response_time = ::ad::physics::Duration(0.01);
939 : 3 : staticDynamics.unstructured_settings.pedestrian_turning_radius = ad::physics::Distance(0.);
940 : 3 : staticDynamics.unstructured_settings.drive_away_max_angle = ad::physics::Angle(0.);
941 : 3 : staticDynamics.unstructured_settings.vehicle_yaw_rate_change = ad::physics::AngularAcceleration(0.);
942 : 3 : staticDynamics.unstructured_settings.vehicle_min_radius = ad::physics::Distance(0.);
943 : 3 : staticDynamics.unstructured_settings.vehicle_trajectory_calculation_step = ad::physics::Duration(0.);
944 : :
945 : 3 : world::OccupiedRegionVector rightBorderOccupiedRegions;
946 : 3 : world::OccupiedRegionVector leftBorderOccupiedRegions;
947 : :
948 [ + + ]: 6 : for (auto const &roadSegment : route.road_segments)
949 : : {
950 : 3 : auto &rightmostLane = roadSegment.drivable_lane_segments.front();
951 : :
952 [ + - ]: 3 : world::OccupiedRegion region;
953 : 3 : region.lon_range.minimum = ::ad::physics::ParametricValue(0.);
954 : 3 : region.lon_range.maximum = ::ad::physics::ParametricValue(1.);
955 : 3 : region.lat_range.minimum = ::ad::physics::ParametricValue(0.999);
956 : 3 : region.lat_range.maximum = ::ad::physics::ParametricValue(1.);
957 : 3 : region.segment_id = world::LaneSegmentId(rightmostLane.lane_interval.lane_id.mLaneId);
958 [ + - ]: 3 : rightBorderOccupiedRegions.push_back(region);
959 : :
960 : 3 : auto &leftmostLane = roadSegment.drivable_lane_segments.back();
961 : :
962 : 3 : region.lon_range.minimum = ::ad::physics::ParametricValue(0.);
963 : 3 : region.lon_range.maximum = ::ad::physics::ParametricValue(1.);
964 : 3 : region.lat_range.minimum = ::ad::physics::ParametricValue(0.);
965 : 3 : region.lat_range.maximum = ::ad::physics::ParametricValue(0.001);
966 : 3 : region.segment_id = world::LaneSegmentId(leftmostLane.lane_interval.lane_id.mLaneId);
967 [ + - ]: 3 : leftBorderOccupiedRegions.push_back(region);
968 : : }
969 : :
970 [ + - ]: 3 : ::ad::map::match::ENUObjectPosition rightBorderPosition;
971 : 3 : ::ad::physics::ParametricValue rightBorderOffsetLat;
972 : 3 : auto const rightBorderInterval = route.road_segments.front().drivable_lane_segments.front().lane_interval;
973 [ + - - + ]: 3 : if (::ad::map::route::isRouteDirectionPositive(rightBorderInterval))
974 : : {
975 : 0 : rightBorderOffsetLat = ::ad::physics::ParametricValue(1.);
976 : : }
977 : : else
978 : : {
979 : 3 : rightBorderOffsetLat = ::ad::physics::ParametricValue(0.);
980 : : }
981 [ + - ]: 3 : auto const rightBorderParaPoint = getIntervalStart(rightBorderInterval);
982 [ + - ]: 3 : rightBorderPosition.enu_reference_point = ::ad::map::access::getENUReferencePoint();
983 [ + - ]: 3 : rightBorderPosition.center_point = ::ad::map::lane::getENULanePoint(rightBorderParaPoint, rightBorderOffsetLat);
984 : : rightBorderPosition.heading
985 [ + - ]: 3 : = ::ad::map::lane::getLaneENUHeading(rightBorderParaPoint, rightBorderPosition.enu_reference_point);
986 : 3 : rightBorderPosition.dimension.height = ::ad::physics::Distance(0.1);
987 : 3 : rightBorderPosition.dimension.length = ::ad::physics::Distance(0.1);
988 : 3 : rightBorderPosition.dimension.width = ::ad::physics::Distance(0.1);
989 : :
990 [ + - ]: 3 : RssObjectData rightBorderObjectData;
991 : 3 : rightBorderObjectData.id = structured::getRightBorderObjectId();
992 : 3 : rightBorderObjectData.type = world::ObjectType::ArtificialObject;
993 : 3 : rightBorderObjectData.match_object.enu_position = rightBorderPosition;
994 : 3 : rightBorderObjectData.speed_range.minimum = ::ad::physics::Speed(0);
995 : 3 : rightBorderObjectData.speed_range.maximum = ::ad::physics::Speed(0);
996 : 3 : rightBorderObjectData.yaw_rate = ::ad::physics::AngularVelocity(0.);
997 : 3 : rightBorderObjectData.steering_angle = ::ad::physics::Angle(0.);
998 : 3 : rightBorderObjectData.rss_dynamics = staticDynamics;
999 : :
1000 [ + - ]: 3 : auto rightBorderObject = std::make_shared<RssObjectConversion>(rightBorderObjectData, rightBorderOccupiedRegions);
1001 [ - + ]: 3 : if (!bool(rightBorderObject))
1002 : : {
1003 [ # # # # ]: 0 : getLogger()->error("RssWorldModelCreator::appendRoadBoundaryConstellations[{}:{}]>> failed to create copy of right "
1004 : : "border RssObjectConversion",
1005 [ # # # # ]: 0 : egoObject->getId(),
1006 : 0 : mWorldModelCreation.mRouteId);
1007 : 0 : return false;
1008 : : }
1009 : :
1010 [ + - ]: 3 : ::ad::map::match::ENUObjectPosition leftBorderPosition;
1011 : 3 : ::ad::physics::ParametricValue leftBorderOffsetLat;
1012 : 3 : auto const leftBorderInterval = route.road_segments.front().drivable_lane_segments.back().lane_interval;
1013 [ + - - + ]: 3 : if (::ad::map::route::isRouteDirectionPositive(leftBorderInterval))
1014 : : {
1015 : 0 : leftBorderOffsetLat = ::ad::physics::ParametricValue(0.);
1016 : : }
1017 : : else
1018 : : {
1019 : 3 : leftBorderOffsetLat = ::ad::physics::ParametricValue(1.);
1020 : : }
1021 [ + - ]: 3 : auto const leftBorderParaPoint = getIntervalStart(leftBorderInterval);
1022 [ + - ]: 3 : leftBorderPosition.enu_reference_point = ::ad::map::access::getENUReferencePoint();
1023 [ + - ]: 3 : leftBorderPosition.center_point = ::ad::map::lane::getENULanePoint(leftBorderParaPoint, leftBorderOffsetLat);
1024 : : leftBorderPosition.heading
1025 [ + - ]: 3 : = ::ad::map::lane::getLaneENUHeading(leftBorderParaPoint, leftBorderPosition.enu_reference_point);
1026 : 3 : leftBorderPosition.dimension.height = ::ad::physics::Distance(0.1);
1027 : 3 : leftBorderPosition.dimension.length = ::ad::physics::Distance(0.1);
1028 : 3 : leftBorderPosition.dimension.width = ::ad::physics::Distance(0.1);
1029 : :
1030 [ + - ]: 3 : RssObjectData leftBorderObjectData;
1031 : 3 : leftBorderObjectData.id = structured::getLeftBorderObjectId();
1032 : 3 : leftBorderObjectData.type = world::ObjectType::ArtificialObject;
1033 : 3 : leftBorderObjectData.match_object.enu_position = leftBorderPosition;
1034 : 3 : leftBorderObjectData.speed_range.minimum = ::ad::physics::Speed(0);
1035 : 3 : leftBorderObjectData.speed_range.maximum = ::ad::physics::Speed(0);
1036 : 3 : leftBorderObjectData.yaw_rate = ::ad::physics::AngularVelocity(0.);
1037 : 3 : leftBorderObjectData.steering_angle = ::ad::physics::Angle(0.);
1038 : 3 : leftBorderObjectData.rss_dynamics = staticDynamics;
1039 : :
1040 [ + - ]: 3 : auto leftBorderObject = std::make_shared<RssObjectConversion>(leftBorderObjectData, leftBorderOccupiedRegions);
1041 [ - + ]: 3 : if (!bool(leftBorderObject))
1042 : : {
1043 [ # # # # ]: 0 : getLogger()->error("RssWorldModelCreator::appendRoadBoundaryConstellations[{}:{}]>> failed to create copy of left "
1044 : : "border RssObjectConversion",
1045 [ # # # # ]: 0 : egoObject->getId(),
1046 : 0 : mWorldModelCreation.mRouteId);
1047 : 0 : return false;
1048 : : }
1049 : :
1050 : 6 : auto result = appendConstellation(
1051 [ + - ]: 9 : world::ConstellationType::SameDirection, egoObject, ego_vehicle_road, rightBorderObject, world::RoadArea());
1052 : :
1053 : 6 : result &= appendConstellation(
1054 [ + - ]: 9 : world::ConstellationType::SameDirection, egoObject, ego_vehicle_road, leftBorderObject, world::RoadArea());
1055 : :
1056 : 3 : return result;
1057 : 3 : }
1058 : :
1059 : 435 : bool RssWorldModelCreator::appendConstellation(world::ConstellationType const &constellation_type,
1060 : : RssObjectConversion::ConstPtr egoObject,
1061 : : world::RoadArea const &egoRoad,
1062 : : RssObjectConversion::ConstPtr otherObject,
1063 : : world::RoadArea const &intersecting_road)
1064 : : {
1065 [ + - - + : 435 : if (!bool(egoObject) || !bool(otherObject))
- + ]
1066 : : {
1067 [ # # # # ]: 0 : getLogger()->error("RssWorldModelCreator::appendConstellation[:{}]>> ego/other object input is NULL",
1068 [ # # ]: 0 : mWorldModelCreation.mRouteId);
1069 : 0 : return false;
1070 : : }
1071 : :
1072 [ + - ]: 435 : world::Constellation constellation;
1073 : 435 : constellation.constellation_type = constellation_type;
1074 [ + - + - ]: 435 : constellation.ego_vehicle = egoObject->getRssObject();
1075 [ + - ]: 435 : constellation.ego_vehicle_rss_dynamics = egoObject->getRssDynamics();
1076 [ + - ]: 435 : constellation.ego_vehicle_road = egoRoad;
1077 [ + - ]: 435 : constellation.intersecting_road = intersecting_road;
1078 [ + - + - ]: 435 : constellation.object = otherObject->getRssObject();
1079 [ + - ]: 435 : constellation.object_rss_dynamics = otherObject->getRssDynamics();
1080 : :
1081 [ + - + - ]: 870 : getLogger()->trace("RssWorldModelCreator::appendConstellation[{}->{}:{}]>> object {}",
1082 [ + - + - ]: 435 : egoObject->getId(),
1083 [ + - ]: 870 : otherObject->getId(),
1084 : 435 : mWorldModelCreation.mRouteId,
1085 : : constellation.object);
1086 [ + - + - ]: 870 : getLogger()->trace("RssWorldModelCreator::appendConstellation[{}->{}:{}]>> ego {}",
1087 [ + - + - ]: 435 : egoObject->getId(),
1088 [ + - ]: 870 : otherObject->getId(),
1089 : 435 : mWorldModelCreation.mRouteId,
1090 : : constellation.ego_vehicle);
1091 : :
1092 [ + + ]: 435 : if (constellation_type != world::ConstellationType::NotRelevant)
1093 : : {
1094 : : // relevant constellations have to consider some more checks to filter out unsupported constellations
1095 : 734 : if ((constellation_type != world::ConstellationType::Unstructured)
1096 [ + + + - : 367 : && (constellation.ego_vehicle.occupied_regions.empty() || constellation.object.occupied_regions.empty()))
- + - + ]
1097 : : {
1098 [ # # ]: 0 : if (constellation.object.occupied_regions.empty())
1099 : : {
1100 [ # # # # ]: 0 : getLogger()->info("RssWorldModelCreator::appendConstellation[{}->{}:{}]>> object not on route. Structured "
1101 : : "constellation not valid. Dropping. {}",
1102 [ # # # # ]: 0 : egoObject->getId(),
1103 [ # # ]: 0 : otherObject->getId(),
1104 : 0 : mWorldModelCreation.mRouteId,
1105 : : constellation);
1106 : : }
1107 : : else
1108 : : {
1109 [ # # # # ]: 0 : getLogger()->info("RssWorldModelCreator::appendConstellation[{}->{}:{}]>> ego not on route. Structured "
1110 : : "constellation not valid. Dropping. {}",
1111 [ # # # # ]: 0 : egoObject->getId(),
1112 [ # # ]: 0 : otherObject->getId(),
1113 : 0 : mWorldModelCreation.mRouteId,
1114 : : constellation);
1115 : : }
1116 : 0 : return false;
1117 : : }
1118 [ + - - + ]: 367 : else if (!egoObject->isOriginalSpeedAcceptable())
1119 : : {
1120 [ # # # # ]: 0 : getLogger()->error(
1121 : : "RssWorldModelCreator::appendConstellation[{}->{}:{}]>> ego original speed {} is not acceptable. Dropping. {}",
1122 [ # # # # ]: 0 : egoObject->getId(),
1123 [ # # ]: 0 : otherObject->getId(),
1124 : 0 : mWorldModelCreation.mRouteId,
1125 : : egoObject->getOriginalObjectSpeed(),
1126 : : constellation);
1127 : 0 : return false;
1128 : : }
1129 [ + - - + ]: 367 : else if (!otherObject->isOriginalSpeedAcceptable())
1130 : : {
1131 [ # # # # ]: 0 : getLogger()->error("RssWorldModelCreator::appendConstellation[{}->{}:{}]>> other original speed {} is not "
1132 : : "acceptable. Dropping. {}",
1133 [ # # # # ]: 0 : egoObject->getId(),
1134 [ # # ]: 0 : otherObject->getId(),
1135 : 0 : mWorldModelCreation.mRouteId,
1136 : : otherObject->getOriginalObjectSpeed(),
1137 : : constellation);
1138 : 0 : return false;
1139 : : }
1140 : : }
1141 : :
1142 [ + - + - ]: 435 : if (withinValidInputRange(constellation))
1143 : : {
1144 [ + - ]: 435 : return mWorldModelCreation.appendConstellationToWorldModel(constellation);
1145 : : }
1146 : : else
1147 : : {
1148 [ # # # # ]: 0 : getLogger()->error("RssWorldModelCreator::appendConstellation[{}->{}:{}]>> constellation not valid {}",
1149 [ # # # # ]: 0 : egoObject->getId(),
1150 [ # # ]: 0 : otherObject->getId(),
1151 : 0 : mWorldModelCreation.mRouteId,
1152 : : constellation);
1153 : 0 : return false;
1154 : : }
1155 : 435 : }
1156 : :
1157 : 2 : bool RssWorldModelCreator::appendUnstructuredConstellation(RssObjectConversion::ConstPtr iEgoObject,
1158 : : RssObjectConversion::ConstPtr iOtherObject)
1159 : : {
1160 [ + - - + : 2 : if (!bool(iEgoObject) || !bool(iOtherObject))
- + ]
1161 : : {
1162 [ # # # # ]: 0 : getLogger()->error("RssWorldModelCreator::appendUnstructuredConstellation[:{}]>> ego/other object input is NULL",
1163 [ # # ]: 0 : mWorldModelCreation.mRouteId);
1164 : 0 : return false;
1165 : : }
1166 [ + - ]: 2 : auto egoObject = std::make_shared<RssObjectConversion>(*iEgoObject);
1167 [ - + ]: 2 : if (!bool(egoObject))
1168 : : {
1169 [ # # # # ]: 0 : getLogger()->error("RssWorldModelCreator::appendUnstructuredConstellation[{}->{}:{}]>> failed to create copy of "
1170 : : "ego RssObjectConversion",
1171 [ # # # # ]: 0 : iEgoObject->getId(),
1172 [ # # ]: 0 : iOtherObject->getId(),
1173 : 0 : mWorldModelCreation.mRouteId);
1174 : 0 : return false;
1175 : : }
1176 [ + - ]: 2 : auto otherObject = std::make_shared<RssObjectConversion>(*iOtherObject);
1177 [ - + ]: 2 : if (!bool(otherObject))
1178 : : {
1179 [ # # # # ]: 0 : getLogger()->error(
1180 : : "RssWorldModelCreator::appendUnstructuredConstellation[{}->{}:{}]>> failed to create copy of other "
1181 : : "RssObjectConversion",
1182 [ # # # # ]: 0 : iEgoObject->getId(),
1183 [ # # ]: 0 : iOtherObject->getId(),
1184 : 0 : mWorldModelCreation.mRouteId);
1185 : 0 : return false;
1186 : : }
1187 : :
1188 [ + - + - ]: 4 : getLogger()->debug("RssWorldModelCreator::appendUnstructuredConstellation[{}->{}:{}]>>",
1189 [ + - + - ]: 2 : egoObject->getId(),
1190 [ + - ]: 4 : otherObject->getId(),
1191 : 2 : mWorldModelCreation.mRouteId);
1192 : :
1193 : 4 : return appendConstellation(
1194 [ + - ]: 6 : world::ConstellationType::Unstructured, egoObject, world::RoadArea(), otherObject, world::RoadArea());
1195 : 2 : }
1196 : : } // namespace map
1197 : : } // namespace rss
1198 : : } // namespace ad
|
