Branch data Line data Source code
1 : : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
2 : : //
3 : : // Copyright (C) 2019-2021 Intel Corporation
4 : : //
5 : : // SPDX-License-Identifier: LGPL-2.1-only
6 : : //
7 : : // ----------------- END LICENSE BLOCK -----------------------------------
8 : :
9 : : #include "ad/rss/map/RssSceneCreator.hpp"
10 : : #include <ad/map/lane/LaneOperation.hpp>
11 : : #include <ad/rss/world/SceneValidInputRange.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 : 213 : RssSceneCreator::RssSceneCreator(RssSceneCreation::RestrictSpeedLimitMode const &restrictSpeedLimitMode,
28 : : ::ad::map::landmark::LandmarkIdSet const &greenTrafficLights,
29 : 213 : RssSceneCreation &sceneCreation)
30 : : : mGreenTrafficLights(greenTrafficLights)
31 : 213 : , mSceneCreation(sceneCreation)
32 : : {
33 [ - - + + ]: 213 : switch (restrictSpeedLimitMode)
34 : : {
35 : 0 : case RssSceneCreation::RestrictSpeedLimitMode::ExactSpeedLimit:
36 : 0 : mSpeedLimitFactor = 1.;
37 : 0 : break;
38 : 0 : case RssSceneCreation::RestrictSpeedLimitMode::IncreasedSpeedLimit5:
39 : 0 : mSpeedLimitFactor = 1.05;
40 : 0 : break;
41 : 151 : case RssSceneCreation::RestrictSpeedLimitMode::IncreasedSpeedLimit10:
42 : 151 : mSpeedLimitFactor = 1.1;
43 : 151 : break;
44 : 62 : case RssSceneCreation::RestrictSpeedLimitMode::None:
45 : : default:
46 : 62 : mSpeedLimitFactor = 0.;
47 : 62 : break;
48 : : }
49 : 213 : }
50 : 3 : RssSceneCreator::RssSceneCreator(RssSceneCreation &sceneCreation)
51 : : : mSpeedLimitFactor(0.)
52 : : , mGreenTrafficLights()
53 : 3 : , mSceneCreation(sceneCreation)
54 : : {
55 : 3 : }
56 : :
57 : 304 : ::ad::rss::world::RoadArea RssSceneCreator::createRoadArea(::ad::map::route::FullRoute const &route,
58 : : ::ad::map::route::RouteLaneOffset const minLaneOffset,
59 : : ::ad::map::route::RouteLaneOffset const maxLaneOffset,
60 : : ::ad::map::lane::LaneIdSet const &intersectionLanes,
61 : : std::vector<RssObjectConversion::Ptr> objects)
62 : : {
63 : 304 : ::ad::rss::world::RoadArea area;
64 : 304 : bool intersectionEntered = false;
65 : 304 : bool intersectionLeft = false;
66 [ + + ]: 900 : for (auto roadSegmentIter = route.roadSegments.begin(); roadSegmentIter != route.roadSegments.end();
67 : 596 : roadSegmentIter++)
68 : : {
69 : 658 : ::ad::rss::world::RoadSegment rssRoadSegment;
70 : : // RoadSegments are in strict order from right to left
71 : : // RSS order is vice-versa
72 : 1606 : for (auto laneSegmentIter = roadSegmentIter->drivableLaneSegments.crbegin();
73 [ + - + + ]: 1606 : laneSegmentIter != roadSegmentIter->drivableLaneSegments.crend();
74 : : laneSegmentIter++)
75 : : {
76 [ + - + - : 1010 : if ((laneSegmentIter->routeLaneOffset >= minLaneOffset) && (laneSegmentIter->routeLaneOffset <= maxLaneOffset))
+ - + + +
+ ]
77 : : {
78 [ + - ]: 828 : ::ad::rss::world::LaneSegment rssLaneSegment;
79 [ + - ]: 828 : rssLaneSegment.id = ::ad::rss::world::LaneSegmentId(laneSegmentIter->laneInterval.laneId);
80 [ + - + - : 828 : if (intersectionLanes.count(laneSegmentIter->laneInterval.laneId) > 0)
+ + ]
81 : : {
82 : 74 : rssLaneSegment.type = ::ad::rss::world::LaneSegmentType::Intersection;
83 : 74 : intersectionEntered = true;
84 : : }
85 : : else
86 : : {
87 [ + + ]: 754 : if (intersectionEntered)
88 : : {
89 : : // route is leaving the intersection again, stop
90 : 62 : intersectionLeft = true;
91 : 62 : break;
92 : : }
93 : 692 : rssLaneSegment.type = ::ad::rss::world::LaneSegmentType::Normal;
94 : : }
95 [ + - + + ]: 766 : if (laneSegmentIter->laneInterval.wrongWay)
96 : : {
97 : 170 : rssLaneSegment.drivingDirection = ::ad::rss::world::LaneDrivingDirection::Negative;
98 : : }
99 : : else
100 : : {
101 : 596 : rssLaneSegment.drivingDirection = ::ad::rss::world::LaneDrivingDirection::Positive;
102 : : }
103 [ + - + - ]: 766 : getMetricRanges(laneSegmentIter->laneInterval, rssLaneSegment.length, rssLaneSegment.width);
104 : :
105 [ + - + - ]: 1532 : auto const speedLimitRestrictions = getSpeedLimits(laneSegmentIter->laneInterval);
106 [ + + ]: 2042 : for (auto object : objects)
107 : : {
108 [ + + ]: 2552 : for (auto const &speedLimitRestriction : speedLimitRestrictions)
109 : : {
110 [ + - + - ]: 1276 : object->updateSpeedLimit(speedLimitRestriction.speedLimit * mSpeedLimitFactor);
111 : : }
112 [ + - + - ]: 1276 : object->laneIntervalAdded(laneSegmentIter->laneInterval);
113 : : }
114 : :
115 [ + - ]: 766 : rssRoadSegment.push_back(rssLaneSegment);
116 : : }
117 : : }
118 [ + + ]: 658 : if (intersectionLeft)
119 : : {
120 : 62 : break;
121 : : }
122 [ + - ]: 596 : area.push_back(rssRoadSegment);
123 : : }
124 [ + + ]: 797 : for (auto object : objects)
125 : : {
126 [ + - ]: 493 : object->updateVelocityOnRoute(route);
127 : : }
128 : 304 : return area;
129 : : }
130 : :
131 : 56 : bool RssSceneCreator::appendNotRelevantScene(::ad::map::route::FullRoute const &route,
132 : : RssObjectConversion::ConstPtr iEgoObject,
133 : : RssObjectConversion::ConstPtr iOtherObject)
134 : : {
135 [ + - - + : 56 : if (!bool(iEgoObject) || !bool(iOtherObject))
- + ]
136 : : {
137 [ # # # # ]: 0 : getLogger()->error("RssSceneCreator::appendNotRelevantScene[]>> ego/other object input is NULL");
138 : 0 : return false;
139 : : }
140 [ + - ]: 112 : auto egoObject = std::make_shared<RssObjectConversion>(*iEgoObject);
141 [ - + ]: 56 : if (!bool(egoObject))
142 : : {
143 [ # # # # ]: 0 : getLogger()->error("RssSceneCreator::appendNotRelevantScene[{}]>> failed to create copy of ego RssObjectConversion",
144 [ # # ]: 0 : iOtherObject->getId());
145 : 0 : return false;
146 : : }
147 [ + - ]: 112 : auto otherObject = std::make_shared<RssObjectConversion>(*iOtherObject);
148 [ - + ]: 56 : if (!bool(otherObject))
149 : : {
150 [ # # # # ]: 0 : getLogger()->error(
151 : : "RssSceneCreator::appendNotRelevantScene[{}]>> failed to create copy of other RssObjectConversion",
152 [ # # ]: 0 : iOtherObject->getId());
153 : 0 : return false;
154 : : }
155 : :
156 : : // in the end this scene is only for convenience to get to know that we considered it and e.g. visualize it
157 : : // It could be left out completely
158 : 56 : ::ad::rss::world::RoadArea egoVehicleRoad;
159 [ + + ]: 56 : if (!route.roadSegments.empty())
160 : : {
161 : : // in case the ego route is actually available, we can try to fill the route as usual to have some hint where the
162 : : // ego is driving
163 : : egoVehicleRoad
164 [ + - + - : 100 : = createRoadArea(route, route.minLaneOffset, route.maxLaneOffset, ::ad::map::lane::LaneIdSet(), {egoObject});
+ + - - ]
165 : : }
166 : :
167 [ + - + - ]: 112 : getLogger()->debug("RssSceneCreator::appendNotRelevantScene[{}]>>situation {}",
168 [ + - ]: 56 : otherObject->getId(),
169 : 112 : ::ad::rss::situation::SituationType::NotRelevant);
170 : :
171 [ + - ]: 112 : return appendScene(::ad::rss::situation::SituationType::NotRelevant,
172 : : egoObject,
173 : : egoVehicleRoad,
174 : : otherObject,
175 : 168 : ::ad::rss::world::RoadArea());
176 : : }
177 : :
178 : 189 : ::ad::rss::world::RoadArea RssSceneCreator::createNonIntersectionRoadArea(::ad::map::route::FullRoute const &route,
179 : : std::vector<RssObjectConversion::Ptr> objects)
180 : : {
181 : : // take only the relevant part of the route, the two vehicles are driving on
182 : 189 : ::ad::map::route::RouteLaneOffset const minLaneOffset = std::min(0, route.destinationLaneOffset);
183 : 189 : ::ad::map::route::RouteLaneOffset const maxLaneOffset = std::max(0, route.destinationLaneOffset);
184 : :
185 : : // no intersections on route
186 [ + - ]: 378 : return createRoadArea(route, minLaneOffset, maxLaneOffset, ::ad::map::lane::LaneIdSet(), objects);
187 : : }
188 : :
189 : 189 : bool RssSceneCreator::appendNonIntersectionScene(::ad::map::route::ConnectingRoute const &connectingRoute,
190 : : ::ad::rss::situation::SituationType const &situationType,
191 : : RssObjectConversion::ConstPtr iEgoObject,
192 : : RssObjectConversion::ConstPtr iOtherObject)
193 : : {
194 [ + - - + : 189 : if (!bool(iEgoObject) || !bool(iOtherObject))
- + ]
195 : : {
196 [ # # # # ]: 0 : getLogger()->error("RssSceneCreator::appendNonIntersectionScene[]>> ego/other object input is NULL");
197 : 0 : return false;
198 : : }
199 [ + - ]: 378 : auto egoObject = std::make_shared<RssObjectConversion>(*iEgoObject);
200 [ - + ]: 189 : if (!bool(egoObject))
201 : : {
202 [ # # # # ]: 0 : getLogger()->error(
203 : : "RssSceneCreator::appendNonIntersectionScene[{}]>> failed to create copy of ego RssObjectConversion",
204 [ # # ]: 0 : iOtherObject->getId());
205 : 0 : return false;
206 : : }
207 [ + - ]: 378 : auto otherObject = std::make_shared<RssObjectConversion>(*iOtherObject);
208 [ - + ]: 189 : if (!bool(otherObject))
209 : : {
210 [ # # # # ]: 0 : getLogger()->error(
211 : : "RssSceneCreator::appendNonIntersectionScene[{}]>> failed to create copy of other RssObjectConversion",
212 [ # # ]: 0 : iOtherObject->getId());
213 : 0 : return false;
214 : : }
215 : :
216 : 378 : ::ad::rss::world::RoadArea egoVehicleRoad;
217 [ + + ]: 189 : if (!connectingRoute.routeA.roadSegments.empty())
218 : : {
219 [ + - + - : 429 : egoVehicleRoad = createNonIntersectionRoadArea(connectingRoute.routeA, {egoObject, otherObject});
+ + - - ]
220 : : }
221 [ + - ]: 46 : else if (!connectingRoute.routeB.roadSegments.empty())
222 : : {
223 [ + - + - : 138 : egoVehicleRoad = createNonIntersectionRoadArea(connectingRoute.routeB, {egoObject, otherObject});
+ + - - ]
224 : : }
225 : : else
226 : : {
227 [ # # # # ]: 0 : getLogger()->error("RssSceneCreator::appendNonIntersectionScene[{}]>> situation {} expected either routeA or "
228 : : "routeB not to be empty {}",
229 [ # # ]: 0 : otherObject->getId(),
230 : : situationType,
231 : : connectingRoute);
232 : 0 : return false;
233 : : }
234 [ + - + - ]: 378 : getLogger()->debug("RssSceneCreator::appendNonIntersectionScene[{}]>> situation {}\n road area {}",
235 [ + - ]: 378 : otherObject->getId(),
236 : : situationType,
237 : : egoVehicleRoad);
238 : :
239 [ + - ]: 189 : return appendScene(situationType, egoObject, egoVehicleRoad, otherObject, ::ad::rss::world::RoadArea());
240 : : }
241 : :
242 : 0 : ::ad::rss::world::RoadArea RssSceneCreator::createMergingRoadArea(::ad::map::route::FullRoute const &route,
243 : : RssObjectConversion::Ptr object)
244 : : {
245 : : // the last road segment of the merging road is the intersecting one
246 : 0 : ::ad::map::lane::LaneIdSet intersectionLanes;
247 : 0 : for (auto laneSegmentIter = route.roadSegments.back().drivableLaneSegments.begin();
248 [ # # ]: 0 : laneSegmentIter != route.roadSegments.back().drivableLaneSegments.end();
249 : 0 : laneSegmentIter++)
250 : : {
251 [ # # ]: 0 : intersectionLanes.insert(laneSegmentIter->laneInterval.laneId);
252 : : }
253 : : // @todo: there might be more lanes in the merging roads that overlap, so add these also to the intersection lanes!
254 : :
255 : : // take the whole route
256 [ # # # # : 0 : return createRoadArea(route, route.minLaneOffset, route.maxLaneOffset, intersectionLanes, {object});
# # # # ]
257 : : }
258 : :
259 : 0 : bool RssSceneCreator::appendMergingScene(::ad::map::route::ConnectingRoute const &connectingRoute,
260 : : ::ad::rss::situation::SituationType const &situationType,
261 : : RssObjectConversion::ConstPtr iEgoObject,
262 : : RssObjectConversion::ConstPtr iOtherObject)
263 : : {
264 [ # # # # : 0 : if (!bool(iEgoObject) || !bool(iOtherObject))
# # ]
265 : : {
266 [ # # # # ]: 0 : getLogger()->error("RssSceneCreator::appendMergingScene[]>> ego/other object input is NULL");
267 : 0 : return false;
268 : : }
269 [ # # # # : 0 : if (connectingRoute.routeA.roadSegments.empty() || connectingRoute.routeB.roadSegments.empty())
# # ]
270 : : {
271 [ # # # # ]: 0 : getLogger()->error("RssSceneCreator::appendMergingScene[{}]>> situation {} connecting route empty {}",
272 [ # # ]: 0 : iOtherObject->getId(),
273 : : situationType,
274 : : connectingRoute);
275 : 0 : return false;
276 : : }
277 : :
278 [ # # ]: 0 : auto egoObject = std::make_shared<RssObjectConversion>(*iEgoObject);
279 [ # # ]: 0 : if (!bool(egoObject))
280 : : {
281 [ # # # # ]: 0 : getLogger()->error("RssSceneCreator::appendMergingScene[{}]>> failed to create copy of ego RssObjectConversion",
282 [ # # ]: 0 : iOtherObject->getId());
283 : 0 : return false;
284 : : }
285 [ # # ]: 0 : auto otherObject = std::make_shared<RssObjectConversion>(*iOtherObject);
286 [ # # ]: 0 : if (!bool(otherObject))
287 : : {
288 [ # # # # ]: 0 : getLogger()->error("RssSceneCreator::appendMergingScene[{}]>> failed to create copy of other RssObjectConversion",
289 [ # # ]: 0 : iOtherObject->getId());
290 : 0 : return false;
291 : : }
292 : :
293 [ # # ]: 0 : auto const egoVehicleRoad = createMergingRoadArea(connectingRoute.routeA, egoObject);
294 [ # # ]: 0 : auto const intersectingRoad = createMergingRoadArea(connectingRoute.routeB, otherObject);
295 [ # # # # ]: 0 : getLogger()->debug("RssSceneCreator::appendMergingScene[{}]>> situation {}\n"
296 : : " ego road area {}\n intersection road area {}",
297 [ # # ]: 0 : otherObject->getId(),
298 : : situationType,
299 : : egoVehicleRoad,
300 : : intersectingRoad);
301 [ # # ]: 0 : return appendScene(situationType, egoObject, egoVehicleRoad, otherObject, intersectingRoad);
302 : : }
303 : :
304 : : ::ad::rss::world::RoadArea
305 : 62 : RssSceneCreator::createIntersectionRoadArea(::ad::map::route::FullRoute const &route,
306 : : ::ad::map::intersection::IntersectionConstPtr intersection,
307 : : RssObjectConversion::Ptr object)
308 : : {
309 : : // only the lanes of the intersection under consideration are marked as intersection;
310 [ + - + - : 124 : return createRoadArea(route, route.minLaneOffset, route.maxLaneOffset, intersection->internalLanes(), {object});
+ - + + -
- ]
311 : : }
312 : :
313 : 31 : bool RssSceneCreator::appendIntersectionScene(::ad::map::intersection::IntersectionPtr intersection,
314 : : ::ad::map::route::FullRoute const &egoRoute,
315 : : ::ad::map::route::FullRoute const &objectRoute,
316 : : ::ad::map::route::FullRoute const &intersectionOtherRoute,
317 : : RssObjectConversion::ConstPtr iEgoObject,
318 : : RssObjectConversion::ConstPtr iOtherObject)
319 : : {
320 [ + - - + : 31 : if (!bool(iEgoObject) || !bool(iOtherObject))
- + ]
321 : : {
322 [ # # # # ]: 0 : getLogger()->error("RssSceneCreator::appendIntersectionScene[]>> ego/other object input is NULL");
323 : 0 : return false;
324 : : }
325 [ + - ]: 62 : auto egoObject = std::make_shared<RssObjectConversion>(*iEgoObject);
326 [ - + ]: 31 : if (!bool(egoObject))
327 : : {
328 [ # # # # ]: 0 : getLogger()->error(
329 : : "RssSceneCreator::appendIntersectionScene[{}]>> failed to create copy of ego RssObjectConversion",
330 [ # # ]: 0 : iOtherObject->getId());
331 : 0 : return false;
332 : : }
333 [ + - ]: 62 : auto otherObject = std::make_shared<RssObjectConversion>(*iOtherObject);
334 [ - + ]: 31 : if (!bool(otherObject))
335 : : {
336 [ # # # # ]: 0 : getLogger()->error(
337 : : "RssSceneCreator::appendIntersectionScene[{}]>> failed to create copy of other RssObjectConversion",
338 [ # # ]: 0 : iOtherObject->getId());
339 : 0 : return false;
340 : : }
341 : :
342 : 31 : auto situationType = ::ad::rss::situation::SituationType::IntersectionObjectHasPriority;
343 [ + - - + ]: 31 : if (intersection->intersectionType() == ::ad::map::intersection::IntersectionType::TrafficLight)
344 : : {
345 : : // in case no traffic light actually modeled (map error), we assume others have always priority
346 : 0 : bool intersectionRouteHasGreen = true;
347 [ # # # # ]: 0 : if (intersection->applicableTrafficLights().empty())
348 : : {
349 [ # # # # ]: 0 : getLogger()->warn("RssSceneCreation::appendIntersectionScene[{}]>> traffic light intersection has no "
350 : : "applicable traffic lights {}",
351 [ # # ]: 0 : otherObject->getId(),
352 [ # # # # ]: 0 : std::to_string(intersection->incomingLanesOnRoute()));
353 : 0 : intersectionRouteHasGreen = false;
354 : : }
355 [ # # # # ]: 0 : for (auto relevantTrafficLight : intersection->applicableTrafficLights())
356 : : {
357 [ # # # # ]: 0 : if (mGreenTrafficLights.find(relevantTrafficLight) == mGreenTrafficLights.end())
358 : : {
359 : : // if any relevant traffic light is not green, intersection route has no priority
360 : 0 : intersectionRouteHasGreen = false;
361 : 0 : break;
362 : : }
363 : : }
364 : :
365 [ # # # # : 0 : if (!intersectionRouteHasGreen || intersection->objectRouteCrossesLanesWithHigherPriority(intersectionOtherRoute))
# # # # ]
366 : : {
367 [ # # ]: 0 : if (intersectionOtherRoute.routePlanningCounter == objectRoute.routePlanningCounter)
368 : : {
369 : 0 : situationType = ::ad::rss::situation::SituationType::IntersectionObjectHasPriority;
370 : : }
371 : : else
372 : : {
373 : 0 : situationType = ::ad::rss::situation::SituationType::IntersectionEgoHasPriority;
374 : : }
375 : : }
376 : : else
377 : : {
378 [ # # ]: 0 : if (intersectionOtherRoute.routePlanningCounter == objectRoute.routePlanningCounter)
379 : : {
380 : 0 : situationType = ::ad::rss::situation::SituationType::IntersectionEgoHasPriority;
381 : : }
382 : : else
383 : : {
384 : 0 : situationType = ::ad::rss::situation::SituationType::IntersectionObjectHasPriority;
385 : : }
386 : : }
387 [ # # # # ]: 0 : getLogger()->debug("RssSceneCreation::appendIntersectionScene[{}]>> traffic light intersection intersection "
388 : : "route has green: {} situation: {}",
389 [ # # ]: 0 : otherObject->getId(),
390 : : intersectionRouteHasGreen,
391 : : situationType);
392 : : }
393 [ + - + + ]: 31 : else if (intersection->objectRouteCrossesLanesWithHigherPriority(intersectionOtherRoute))
394 : : {
395 [ + + ]: 8 : if (intersectionOtherRoute.routePlanningCounter == objectRoute.routePlanningCounter)
396 : : {
397 : 4 : situationType = ::ad::rss::situation::SituationType::IntersectionObjectHasPriority;
398 : : }
399 : : else
400 : : {
401 : 4 : situationType = ::ad::rss::situation::SituationType::IntersectionEgoHasPriority;
402 : : }
403 : : }
404 : : else
405 : : {
406 [ + + ]: 23 : if (intersectionOtherRoute.routePlanningCounter == objectRoute.routePlanningCounter)
407 : : {
408 : 19 : situationType = ::ad::rss::situation::SituationType::IntersectionEgoHasPriority;
409 : : }
410 : : else
411 : : {
412 : 4 : situationType = ::ad::rss::situation::SituationType::IntersectionObjectHasPriority;
413 : : }
414 : : }
415 : :
416 [ + - ]: 93 : auto const egoVehicleRoad = createIntersectionRoadArea(egoRoute, intersection, egoObject);
417 [ + - ]: 62 : auto const intersectingRoad = createIntersectionRoadArea(objectRoute, intersection, otherObject);
418 [ + - + - ]: 62 : getLogger()->debug("RssSceneCreation::appendIntersectionScene[{}]>> situation {}\n"
419 : : " ego road area {}\n intersection road area {}",
420 [ + - ]: 62 : otherObject->getId(),
421 : : situationType,
422 : : egoVehicleRoad,
423 : : intersectingRoad);
424 : :
425 [ + - ]: 31 : return appendScene(situationType, egoObject, egoVehicleRoad, otherObject, intersectingRoad);
426 : : }
427 : :
428 : 3 : bool RssSceneCreator::appendRoadBoundaryScenes(::ad::map::route::FullRoute const &route,
429 : : RssObjectConversion::ConstPtr iEgoObject)
430 : : {
431 [ - + ]: 3 : if (!bool(iEgoObject))
432 : : {
433 [ # # # # ]: 0 : getLogger()->error("RssSceneCreator::appendRoadBoundaryScenes[]>> ego object input is NULL");
434 : 0 : return false;
435 : : }
436 [ + - ]: 6 : auto egoObject = std::make_shared<RssObjectConversion>(*iEgoObject);
437 [ - + ]: 3 : if (!bool(egoObject))
438 : : {
439 [ # # # # ]: 0 : getLogger()->error("RssSceneCreator::appendRoadBoundaryScenes[]>> failed to create copy of RssObjectConversion");
440 : 0 : return false;
441 : : }
442 : :
443 : : auto const egoVehicleRoad
444 [ + - + - ]: 12 : = createRoadArea(route, route.minLaneOffset, route.maxLaneOffset, ::ad::map::lane::LaneIdSet(), {egoObject});
445 : :
446 [ + - + - ]: 6 : getLogger()->debug("RssSceneCreator::appendRoadBoundaryScenes[]>> ego road area {}", egoVehicleRoad);
447 : :
448 [ + - ]: 3 : ::ad::rss::world::RssDynamics staticDynamics;
449 : 3 : staticDynamics.alphaLat.accelMax = ::ad::physics::Acceleration(0.);
450 : 3 : staticDynamics.alphaLat.brakeMin = ::ad::physics::Acceleration(-0.01);
451 : 3 : staticDynamics.alphaLon.accelMax = ::ad::physics::Acceleration(0.);
452 : 3 : staticDynamics.alphaLon.brakeMax = ::ad::physics::Acceleration(-0.01);
453 : 3 : staticDynamics.alphaLon.brakeMin = ::ad::physics::Acceleration(-0.01);
454 : 3 : staticDynamics.alphaLon.brakeMinCorrect = ::ad::physics::Acceleration(-0.01);
455 : 3 : staticDynamics.lateralFluctuationMargin = ::ad::physics::Distance(0.0);
456 : 3 : staticDynamics.responseTime = ::ad::physics::Duration(0.01);
457 : 3 : staticDynamics.unstructuredSettings.pedestrianTurningRadius = ad::physics::Distance(0.);
458 : 3 : staticDynamics.unstructuredSettings.driveAwayMaxAngle = ad::physics::Angle(0.);
459 : 3 : staticDynamics.unstructuredSettings.vehicleYawRateChange = ad::physics::AngularAcceleration(0.);
460 : 3 : staticDynamics.unstructuredSettings.vehicleMinRadius = ad::physics::Distance(0.);
461 : 3 : staticDynamics.unstructuredSettings.vehicleTrajectoryCalculationStep = ad::physics::Duration(0.);
462 : :
463 : 6 : ::ad::rss::world::OccupiedRegionVector rightBorderOccupiedRegions;
464 : 6 : ::ad::rss::world::OccupiedRegionVector leftBorderOccupiedRegions;
465 : :
466 [ + + ]: 6 : for (auto const &roadSegment : route.roadSegments)
467 : : {
468 : 3 : auto &rightmostLane = roadSegment.drivableLaneSegments.front();
469 : :
470 [ + - ]: 3 : ::ad::rss::world::OccupiedRegion region;
471 : 3 : region.lonRange.minimum = ::ad::physics::ParametricValue(0.);
472 : 3 : region.lonRange.maximum = ::ad::physics::ParametricValue(1.);
473 : 3 : region.latRange.minimum = ::ad::physics::ParametricValue(0.999);
474 : 3 : region.latRange.maximum = ::ad::physics::ParametricValue(1.);
475 : 3 : region.segmentId = ::ad::rss::world::LaneSegmentId(rightmostLane.laneInterval.laneId);
476 [ + - ]: 3 : rightBorderOccupiedRegions.push_back(region);
477 : :
478 : 3 : auto &leftmostLane = roadSegment.drivableLaneSegments.back();
479 : :
480 : 3 : region.lonRange.minimum = ::ad::physics::ParametricValue(0.);
481 : 3 : region.lonRange.maximum = ::ad::physics::ParametricValue(1.);
482 : 3 : region.latRange.minimum = ::ad::physics::ParametricValue(0.);
483 : 3 : region.latRange.maximum = ::ad::physics::ParametricValue(0.001);
484 : 3 : region.segmentId = ::ad::rss::world::LaneSegmentId(leftmostLane.laneInterval.laneId);
485 [ + - ]: 3 : leftBorderOccupiedRegions.push_back(region);
486 : : }
487 : :
488 [ + - ]: 3 : ::ad::map::match::ENUObjectPosition rightBorderPosition;
489 : 3 : ::ad::physics::ParametricValue rightBorderOffsetLat;
490 : 3 : auto const rightBorderInterval = route.roadSegments.front().drivableLaneSegments.front().laneInterval;
491 [ + - - + ]: 3 : if (::ad::map::route::isRouteDirectionPositive(rightBorderInterval))
492 : : {
493 : 0 : rightBorderOffsetLat = ::ad::physics::ParametricValue(1.);
494 : : }
495 : : else
496 : : {
497 : 3 : rightBorderOffsetLat = ::ad::physics::ParametricValue(0.);
498 : : }
499 [ + - ]: 3 : auto const rightBorderParaPoint = getIntervalStart(rightBorderInterval);
500 [ + - ]: 3 : rightBorderPosition.enuReferencePoint = ::ad::map::access::getENUReferencePoint();
501 [ + - ]: 3 : rightBorderPosition.centerPoint = ::ad::map::lane::getENULanePoint(rightBorderParaPoint, rightBorderOffsetLat);
502 : : rightBorderPosition.heading
503 [ + - ]: 3 : = ::ad::map::lane::getLaneENUHeading(rightBorderParaPoint, rightBorderPosition.enuReferencePoint);
504 : 3 : rightBorderPosition.dimension.height = ::ad::physics::Distance(0.1);
505 : 3 : rightBorderPosition.dimension.length = ::ad::physics::Distance(0.1);
506 : 3 : rightBorderPosition.dimension.width = ::ad::physics::Distance(0.1);
507 : :
508 [ + - ]: 6 : RssObjectData rightBorderObjectData;
509 : 3 : rightBorderObjectData.id = getRightBorderObjectId();
510 : 3 : rightBorderObjectData.type = ::ad::rss::world::ObjectType::ArtificialObject;
511 : 3 : rightBorderObjectData.matchObject.enuPosition = rightBorderPosition;
512 : 3 : rightBorderObjectData.speed = ::ad::physics::Speed(0);
513 : 3 : rightBorderObjectData.yawRate = ::ad::physics::AngularVelocity(0.);
514 : 3 : rightBorderObjectData.steeringAngle = ::ad::physics::Angle(0.);
515 : 3 : rightBorderObjectData.rssDynamics = staticDynamics;
516 : :
517 [ + - ]: 6 : auto rightBorderObject = std::make_shared<RssObjectConversion>(rightBorderObjectData, rightBorderOccupiedRegions);
518 [ - + ]: 3 : if (!bool(rightBorderObject))
519 : : {
520 [ # # # # ]: 0 : getLogger()->error(
521 : : "RssSceneCreator::appendRoadBoundaryScenes[]>> failed to create copy of right border RssObjectConversion");
522 : 0 : return false;
523 : : }
524 : :
525 [ + - ]: 3 : ::ad::map::match::ENUObjectPosition leftBorderPosition;
526 : 3 : ::ad::physics::ParametricValue leftBorderOffsetLat;
527 : 3 : auto const leftBorderInterval = route.roadSegments.front().drivableLaneSegments.back().laneInterval;
528 [ + - - + ]: 3 : if (::ad::map::route::isRouteDirectionPositive(leftBorderInterval))
529 : : {
530 : 0 : leftBorderOffsetLat = ::ad::physics::ParametricValue(0.);
531 : : }
532 : : else
533 : : {
534 : 3 : leftBorderOffsetLat = ::ad::physics::ParametricValue(1.);
535 : : }
536 [ + - ]: 3 : auto const leftBorderParaPoint = getIntervalStart(leftBorderInterval);
537 [ + - ]: 3 : leftBorderPosition.enuReferencePoint = ::ad::map::access::getENUReferencePoint();
538 [ + - ]: 3 : leftBorderPosition.centerPoint = ::ad::map::lane::getENULanePoint(leftBorderParaPoint, leftBorderOffsetLat);
539 : : leftBorderPosition.heading
540 [ + - ]: 3 : = ::ad::map::lane::getLaneENUHeading(leftBorderParaPoint, leftBorderPosition.enuReferencePoint);
541 : 3 : leftBorderPosition.dimension.height = ::ad::physics::Distance(0.1);
542 : 3 : leftBorderPosition.dimension.length = ::ad::physics::Distance(0.1);
543 : 3 : leftBorderPosition.dimension.width = ::ad::physics::Distance(0.1);
544 : :
545 [ + - ]: 6 : RssObjectData leftBorderObjectData;
546 : 3 : leftBorderObjectData.id = getLeftBorderObjectId();
547 : 3 : leftBorderObjectData.type = ::ad::rss::world::ObjectType::ArtificialObject;
548 : 3 : leftBorderObjectData.matchObject.enuPosition = leftBorderPosition;
549 : 3 : leftBorderObjectData.speed = ::ad::physics::Speed(0);
550 : 3 : leftBorderObjectData.yawRate = ::ad::physics::AngularVelocity(0.);
551 : 3 : leftBorderObjectData.steeringAngle = ::ad::physics::Angle(0.);
552 : 3 : leftBorderObjectData.rssDynamics = staticDynamics;
553 : :
554 [ + - ]: 6 : auto leftBorderObject = std::make_shared<RssObjectConversion>(leftBorderObjectData, leftBorderOccupiedRegions);
555 [ - + ]: 3 : if (!bool(leftBorderObject))
556 : : {
557 [ # # # # ]: 0 : getLogger()->error(
558 : : "RssSceneCreator::appendRoadBoundaryScenes[]>> failed to create copy of left border RssObjectConversion");
559 : 0 : return false;
560 : : }
561 : :
562 [ + - ]: 3 : auto result = appendScene(::ad::rss::situation::SituationType::SameDirection,
563 : : egoObject,
564 : : egoVehicleRoad,
565 : : rightBorderObject,
566 : 6 : ::ad::rss::world::RoadArea());
567 : :
568 [ + - ]: 6 : result &= appendScene(::ad::rss::situation::SituationType::SameDirection,
569 : : egoObject,
570 : : egoVehicleRoad,
571 : : leftBorderObject,
572 : 9 : ::ad::rss::world::RoadArea());
573 : :
574 : 3 : return result;
575 : : }
576 : :
577 : 282 : bool RssSceneCreator::appendScene(::ad::rss::situation::SituationType const &situationType,
578 : : RssObjectConversion::ConstPtr egoObject,
579 : : ::ad::rss::world::RoadArea const &egoRoad,
580 : : RssObjectConversion::ConstPtr otherObject,
581 : : ::ad::rss::world::RoadArea const &intersectingRoad)
582 : : {
583 [ + - - + : 282 : if (!bool(egoObject) || !bool(otherObject))
- + ]
584 : : {
585 [ # # # # ]: 0 : getLogger()->error("RssSceneCreator::appendScene[]>> ego/other object input is NULL");
586 : 0 : return false;
587 : : }
588 : :
589 [ + - ]: 564 : ::ad::rss::world::Scene scene;
590 : 282 : scene.situationType = situationType;
591 [ + - + - ]: 282 : scene.egoVehicle = egoObject->getRssObject();
592 [ + - ]: 282 : scene.egoVehicleRssDynamics = egoObject->getRssDynamics();
593 [ + - ]: 282 : scene.egoVehicleRoad = egoRoad;
594 [ + - ]: 282 : scene.intersectingRoad = intersectingRoad;
595 [ + - + - ]: 282 : scene.object = otherObject->getRssObject();
596 [ + - ]: 282 : scene.objectRssDynamics = otherObject->getRssDynamics();
597 : :
598 [ + - + - : 564 : getLogger()->trace("RssSceneCreator::appendScene[{}]>> object {}", otherObject->getId(), scene.object);
+ - ]
599 [ + - + - : 564 : getLogger()->trace("RssSceneCreator::appendScene[{}]>> ego {}", otherObject->getId(), scene.egoVehicle);
+ - ]
600 : :
601 [ + + ]: 282 : if (situationType != ::ad::rss::situation::SituationType::NotRelevant)
602 : : {
603 : : // relevant situations have to consider some more checks to filter out unsupported situations
604 : 452 : if ((situationType != ::ad::rss::situation::SituationType::Unstructured)
605 [ + - + - : 226 : && (scene.egoVehicle.occupiedRegions.empty() || scene.object.occupiedRegions.empty()))
- + - + ]
606 : : {
607 [ # # # # ]: 0 : getLogger()->debug(
608 : : "RssSceneCreator::appendScene[{}]>> ego or object not on route. Structured scene not valid. Dropping. {}",
609 [ # # ]: 0 : otherObject->getId(),
610 : : scene);
611 : 0 : return false;
612 : : }
613 [ + - - + ]: 226 : else if (!egoObject->isOriginalSpeedAcceptable())
614 : : {
615 [ # # # # ]: 0 : getLogger()->debug("RssSceneCreator::appendScene[{}]>> ego original speed {} is not acceptable. Dropping. {}",
616 [ # # ]: 0 : otherObject->getId(),
617 : : egoObject->getOriginalObjectSpeed(),
618 : : scene);
619 : 0 : return false;
620 : : }
621 [ + - - + ]: 226 : else if (!otherObject->isOriginalSpeedAcceptable())
622 : : {
623 [ # # # # ]: 0 : getLogger()->warn("RssSceneCreator::appendScene[{}]>> other original speed {} is not acceptable. Dropping. {}",
624 [ # # ]: 0 : otherObject->getId(),
625 : : otherObject->getOriginalObjectSpeed(),
626 : : scene);
627 : 0 : return false;
628 : : }
629 : : }
630 : :
631 [ + - + - ]: 282 : if (withinValidInputRange(scene))
632 : : {
633 [ + - ]: 282 : return mSceneCreation.appendSceneToWorldModel(scene);
634 : : }
635 : : else
636 : : {
637 [ # # # # : 0 : getLogger()->error("RssSceneCreator::appendScene[{}]>> scene no valid {}", otherObject->getId(), scene);
# # ]
638 : 0 : return false;
639 : : }
640 : : }
641 : :
642 : 0 : bool RssSceneCreator::appendUnstructuredScene(RssObjectConversion::ConstPtr iEgoObject,
643 : : RssObjectConversion::ConstPtr iOtherObject)
644 : : {
645 [ # # # # : 0 : if (!bool(iEgoObject) || !bool(iOtherObject))
# # ]
646 : : {
647 [ # # # # ]: 0 : getLogger()->error("RssSceneCreator::appendUnstructuredScene[]>> ego/other object input is NULL");
648 : 0 : return false;
649 : : }
650 [ # # ]: 0 : auto egoObject = std::make_shared<RssObjectConversion>(*iEgoObject);
651 [ # # ]: 0 : if (!bool(egoObject))
652 : : {
653 [ # # # # ]: 0 : getLogger()->error(
654 : : "RssSceneCreator::appendUnstructuredScene[{}]>> failed to create copy of ego RssObjectConversion",
655 [ # # ]: 0 : iOtherObject->getId());
656 : 0 : return false;
657 : : }
658 [ # # ]: 0 : auto otherObject = std::make_shared<RssObjectConversion>(*iOtherObject);
659 [ # # ]: 0 : if (!bool(otherObject))
660 : : {
661 [ # # # # ]: 0 : getLogger()->error(
662 : : "RssSceneCreator::appendUnstructuredScene[{}]>> failed to create copy of other RssObjectConversion",
663 [ # # ]: 0 : iOtherObject->getId());
664 : 0 : return false;
665 : : }
666 : :
667 [ # # # # : 0 : getLogger()->debug("RssSceneCreator::appendUnstructuredScene[{}]>>", otherObject->getId());
# # ]
668 : :
669 [ # # ]: 0 : return appendScene(::ad::rss::situation::SituationType::Unstructured,
670 : : egoObject,
671 : 0 : ::ad::rss::world::RoadArea(),
672 : : otherObject,
673 : 0 : ::ad::rss::world::RoadArea());
674 : : }
675 : : } // namespace map
676 : : } // namespace rss
677 : : } // namespace ad
|
