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/map/RssSceneCreation.hpp"
10 : : #include <ad/map/route/Planning.hpp>
11 : : #include <ad/rss/world/WorldModelValidInputRange.hpp>
12 : : #include "ad/rss/map/Logging.hpp"
13 : : #include "ad/rss/map/RssObjectConversion.hpp"
14 : : #include "ad/rss/map/RssSceneCreator.hpp"
15 : :
16 : : /*!
17 : : * @brief namespace rss
18 : : */
19 : : namespace ad {
20 : : /*!
21 : : * @brief namespace rss
22 : : */
23 : : namespace rss {
24 : : /*!
25 : : * @brief namespace map
26 : : */
27 : : namespace map {
28 : :
29 : : const ::ad::physics::Distance cMinSceneDistance(100.);
30 : :
31 : 214 : RssSceneCreation::RssSceneCreation(::ad::rss::world::TimeIndex const &timeIndex,
32 : 214 : ::ad::rss::world::RssDynamics const &defaultEgoRssDynamics)
33 : : : mFinalized(false)
34 : 214 : , mWorldModelLock()
35 : : {
36 : 214 : mWorldModel.timeIndex = timeIndex;
37 : 214 : mWorldModel.defaultEgoVehicleRssDynamics = defaultEgoRssDynamics;
38 : 214 : }
39 : :
40 : 214 : ::ad::rss::world::WorldModel RssSceneCreation::getWorldModel()
41 : : {
42 [ + - ]: 428 : const std::lock_guard<std::mutex> lock(mWorldModelLock);
43 [ - + ]: 214 : if (mFinalized)
44 : : {
45 [ # # # # ]: 0 : getLogger()->error("RssSceneCreation::getWorldModel>> error world model already finalized.");
46 [ # # ]: 0 : return ::ad::rss::world::WorldModel();
47 : : }
48 : :
49 : 214 : mFinalized = true;
50 : 214 : return std::move(mWorldModel);
51 : : }
52 : :
53 : 213 : bool RssSceneCreation::appendScenes(RssObjectData const &egoObjectData,
54 : : ::ad::map::route::FullRoute const &egoRouteInput,
55 : : RssObjectData const &otherObjectData,
56 : : RestrictSpeedLimitMode const &restrictSpeedLimitMode,
57 : : ::ad::map::landmark::LandmarkIdSet const &greenTrafficLights,
58 : : ::ad::rss::map::RssMode const &mode)
59 : : {
60 [ - + ]: 213 : if (mFinalized)
61 : : {
62 [ # # # # ]: 0 : getLogger()->error("RssSceneCreation::appendScenes[{}]>> error world model already finalized.", otherObjectData.id);
63 : 0 : return false;
64 : : }
65 : :
66 [ + - ]: 426 : auto egoObject = std::make_shared<RssObjectConversion const>(egoObjectData);
67 [ + - ]: 426 : auto otherObject = std::make_shared<RssObjectConversion const>(otherObjectData);
68 : :
69 [ + - ]: 426 : RssSceneCreator sceneCreator(restrictSpeedLimitMode, greenTrafficLights, *this);
70 : :
71 [ - + ]: 213 : if (mode == ::ad::rss::map::RssMode::NotRelevant)
72 : : {
73 [ # # ]: 0 : return sceneCreator.appendNotRelevantScene(egoRouteInput, egoObject, otherObject);
74 : : }
75 [ - + ]: 213 : else if (mode == ::ad::rss::map::RssMode::Unstructured)
76 : : {
77 [ # # ]: 0 : return sceneCreator.appendUnstructuredScene(egoObject, otherObject);
78 : : }
79 : : else
80 : : {
81 [ + - ]: 213 : return appendStructuredScenes(sceneCreator, egoObject, egoRouteInput, otherObject);
82 : : }
83 : : }
84 : :
85 : 213 : bool RssSceneCreation::appendStructuredScenes(::ad::rss::map::RssSceneCreator &sceneCreator,
86 : : std::shared_ptr<RssObjectConversion const> const &egoObject,
87 : : ::ad::map::route::FullRoute const &egoRouteInput,
88 : : std::shared_ptr<RssObjectConversion const> const &otherObject)
89 : : {
90 : 213 : bool result = false;
91 : 213 : bool sceneAppended = false;
92 : :
93 [ + - - + : 213 : if ((egoObject->getObjectMapMatchedPosition() == nullptr) || (otherObject->getObjectMapMatchedPosition() == nullptr))
- + ]
94 : : {
95 [ # # ]: 0 : getLogger()->error("RssSceneCreation::appendStructuredScenes[{}]>> invalid RssObjectConversion entity passed",
96 [ # # ]: 0 : otherObject->getId());
97 : 0 : return false;
98 : : }
99 : :
100 : 213 : auto const &egoMatchObject = *egoObject->getObjectMapMatchedPosition();
101 : 213 : auto const &objectMatchObject = *otherObject->getObjectMapMatchedPosition();
102 : :
103 [ - + ]: 213 : if (egoMatchObject.mapMatchedBoundingBox.laneOccupiedRegions.empty())
104 : : {
105 [ # # ]: 0 : getLogger()->warn("RssSceneCreation::appendStructuredScenes[{}]>> ego without occupied regions skipping.",
106 [ # # ]: 0 : otherObject->getId());
107 : 0 : return false;
108 : : }
109 [ - + ]: 213 : if (objectMatchObject.mapMatchedBoundingBox.laneOccupiedRegions.empty())
110 : : {
111 [ # # ]: 0 : getLogger()->warn("RssSceneCreation::appendStructuredScenes[{}]>> object without occupied regions skipping.",
112 [ # # ]: 0 : otherObject->getId());
113 : 0 : return false;
114 : : }
115 : :
116 : : try
117 : : {
118 : 213 : physics::Distance minStoppingDistance;
119 [ + - - + ]: 213 : if (!egoObject->calculateMinStoppingDistance(minStoppingDistance))
120 : : {
121 [ # # # # ]: 0 : getLogger()->error("RssSceneCreation::appendStructuredScenes[{}]>> error calculating min stopping distance",
122 [ # # ]: 0 : otherObject->getId());
123 : 38 : return false;
124 : : }
125 : :
126 : : // and for sure consider some minimum distance
127 [ + - ]: 213 : auto maxConnectingRouteDistance = std::max(minStoppingDistance, cMinSceneDistance);
128 : :
129 : : // Calculate shortest route (ignore driving direction) between the two objects MapMatchedObjectBoundingBox
130 : : auto const connectingRoute = ::ad::map::route::planning::calculateConnectingRoute(
131 [ + - ]: 426 : egoMatchObject, objectMatchObject, maxConnectingRouteDistance);
132 : :
133 [ + + ]: 213 : if (connectingRoute.type == ::ad::map::route::ConnectingRouteType::Invalid)
134 : : {
135 [ + - + - ]: 76 : getLogger()->debug(
136 : : "RssSceneCreation::appendStructuredScenes[{}]>> no connecting route available to object with {} meters",
137 [ + - ]: 76 : otherObject->getId(),
138 : : maxConnectingRouteDistance);
139 [ + - ]: 38 : auto const appendResult = sceneCreator.appendNotRelevantScene(egoRouteInput, egoObject, otherObject);
140 : 38 : return appendResult;
141 : : }
142 [ + - + - ]: 350 : getLogger()->trace(
143 : : "RssSceneCreation::appendStructuredScenes[{}]>> connecting route of length {} available to object:\n {}",
144 [ + - ]: 175 : otherObject->getId(),
145 [ + - ]: 350 : ad::map::route::calcLength(connectingRoute),
146 : : connectingRoute);
147 : :
148 : : // prepare ego route
149 [ + - + - ]: 175 : auto const predictionLength = ::ad::map::route::calcLength(connectingRoute) + ::ad::physics::Distance(20.);
150 : 350 : std::vector<::ad::map::route::FullRoute> egoPredictedRoutes;
151 [ + + ]: 175 : if (egoRouteInput.roadSegments.empty())
152 : : {
153 : : egoPredictedRoutes
154 [ + - ]: 56 : = ::ad::map::route::planning::predictRoutesOnDistance(egoMatchObject.mapMatchedBoundingBox, predictionLength);
155 : : }
156 : : else
157 : : {
158 : : // for the analysis we are only interested in the near term route
159 [ + - ]: 238 : auto shortenedRoute = egoRouteInput;
160 [ + - + - ]: 119 : ::ad::map::route::shortenRouteToDistance(shortenedRoute,
161 : 119 : std::max(predictionLength, ::ad::physics::Distance(100.)));
162 [ + - ]: 119 : egoPredictedRoutes.push_back(shortenedRoute);
163 : : }
164 : :
165 : : // reset the result to ensure we don't miss anything
166 : 175 : result = false;
167 [ + + ]: 175 : if (connectingRoute.type == ::ad::map::route::ConnectingRouteType::Following)
168 : : {
169 : : // vehicle following mode: it doesn't matter if there is an intersection in between or not
170 : : // as the cars might follow each other through the intersection
171 : :
172 : : // @todo: to be clarified finally, if we actually can ignore others not on our route
173 : : // for the moment we do so if we are following another one, but that one already left our route
174 : : // (e.g. standing at the intersection exit when turning left, but we are driving straight through)
175 : 120 : bool objectOnOneOfOurRoutes = false;
176 [ + + ]: 120 : if (connectingRoute.routeA.roadSegments.empty())
177 : : {
178 : : // we are in front -> situation has to be considered since the other might follow us in our route
179 : 46 : objectOnOneOfOurRoutes = true;
180 : : }
181 : : else
182 : : {
183 [ + + ]: 102 : for (auto const &egoRoute : egoPredictedRoutes)
184 : : {
185 [ + - ]: 84 : auto const findObject = ::ad::map::route::objectOnRoute(objectMatchObject.mapMatchedBoundingBox, egoRoute);
186 [ + + ]: 84 : if (findObject.isValid())
187 : : {
188 : 56 : objectOnOneOfOurRoutes = true;
189 [ + - + - ]: 112 : getLogger()->trace("RssSceneCreation::appendStructuredScenes[{}]>> driving in front on our route:\n"
190 : : " objectMatchObject({})\n egoRoute({})",
191 [ + - ]: 112 : otherObject->getId(),
192 : : objectMatchObject,
193 : : egoRoute);
194 : 56 : break;
195 : : }
196 : : else
197 : : {
198 [ + - + - ]: 84 : getLogger()->trace("RssSceneCreation::appendStructuredScenes[{}]>> driving in front but not on route:\n"
199 : : " objectMatchObject({})\n egoRoute({})",
200 [ + - ]: 56 : otherObject->getId(),
201 : : objectMatchObject,
202 : : egoRoute);
203 : : }
204 : : }
205 : : }
206 [ + + ]: 120 : if (objectOnOneOfOurRoutes)
207 : : {
208 [ + - + - ]: 204 : getLogger()->debug(
209 : : "RssSceneCreation::appendStructuredScenes[{}]>> relevant following connecting route found: create "
210 : : "same direction use-case\n {}",
211 [ + - ]: 204 : otherObject->getId(),
212 : : connectingRoute);
213 : 204 : auto const appendResult = sceneCreator.appendNonIntersectionScene(
214 [ + - ]: 102 : connectingRoute, ::ad::rss::situation::SituationType::SameDirection, egoObject, otherObject);
215 : 102 : result = appendResult;
216 : 102 : sceneAppended = appendResult;
217 : : }
218 : : else
219 : : {
220 : : // no relevant object found
221 : : // @todo: is there anything else to be done here?
222 : 18 : result = true;
223 : : }
224 : : }
225 [ + - + + ]: 55 : else if (!::ad::map::route::isConnectedRoutePartOfAnIntersection(connectingRoute))
226 : : {
227 [ + - ]: 10 : if (connectingRoute.type == ::ad::map::route::ConnectingRouteType::Opposing)
228 : : {
229 [ + - + - ]: 20 : getLogger()->debug(
230 : : "RssSceneCreation::appendStructuredScenes[{}]>> no intersections on opposing connecting route found: create "
231 : : "opposite direction use-case\n {}",
232 [ + - ]: 20 : otherObject->getId(),
233 : : connectingRoute);
234 : 20 : auto const appendResult = sceneCreator.appendNonIntersectionScene(
235 [ + - ]: 10 : connectingRoute, ::ad::rss::situation::SituationType::OppositeDirection, egoObject, otherObject);
236 : 10 : result = appendResult;
237 : 10 : sceneAppended = appendResult;
238 : : }
239 : : else
240 : : {
241 : : // ConnectingRouteType::Merging
242 : : // no intersection in between, but merging
243 : : // we interpret this now as intersection case
244 : : // @todo: analyze in more detail how such scenes have to be handled, keep as info message for now
245 [ # # # # ]: 0 : getLogger()->info(
246 : : "RssSceneCreation::appendStructuredScenes[{}]>> no intersections on merging connecting route found: create "
247 : : "merging-intersection same prio use-case\n {}",
248 [ # # ]: 0 : otherObject->getId(),
249 : : connectingRoute);
250 : 0 : auto const appendResult = sceneCreator.appendMergingScene(
251 [ # # ]: 0 : connectingRoute, ::ad::rss::situation::SituationType::IntersectionSamePriority, egoObject, otherObject);
252 : 0 : result = appendResult;
253 : 0 : sceneAppended = appendResult;
254 : : }
255 : : }
256 : : else
257 : : {
258 : 45 : result = true;
259 : : // case: ConnectingRouteType::Opposing with intersections
260 : : // or: ConnectingRouteType::Merging with intersections
261 [ + + ]: 98 : for (auto const &egoRoute : egoPredictedRoutes)
262 : : {
263 [ + - + - ]: 106 : getLogger()->trace(
264 [ + - ]: 106 : "RssSceneCreation::appendStructuredScenes[{}]>> investigate egoRoute({})", otherObject->getId(), egoRoute);
265 : : // @todo: intersection creation has to support also creation of already entered intersections
266 : : // to get intersections currently driving in also considered appropriate!!
267 : : // The below is calculated wrong if there is -- besides the currently ignored entered intersection another one
268 : : // ...
269 : : //
270 [ + - ]: 106 : auto intersectionsOnEgoRoute = ::ad::map::intersection::Intersection::getIntersectionsForRoute(egoRoute);
271 [ + + - + : 53 : if (intersectionsOnEgoRoute.empty() && (connectingRoute.type == ::ad::map::route::ConnectingRouteType::Merging))
- + ]
272 : : {
273 [ # # # # ]: 0 : getLogger()->debug("RssSceneCreation::appendStructuredScenes[{}]>> no intersections on route found: create"
274 : : "merging-intersection same prio use-case:\n egoRoute({})",
275 [ # # ]: 0 : otherObject->getId(),
276 : : egoRoute);
277 : 0 : auto const appendResult = sceneCreator.appendMergingScene(
278 [ # # ]: 0 : connectingRoute, ::ad::rss::situation::SituationType::IntersectionSamePriority, egoObject, otherObject);
279 [ # # # # ]: 0 : result = result && appendResult;
280 : 0 : sceneAppended |= appendResult;
281 : : }
282 : : else
283 : : {
284 : : // analyse object predictions in detail
285 : : auto const objectPredictedRoutes = ::ad::map::route::planning::predictRoutesOnDistance(
286 [ + - ]: 106 : objectMatchObject.mapMatchedBoundingBox, predictionLength);
287 [ + + ]: 161 : for (auto const &objectRoute : objectPredictedRoutes)
288 : : {
289 [ + - ]: 216 : auto intersectionsOnRoute = intersectionsOnEgoRoute;
290 : 108 : auto const *intersectionOtherRoute = &objectRoute;
291 [ + + ]: 108 : if (intersectionsOnRoute.empty())
292 : : {
293 : : // no intersection on ego route, but within opposing connectingRoute
294 : : // Currently this can happen if the ego vehicle route starts inside the intersection,
295 : : // and the object is outside, let's then take the intersections on the object route
296 [ + - ]: 16 : intersectionsOnRoute = ::ad::map::intersection::Intersection::getIntersectionsForRoute(objectRoute);
297 [ - + ]: 16 : if (intersectionsOnRoute.empty())
298 : : {
299 : : // looks like not a straight forward road layout, but
300 : : // need to ensure we don't miss this situation
301 : : // @todo: analyze when this can happen and if the created scene is appropriate
302 : :
303 [ # # # # ]: 0 : getLogger()->debug(
304 : : "RssSceneCreation::appendStructuredScenes[{}]>> no intersections on route found: create "
305 : : "opposite direction use-case:\n"
306 : : " egoRoute({})",
307 [ # # ]: 0 : otherObject->getId(),
308 : : egoRoute);
309 : 0 : auto const appendResult = sceneCreator.appendNonIntersectionScene(
310 [ # # ]: 0 : connectingRoute, ::ad::rss::situation::SituationType::OppositeDirection, egoObject, otherObject);
311 [ # # # # ]: 0 : result = result && appendResult;
312 : 0 : sceneAppended |= appendResult;
313 : : }
314 : : else
315 : : {
316 : 16 : intersectionOtherRoute = &egoRoute;
317 [ + - + - ]: 48 : getLogger()->trace("RssSceneCreation::appendStructuredScenes[{}]>> no intersection on ego route, but"
318 : : "opposite connecting route; checking intersections in object route:\n"
319 : : " egoRoute({})\n objectRoute({})",
320 [ + - ]: 32 : otherObject->getId(),
321 : : egoRoute,
322 : : objectRoute);
323 : : }
324 : : }
325 : :
326 [ + + ]: 216 : for (auto const &intersection : intersectionsOnRoute)
327 : : {
328 [ + - - + ]: 108 : if (!intersection->objectRouteCrossesIntersection(*intersectionOtherRoute))
329 : : {
330 [ # # # # ]: 0 : getLogger()->trace("RssSceneCreation::appendStructuredScenes[{}]>> found object route not crossing "
331 : : "intersection on route:\n"
332 : : " egoRoute({})\n objectRoute({})\n intersection({})",
333 [ # # ]: 0 : otherObject->getId(),
334 : : egoRoute,
335 : : objectRoute,
336 [ # # ]: 0 : intersection->entryParaPoints());
337 : : }
338 [ + - - + ]: 108 : else if (intersection->objectRouteFromSameArmAsIntersectionRoute(*intersectionOtherRoute))
339 : : {
340 : : // This can happen if the intersection is not necessarily the first intersection of both vehicles
341 : : // and so being a secondary merging use-case at that far intersection
342 : : // Therefore, later this situation might switch to an actual merging connected route or following use
343 : : // case from above.
344 : : // Now: Create an intersection scene
345 : : // @todo: find out more here: is it required now to search the two routes for a merge even before the
346 : : // intersection?
347 [ # # # # ]: 0 : getLogger()->debug(
348 : : "RssSceneCreation::appendStructuredScenes[{}]>> object route from same arm as intersection "
349 : : "looks like merging situation far in future: create intersection scene\n"
350 : : " egoRoute({})\n objectRoute({})\n intersection({})\n {}",
351 [ # # ]: 0 : otherObject->getId(),
352 : : egoRoute,
353 : : objectRoute,
354 [ # # ]: 0 : intersection->entryParaPoints(),
355 : : connectingRoute);
356 [ # # ]: 0 : auto const appendResult = sceneCreator.appendIntersectionScene(
357 : : intersection, egoRoute, objectRoute, *intersectionOtherRoute, egoObject, otherObject);
358 [ # # # # ]: 0 : result = result && appendResult;
359 : 0 : sceneAppended |= appendResult;
360 : : }
361 [ + - ]: 108 : else if (intersection->objectRouteOppositeToIntersectionRoute(*intersectionOtherRoute)
362 [ + + + - : 108 : || !intersection->objectRouteCrossesIntersectionRoute(*intersectionOtherRoute))
+ + + + ]
363 : : {
364 [ + - ]: 77 : if (connectingRoute.type == ::ad::map::route::ConnectingRouteType::Opposing)
365 : : {
366 : : // here, the connecting route should be correct; otherwise we would not oppose to each other
367 [ + - + - ]: 154 : getLogger()->debug(
368 : : "RssSceneCreation::appendStructuredScenes[{}]>> object route opposite and not crossing: "
369 : : "opposite direction scene:\n"
370 : : " egoRoute({})\n objectRoute({})\n intersection({})",
371 [ + - ]: 154 : otherObject->getId(),
372 : : egoRoute,
373 : : objectRoute,
374 [ + - ]: 77 : intersection->entryParaPoints());
375 : 154 : auto const appendResult = sceneCreator.appendNonIntersectionScene(
376 [ + - ]: 77 : connectingRoute, ::ad::rss::situation::SituationType::OppositeDirection, egoObject, otherObject);
377 [ + - + - ]: 77 : result = result && appendResult;
378 : 77 : sceneAppended |= appendResult;
379 : : }
380 : : else
381 : : {
382 : : // This can happen if the cars are too far away from each other for creating an opposing
383 : : // connecting route, but we find two predictions of the two vehicles ending at the same
384 : : // intersection
385 : : // Therefore, later this situation might switch to the opposite type above.
386 : : // Now: Create an intersection scene
387 [ # # # # ]: 0 : getLogger()->debug(
388 : : "RssSceneCreation::appendStructuredScenes[{}]>> object route opposite and not crossing, but "
389 : : "merging situation: create "
390 : : "intersection scene\n",
391 : : " egoRoute({})\n objectRoute({})\n intersection({})\n {}",
392 [ # # ]: 0 : otherObject->getId(),
393 : : egoRoute,
394 : : objectRoute,
395 [ # # ]: 0 : intersection->entryParaPoints(),
396 : : connectingRoute);
397 [ # # ]: 0 : auto const appendResult = sceneCreator.appendIntersectionScene(
398 : : intersection, egoRoute, objectRoute, *intersectionOtherRoute, egoObject, otherObject);
399 [ # # # # ]: 0 : result = result && appendResult;
400 : 0 : sceneAppended |= appendResult;
401 : : }
402 : : }
403 : : else
404 : : {
405 [ + - + - ]: 62 : getLogger()->debug(
406 : : "RssSceneCreation::appendStructuredScenes[{}]>> object route crosses intersection route: "
407 : : "intersection scene\n"
408 : : " egoRoute({})\n objectRoute({})\n intersectionLanesOnRoute({})\n intersection({})",
409 [ + - ]: 62 : otherObject->getId(),
410 : : egoRoute,
411 : : objectRoute,
412 [ + - ]: 31 : intersection->paraPointsOnRoute(),
413 [ + - ]: 31 : intersection->entryParaPoints());
414 [ + - ]: 31 : auto const appendResult = sceneCreator.appendIntersectionScene(
415 : : intersection, egoRoute, objectRoute, *intersectionOtherRoute, egoObject, otherObject);
416 [ + - + - ]: 31 : result = result && appendResult;
417 : 31 : sceneAppended |= appendResult;
418 : : }
419 : : }
420 : : }
421 : : }
422 : : }
423 : : }
424 : : }
425 : 0 : catch (std::exception const &e)
426 : : {
427 [ - - - - ]: 0 : getLogger()->error(
428 [ - - ]: 0 : "RssSceneCreation::appendStructuredScenes[{}]>> Failed. Catched exception: {}", otherObject->getId(), e.what());
429 : : }
430 : :
431 [ + + ]: 175 : if (!sceneAppended)
432 : : {
433 [ + - ]: 18 : if (result)
434 : : {
435 [ + - ]: 54 : getLogger()->debug("RssSceneCreation::appendStructuredScenes[{}]>> detailed analysis did not find any relevant "
436 : : "situation. Append non relevant scene\n"
437 : : " objectMatchObject({})\n egoMatchObject({})\n egoRoute({})",
438 [ + - ]: 36 : otherObject->getId(),
439 : : objectMatchObject,
440 : : egoMatchObject,
441 : : egoRouteInput);
442 : : }
443 : : else
444 : : {
445 [ # # ]: 0 : getLogger()->info(
446 : : "RssSceneCreation::appendStructuredScenes[{}]>> an operation failed and no scene could have been "
447 : : "created. Try to append non relevant scene\n"
448 : : " objectMatchObject({})\n egoMatchObject({})\n egoRoute({})",
449 [ # # ]: 0 : otherObject->getId(),
450 : : objectMatchObject,
451 : : egoMatchObject,
452 : : egoRouteInput);
453 : : }
454 [ + - ]: 18 : auto const appendResult = sceneCreator.appendNotRelevantScene(egoRouteInput, egoObject, otherObject);
455 [ + - + - ]: 18 : result = result && appendResult;
456 : : }
457 : :
458 : 175 : return result;
459 : : }
460 : :
461 : 6 : bool RssSceneCreation::appendRoadBoundaries(RssObjectData const &egoObjectData,
462 : : ::ad::map::route::FullRoute const &inputRoute,
463 : : AppendRoadBoundariesMode const operationMode)
464 : : {
465 [ - + ]: 6 : if (mFinalized)
466 : : {
467 [ # # ]: 0 : getLogger()->error("RssSceneCreation::appendRoadBoundaries>> error world model already finalized.");
468 : 0 : return false;
469 : : }
470 [ + + ]: 6 : if (egoObjectData.matchObject.mapMatchedBoundingBox.laneOccupiedRegions.empty())
471 : : {
472 [ + - ]: 1 : getLogger()->warn("RssSceneCreation::appendRoadBoundaries>> ego without occupied regions skipping.");
473 : 1 : return false;
474 : : }
475 : :
476 [ + + ]: 5 : if (inputRoute.roadSegments.empty())
477 : : {
478 : 1 : return false;
479 : : }
480 : 4 : bool result = false;
481 : : try
482 : : {
483 : : // for the analysis we are only interested in the near term route
484 [ + - ]: 8 : ::ad::map::route::FullRoute route = inputRoute;
485 [ + - ]: 4 : ::ad::map::route::shortenRouteToDistance(route, ::ad::physics::Distance(20.));
486 : :
487 [ + + ]: 4 : if (operationMode == AppendRoadBoundariesMode::ExpandRouteToOppositeLanes)
488 : : {
489 [ + - ]: 1 : route = ::ad::map::route::getRouteExpandedToOppositeLanes(route);
490 : : }
491 [ + + ]: 3 : else if (operationMode == AppendRoadBoundariesMode::ExpandRouteToAllNeighbors)
492 : : {
493 [ + - ]: 1 : route = ::ad::map::route::getRouteExpandedToAllNeighborLanes(route);
494 : : }
495 : :
496 [ + + ]: 7 : auto egoObject = std::make_shared<RssObjectConversion const>(egoObjectData);
497 : :
498 [ + - ]: 3 : RssSceneCreator sceneCreator(*this);
499 [ + - + - ]: 6 : getLogger()->debug("RssSceneCreation::appendRoadBoundaries[]>>\n"
500 : : " egoRoute({})\n egoObject({})",
501 : : route,
502 : : egoObject);
503 [ + - ]: 3 : result = sceneCreator.appendRoadBoundaryScenes(route, egoObject);
504 : : }
505 : 1 : catch (std::exception const &e)
506 : : {
507 [ + - + - ]: 2 : getLogger()->error("RssSceneCreation::appendRoadBoundaries[ ]>> Failed. Catched exception: {}", e.what());
508 : : }
509 : 4 : return result;
510 : : }
511 : :
512 : 282 : bool RssSceneCreation::appendSceneToWorldModel(::ad::rss::world::Scene const &scene)
513 : : {
514 [ + - ]: 564 : const std::lock_guard<std::mutex> lock(mWorldModelLock);
515 [ - + ]: 282 : if (mFinalized)
516 : : {
517 [ # # # # ]: 0 : getLogger()->error("RssSceneCreation::appendSceneToWorldModel>> error world model already finalized.");
518 : 0 : return false;
519 : : }
520 : :
521 [ + - ]: 282 : mWorldModel.scenes.push_back(scene);
522 : 282 : return true;
523 : : }
524 : :
525 : : } // namespace map
526 : : } // namespace rss
527 : : } // namespace ad
|
