Branch data Line data Source code
1 : : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
2 : : //
3 : : // Copyright (C) 2018-2022 Intel Corporation
4 : : //
5 : : // SPDX-License-Identifier: LGPL-2.1-only
6 : : //
7 : : // ----------------- END LICENSE BLOCK -----------------------------------
8 : :
9 : : #include "ad/rss/map/RssWorldModelCreation.hpp"
10 : : #include <ad/map/route/Planning.hpp>
11 : : #include <ad/map/route/RouteOperation.hpp>
12 : : #include <ad/rss/world/WorldModelValidInputRange.hpp>
13 : : #include "ad/rss/map/Logging.hpp"
14 : : #include "ad/rss/map/RssObjectConversion.hpp"
15 : : #include "ad/rss/map/RssWorldModelCreator.hpp"
16 : :
17 : : /*!
18 : : * @brief namespace rss
19 : : */
20 : : namespace ad {
21 : : /*!
22 : : * @brief namespace rss
23 : : */
24 : : namespace rss {
25 : : /*!
26 : : * @brief namespace map
27 : : */
28 : : namespace map {
29 : :
30 : : const ::ad::physics::Distance RssWorldModelCreation::cMinConnectedRouteLength(100.);
31 : : const ::ad::physics::Distance cNearTermRoadBoundariesRouteLength(10.);
32 : :
33 : 214 : RssWorldModelCreation::RssWorldModelCreation(world::TimeIndex const &time_index,
34 : 214 : world::RssDynamics const &defaultEgoRssDynamics)
35 : 214 : : mFinalized(false)
36 : 214 : , mWorldModelLock()
37 : 214 : , mConnectingRoutesCache(nullptr)
38 : : {
39 : 214 : mWorldModel.time_index = time_index;
40 : 214 : mWorldModel.default_ego_vehicle_rss_dynamics = defaultEgoRssDynamics;
41 : 214 : }
42 : :
43 : 28 : RssWorldModelCreation::RssWorldModelCreation(::ad::rss::world::TimeIndex const &time_index,
44 : : ::ad::rss::world::RssDynamics const &defaultEgoRssDynamics,
45 : 28 : ConnectingRoutesCache &connecting_routes_cache)
46 : 28 : : mFinalized(false)
47 : 28 : , mWorldModelLock()
48 : 28 : , mConnectingRoutesCache(&connecting_routes_cache)
49 : : {
50 : 28 : mWorldModel.time_index = time_index;
51 : 28 : mWorldModel.default_ego_vehicle_rss_dynamics = defaultEgoRssDynamics;
52 : 28 : }
53 : :
54 : 242 : world::WorldModel RssWorldModelCreation::getWorldModel()
55 : : {
56 [ + - ]: 242 : const std::lock_guard<std::mutex> lock(mWorldModelLock);
57 [ - + ]: 242 : if (mFinalized)
58 : : {
59 [ # # # # : 0 : getLogger()->error("RssWorldModelCreation::getWorldModel[:{}]>> error world model already finalized.", mRouteId);
# # ]
60 [ # # ]: 0 : return world::WorldModel();
61 : : }
62 : :
63 : 242 : mFinalized = true;
64 : 242 : return std::move(mWorldModel);
65 : 242 : }
66 : :
67 : 217 : bool RssWorldModelCreation::appendConstellations(RssObjectData const &egoObjectData,
68 : : ::ad::map::route::FullRoute const &egoRouteInput,
69 : : RssObjectData const &otherObjectData,
70 : : RssRestrictSpeedLimitMode const &restrict_speed_limit_mode,
71 : : ::ad::map::landmark::LandmarkIdSet const &greenTrafficLights,
72 : : map::RssConstellationCreationMode const &mode,
73 : : ::ad::map::lane::LaneIdSet const &relevantLanes)
74 : : {
75 : 217 : RssRouteList egoObjectPredictionHints;
76 : 217 : RssRouteList otherObjectPredictionHints;
77 [ + - ]: 217 : return appendConstellations(egoObjectData,
78 : : egoRouteInput,
79 : : otherObjectData,
80 : : restrict_speed_limit_mode,
81 : : greenTrafficLights,
82 : : mode,
83 : : egoObjectPredictionHints,
84 : : otherObjectPredictionHints,
85 : 434 : relevantLanes);
86 : 217 : }
87 : :
88 : 261 : bool RssWorldModelCreation::appendConstellations(RssObjectData const &egoObjectData,
89 : : ::ad::map::route::FullRoute const &egoRouteInput,
90 : : RssObjectData const &otherObjectData,
91 : : RssRestrictSpeedLimitMode const &restrict_speed_limit_mode,
92 : : ::ad::map::landmark::LandmarkIdSet const &greenTrafficLights,
93 : : map::RssConstellationCreationMode const &mode,
94 : : RssRouteList const &egoObjectPredictionHints,
95 : : RssRouteList const &otherObjectPredictionHints,
96 : : ::ad::map::lane::LaneIdSet const &relevantLanes)
97 : : {
98 [ - + ]: 261 : if (mFinalized)
99 : : {
100 [ # # # # ]: 0 : getLogger()->error("RssWorldModelCreation::appendConstellations[{}->{}:{}]>> error world model already finalized.",
101 : 0 : egoObjectData.id,
102 : 0 : otherObjectData.id,
103 [ # # ]: 0 : mRouteId);
104 : 0 : return false;
105 : : }
106 : :
107 [ - + ]: 261 : if (mode == map::RssConstellationCreationMode::Ignore)
108 : : {
109 : 0 : return true;
110 : : }
111 : :
112 [ + - ]: 261 : auto egoObject = std::make_shared<RssObjectConversion const>(egoObjectData);
113 [ + - ]: 261 : auto otherObject = std::make_shared<RssObjectConversion const>(otherObjectData);
114 [ + - - + : 261 : if (!bool(egoObject) || !bool(otherObject))
- + ]
115 : : {
116 [ # # # # ]: 0 : getLogger()->error("RssWorldModelCreation::appendConstellations[{}->{}:{}]>> error allocating object data.",
117 : 0 : egoObjectData.id,
118 : 0 : otherObjectData.id,
119 [ # # ]: 0 : mRouteId);
120 : 0 : return false;
121 : : }
122 [ + - ]: 261 : RssWorldModelCreator constellationCreator(restrict_speed_limit_mode, greenTrafficLights, *this);
123 : :
124 [ - + ]: 261 : if (mode == map::RssConstellationCreationMode::NotRelevant)
125 : : {
126 [ # # ]: 0 : return constellationCreator.appendNotRelevantConstellation(egoRouteInput, egoObject, otherObject);
127 : : }
128 : : else
129 : : {
130 : 261 : physics::Distance minEgoStoppingDistance;
131 [ + - - + ]: 261 : if (!egoObject->calculateConservativeMinStoppingDistance(minEgoStoppingDistance))
132 : : {
133 [ # # # # ]: 0 : getLogger()->error("RssWorldModelCreation::appendConstellations[{}->{}:{}]>> error calculating ego min "
134 : : "stopping distance",
135 [ # # # # ]: 0 : egoObject->getId(),
136 [ # # ]: 0 : otherObject->getId(),
137 : 0 : mRouteId);
138 : 0 : return false;
139 : : }
140 : 261 : physics::Distance minOtherStoppingDistance;
141 [ + - - + ]: 261 : if (!otherObject->calculateConservativeMinStoppingDistance(minOtherStoppingDistance))
142 : : {
143 [ # # # # ]: 0 : getLogger()->error("RssWorldModelCreation::appendConstellations[{}->{}:{}]>> error calculating other "
144 : : "min stopping distance",
145 [ # # # # ]: 0 : egoObject->getId(),
146 [ # # ]: 0 : otherObject->getId(),
147 : 0 : mRouteId);
148 : 0 : return false;
149 : : }
150 : :
151 : : // consider the sum of both stopping distances and some minimum distance
152 : : auto const maxObjectDistance
153 [ + - + - ]: 261 : = std::max(minEgoStoppingDistance + minOtherStoppingDistance, mMinimumDistanceToObjectsThatHaveToBeAnalyzed);
154 : :
155 [ + - + - : 261 : auto const roughObjectDistance = egoObject->getDistanceEstimate(otherObject) - egoObject->getVehicleLength() / 2.
+ - + - ]
156 [ + - + - : 522 : - egoObject->getVehicleWidth() / 2. - otherObject->getVehicleLength() / 2. - otherObject->getVehicleWidth() / 2.;
+ - + - +
- + - + -
+ - + - ]
157 : : // early check on the object distance
158 [ + - + + ]: 261 : if (roughObjectDistance > maxObjectDistance)
159 : : {
160 [ + - + - ]: 6 : getLogger()->debug("RssWorldModelCreation::appendConstellations[{}->{}:{}]>> object distance {} larger than {} "
161 : : "meters: not relevant",
162 [ + - + - ]: 3 : egoObject->getId(),
163 [ + - ]: 6 : otherObject->getId(),
164 : 3 : mRouteId,
165 : : roughObjectDistance,
166 : : maxObjectDistance);
167 : : auto const appendResult
168 [ + - ]: 3 : = constellationCreator.appendNotRelevantConstellation(egoRouteInput, egoObject, otherObject);
169 : 3 : return appendResult;
170 : : }
171 : :
172 [ + + ]: 258 : if (mode == map::RssConstellationCreationMode::Unstructured)
173 : : {
174 [ + - ]: 2 : return constellationCreator.appendUnstructuredConstellation(egoObject, otherObject);
175 : : }
176 : : else
177 : : {
178 [ + - ]: 256 : return appendStructuredConstellations(constellationCreator,
179 : : egoObject,
180 : : egoRouteInput,
181 : : otherObject,
182 : : egoObjectPredictionHints,
183 : : otherObjectPredictionHints,
184 : : relevantLanes,
185 : 256 : maxObjectDistance);
186 : : }
187 : : }
188 : 261 : }
189 : :
190 : : // resize the routes in length and width if required
191 : :
192 : : enum class ResizeRoutePrefixMode
193 : : {
194 : : KeepRoutePrefix,
195 : : ShortenRoutePrefix
196 : : };
197 : :
198 : 637 : ::ad::map::route::FullRouteList resizeRoutesIfRequired(RssRouteList const &rssRoutes,
199 : : physics::Distance const expectedRoutePreviewDistance,
200 : : ad::map::route::RouteCreationMode const route_creation_mode,
201 : : ::ad::map::lane::LaneIdSet const &relevantLanes,
202 : : ResizeRoutePrefixMode const prefix_mode
203 : : = ResizeRoutePrefixMode::KeepRoutePrefix)
204 : : {
205 : 637 : ::ad::map::route::FullRouteList expandedRoutes;
206 [ + + ]: 884 : for (auto const &rss_route : rssRoutes)
207 : : {
208 [ + - ]: 247 : auto expanded_route = ad::map::route::getRouteExpandedTo(rss_route.route, route_creation_mode);
209 : : // handle potential intersection prefix
210 [ + + ]: 247 : if (prefix_mode == ResizeRoutePrefixMode::ShortenRoutePrefix)
211 : : {
212 [ + - ]: 65 : ::ad::map::route::shortenRoute(rss_route.progress_on_route, expanded_route);
213 : : }
214 : :
215 : : // in case the route is not long enough, we have to extend is
216 : 247 : ::ad::map::route::FullRouteList additionalRoutes;
217 [ + - ]: 247 : auto const desiredRouteLength = rss_route.progress_on_route + expectedRoutePreviewDistance;
218 [ + - ]: 247 : ::ad::map::route::extendRouteToDistance(expanded_route, desiredRouteLength, additionalRoutes, relevantLanes);
219 : :
220 : : // and shorten if it's too long and add to results
221 [ + - ]: 247 : ::ad::map::route::shortenRouteToDistance(expanded_route, desiredRouteLength);
222 [ + - ]: 247 : expandedRoutes.push_back(expanded_route);
223 : : // shorten potential additional routes and add to results
224 [ + + ]: 273 : for (auto &additionalRoute : additionalRoutes)
225 : : {
226 [ + - ]: 26 : ::ad::map::route::shortenRouteToDistance(additionalRoute, desiredRouteLength);
227 [ + - ]: 26 : expandedRoutes.push_back(additionalRoute);
228 : : }
229 : 247 : }
230 : :
231 : 637 : return expandedRoutes;
232 : 0 : }
233 : :
234 : 162 : ::ad::rss::map::RssRoute createRssRoute(::ad::map::route::FullRoute const &route,
235 : : ::ad::map::match::Object const &match_object)
236 : : {
237 [ + - ]: 162 : ::ad::rss::map::RssRoute rss_route;
238 [ + - ]: 162 : rss_route.route = route;
239 : 162 : rss_route.progress_on_route = ad::physics::Distance(0.);
240 [ + - ]: 162 : auto findWaypointResult = ::ad::map::route::findCenterWaypoint(match_object, route);
241 [ + - ]: 162 : if (findWaypointResult.isValid())
242 : : {
243 [ + - ]: 162 : rss_route.progress_on_route = ::ad::map::route::calcLength(findWaypointResult);
244 : : }
245 : 324 : return rss_route;
246 : 0 : }
247 : :
248 : 256 : bool RssWorldModelCreation::appendStructuredConstellations(
249 : : map::RssWorldModelCreator &constellationCreator,
250 : : std::shared_ptr<RssObjectConversion const> const &egoObject,
251 : : ::ad::map::route::FullRoute const &egoRouteInput,
252 : : std::shared_ptr<RssObjectConversion const> const &otherObject,
253 : : RssRouteList const &egoObjectPredictionHints,
254 : : RssRouteList const &otherObjectPredictionHints,
255 : : ::ad::map::lane::LaneIdSet const &relevantLanes,
256 : : ad::physics::Distance const &maxConnectingRouteDistance)
257 : : {
258 : 256 : bool result = false;
259 : 256 : bool constellationAppended = false;
260 : :
261 [ + - - + : 256 : if ((egoObject->getObjectMapMatchedPosition() == nullptr) || (otherObject->getObjectMapMatchedPosition() == nullptr))
- + ]
262 : : {
263 [ # # ]: 0 : getLogger()->error(
264 : : "RssWorldModelCreation::appendStructuredConstellations[{}->{}:{}]>> invalid RssObjectConversion entity passed",
265 [ # # # # ]: 0 : egoObject->getId(),
266 [ # # ]: 0 : otherObject->getId(),
267 : 0 : mRouteId);
268 : 0 : return false;
269 : : }
270 : :
271 : 256 : auto const &egoMatchObject = *egoObject->getObjectMapMatchedPosition();
272 : 256 : auto const &objectMatchObject = *otherObject->getObjectMapMatchedPosition();
273 : :
274 [ - + ]: 256 : if (egoMatchObject.map_matched_bounding_box.lane_occupied_regions.empty())
275 : : {
276 [ # # ]: 0 : getLogger()->warn(
277 : : "RssWorldModelCreation::appendStructuredConstellations[{}->{}:{}]>> ego without occupied regions skipping.",
278 [ # # # # ]: 0 : egoObject->getId(),
279 [ # # ]: 0 : otherObject->getId(),
280 : 0 : mRouteId);
281 : 0 : return false;
282 : : }
283 [ - + ]: 256 : if (objectMatchObject.map_matched_bounding_box.lane_occupied_regions.empty())
284 : : {
285 [ # # ]: 0 : getLogger()->warn(
286 : : "RssWorldModelCreation::appendStructuredConstellations[{}->{}:{}]>> object without occupied regions skipping.",
287 [ # # # # ]: 0 : egoObject->getId(),
288 [ # # ]: 0 : otherObject->getId(),
289 : 0 : mRouteId);
290 : 0 : return false;
291 : : }
292 : :
293 : : try
294 : : {
295 : 256 : ::ad::map::route::ConnectingRouteList connectingRoutes;
296 : 256 : bool calculateConnectingRoutes = true;
297 : : // make use of the connecting routes cache
298 [ + + ]: 256 : if (mConnectingRoutesCache != nullptr)
299 : : {
300 : 129 : calculateConnectingRoutes = !mConnectingRoutesCache->getConnectingRoutes(
301 [ + - + - : 43 : egoObject->getId(), otherObject->getId(), maxConnectingRouteDistance, connectingRoutes);
+ - ]
302 : : }
303 : :
304 [ + + ]: 256 : if (calculateConnectingRoutes)
305 : : {
306 : : // for calculation of the connecting route, the potential route prefix at intersection must be removed
307 : : // to prevent from creating of merging routes in the past (predictions are only used for merging route)
308 : : ::ad::map::route::FullRouteList egoPredictedRoutesConnectingRoutes
309 : : = resizeRoutesIfRequired(egoObjectPredictionHints,
310 : : maxConnectingRouteDistance,
311 : : ad::map::route::RouteCreationMode::AllRoutableLanes,
312 : : relevantLanes,
313 [ + - ]: 232 : ResizeRoutePrefixMode::ShortenRoutePrefix);
314 : : ::ad::map::route::FullRouteList objectPredictedRoutesConnectingRoutes
315 : : = resizeRoutesIfRequired(otherObjectPredictionHints,
316 : : maxConnectingRouteDistance,
317 : : ad::map::route::RouteCreationMode::AllRoutableLanes,
318 : : relevantLanes,
319 [ + - ]: 232 : ResizeRoutePrefixMode::ShortenRoutePrefix);
320 : : // Calculate shortest route (ignore driving direction) between the two objects MapMatchedObjectBoundingBox
321 [ + - ]: 464 : connectingRoutes = ::ad::map::route::planning::calculateConnectingRoutes(egoMatchObject,
322 : : objectMatchObject,
323 : : maxConnectingRouteDistance,
324 : : egoPredictedRoutesConnectingRoutes,
325 : : objectPredictedRoutesConnectingRoutes,
326 : 232 : relevantLanes);
327 [ + + ]: 232 : if (mConnectingRoutesCache != nullptr)
328 : : {
329 : 38 : mConnectingRoutesCache->addConnectingRoutes(
330 [ + - + - : 19 : egoObject->getId(), otherObject->getId(), connectingRoutes, maxConnectingRouteDistance);
+ - ]
331 : : }
332 : 232 : }
333 : :
334 : 256 : ad::physics::Distance maxConnectingRouteLength(0.);
335 [ + + ]: 561 : for (auto connectingRouteIter = connectingRoutes.begin(); connectingRouteIter != connectingRoutes.end();)
336 : : {
337 [ + - ]: 305 : auto const connectingRoute = *connectingRouteIter;
338 [ + - ]: 305 : auto const feasibility = ad::map::route::getHeadingFeasibility(connectingRoute);
339 : : // drop infeasible routes, but ensure to not drop the last one!
340 [ + + + - : 305 : if ((connectingRoutes.size() > 1u) && (feasibility < ad::physics::Probability(0.1)))
+ + + + ]
341 : : {
342 [ + - + - ]: 6 : getLogger()->trace(
343 : : "RssWorldModelCreation::appendStructuredConstellations[{}->{}:{}]>> ignoring infeasible connecting route "
344 : : "to object:\n {}",
345 [ + - + - ]: 3 : egoObject->getId(),
346 [ + - ]: 6 : otherObject->getId(),
347 : 3 : mRouteId,
348 : : connectingRoute);
349 [ + - ]: 3 : connectingRouteIter = connectingRoutes.erase(connectingRouteIter);
350 : : }
351 : : else
352 : : {
353 [ + - ]: 302 : auto const connectingRouteLength = ad::map::route::calcLength(connectingRoute);
354 [ + - ]: 302 : maxConnectingRouteLength = std::max(maxConnectingRouteLength, connectingRouteLength);
355 [ + - + - ]: 604 : getLogger()->trace(
356 : : "RssWorldModelCreation::appendStructuredConstellations[{}->{}:{}]>> connecting route of length {} "
357 : : "available to object:\n {}",
358 [ + - + - ]: 302 : egoObject->getId(),
359 [ + - ]: 604 : otherObject->getId(),
360 : 302 : mRouteId,
361 : : connectingRouteLength,
362 : : connectingRoute);
363 : 302 : connectingRouteIter++;
364 : : }
365 : 305 : }
366 : :
367 [ + + ]: 256 : if (connectingRoutes.empty())
368 : : {
369 [ + - + - ]: 76 : getLogger()->debug("RssWorldModelCreation::appendStructuredConstellations[{}->{}:{}]>> no connecting route "
370 : : "available to object within {} meters",
371 [ + - + - ]: 38 : egoObject->getId(),
372 [ + - ]: 76 : otherObject->getId(),
373 : 38 : mRouteId,
374 : : maxConnectingRouteDistance);
375 : : auto const appendResult
376 [ + - ]: 38 : = constellationCreator.appendNotRelevantConstellation(egoRouteInput, egoObject, otherObject);
377 : 38 : return appendResult;
378 : : }
379 : :
380 : : // prepare ego route
381 : : // ensure the prediction spans at least both vehicles: ego-length + connecting-route-length + other-length
382 : : // ensure the prediction consideres only the same driving direction, otherwhise also mainly opposite use-cases
383 : : // (e.g. ego-turning-right, other-from-right) will lead to actual intersection use-cases!
384 : : // Be aware: if the ego is located outside of its nominal driving lane, the computation might ignore it by this,
385 : : // because predictions could be incomplete
386 : : // @todo: add support for ego outside nominal routes
387 : : auto const predictionLength
388 [ + - + - : 218 : = egoObject->getVehicleLength() + maxConnectingRouteLength + otherObject->getVehicleLength();
+ - + - ]
389 : : // for the rest of the calculations below we stick to the current egoRouteInput if available
390 : 218 : ::ad::map::route::FullRouteList egoPredictedRoutes;
391 [ + + ]: 218 : if (!egoRouteInput.road_segments.empty())
392 : : {
393 [ + - ]: 162 : auto const rssRoute = createRssRoute(egoRouteInput, egoMatchObject);
394 [ + - + - : 486 : egoPredictedRoutes = resizeRoutesIfRequired(
+ - + + -
- ]
395 : 162 : {rssRoute}, predictionLength, ::ad::map::route::RouteCreationMode::SameDrivingDirection, relevantLanes);
396 : 162 : }
397 [ + - ]: 56 : else if (egoObjectPredictionHints.empty())
398 : : {
399 [ + - ]: 112 : egoPredictedRoutes = ::ad::map::route::planning::predictRoutesOnDistance(
400 : : egoMatchObject,
401 : : predictionLength,
402 : : ::ad::map::route::RouteCreationMode::SameDrivingDirection,
403 : : ::ad::map::route::planning::FilterDuplicatesMode::SubRoutesPreferLongerOnes,
404 : 56 : relevantLanes);
405 : : }
406 : : else
407 : : {
408 [ # # ]: 0 : egoPredictedRoutes = resizeRoutesIfRequired(egoObjectPredictionHints,
409 : : predictionLength,
410 : : ::ad::map::route::RouteCreationMode::SameDrivingDirection,
411 : 0 : relevantLanes);
412 : : }
413 : :
414 : : // reset the result to ensure we don't miss anything
415 : 218 : result = false;
416 [ + + ]: 520 : for (auto const &connectingRoute : connectingRoutes)
417 : : {
418 [ + - + - ]: 604 : getLogger()->trace("RssWorldModelCreation::appendStructuredConstellations[{}->{}:{}]>> process connecting route "
419 : : "available to object:\n {}",
420 [ + - + - ]: 302 : egoObject->getId(),
421 [ + - ]: 604 : otherObject->getId(),
422 : 302 : mRouteId,
423 : : connectingRoute);
424 : 302 : bool relevantFollowingConstellationFound = false;
425 [ + + ]: 302 : if (connectingRoute.type == ::ad::map::route::ConnectingRouteType::Following)
426 : : {
427 : : // vehicle following mode: it doesn't matter if there is an intersection in between or not
428 : : // as the cars might follow each other through the intersection
429 : :
430 : 222 : bool objectOnOneOfOurRoutes = false;
431 [ + + ]: 222 : if (connectingRoute.route_a.road_segments.empty())
432 : : {
433 : : // we are in front -> constellation has to be considered since the other might follow us in our route
434 : 88 : objectOnOneOfOurRoutes = true;
435 : : }
436 : : else
437 : : {
438 [ + + ]: 172 : for (auto const &egoRoute : egoPredictedRoutes)
439 : : {
440 : : auto const findObject
441 [ + - ]: 143 : = ::ad::map::route::objectOnRoute(objectMatchObject.map_matched_bounding_box, egoRoute);
442 [ + + ]: 143 : if (findObject.isValid())
443 : : {
444 : 105 : objectOnOneOfOurRoutes = true;
445 [ + - + - ]: 210 : getLogger()->trace(
446 : : "RssWorldModelCreation::appendStructuredConstellations[{}->{}:{}]>> driving in front on our route:\n"
447 : : " objectMatchObject({})\n egoRoute({})",
448 [ + - + - ]: 105 : egoObject->getId(),
449 [ + - ]: 210 : otherObject->getId(),
450 : 105 : mRouteId,
451 : : objectMatchObject,
452 : : egoRoute);
453 : 105 : break;
454 : : }
455 : : else
456 : : {
457 [ + - + - ]: 114 : getLogger()->trace("RssWorldModelCreation::appendStructuredConstellations[{}->{}:{}]>> driving in front "
458 : : "but not on route:\n"
459 : : " objectMatchObject({})\n egoRoute({})",
460 [ + - + - ]: 38 : egoObject->getId(),
461 [ + - ]: 76 : otherObject->getId(),
462 : 38 : mRouteId,
463 : : objectMatchObject,
464 : : egoRoute);
465 : : }
466 : : }
467 : : }
468 [ + + ]: 222 : if (objectOnOneOfOurRoutes)
469 : : {
470 [ + - + - ]: 386 : getLogger()->debug("RssWorldModelCreation::appendStructuredConstellations[{}->{}:{}]>> relevant following "
471 : : "connecting route found: create "
472 : : "same direction use-case\n {}",
473 [ + - + - ]: 193 : egoObject->getId(),
474 [ + - ]: 386 : otherObject->getId(),
475 : 193 : mRouteId,
476 : : connectingRoute);
477 : 386 : auto const appendResult = constellationCreator.appendNonIntersectionConstellation(
478 [ + - ]: 193 : connectingRoute, world::ConstellationType::SameDirection, egoObject, otherObject);
479 : 193 : result = appendResult;
480 : 193 : constellationAppended = appendResult;
481 : 193 : relevantFollowingConstellationFound = true;
482 : : }
483 : : else
484 : : {
485 : : // no relevant object found for the following use-case, but still intersection use-case has to be analyzed
486 : : }
487 : : }
488 [ + + ]: 302 : if (!relevantFollowingConstellationFound)
489 : : {
490 [ + - + + ]: 109 : if (!::ad::map::route::isConnectedRoutePartOfAnIntersection(connectingRoute))
491 : : {
492 [ - + ]: 14 : if (connectingRoute.type == ::ad::map::route::ConnectingRouteType::Following)
493 : : {
494 : : // no relevant object found for the following use-case, without intersection no problem
495 : 0 : result = true;
496 : : }
497 [ + - ]: 14 : else if (connectingRoute.type == ::ad::map::route::ConnectingRouteType::Opposing)
498 : : {
499 [ + - + - ]: 28 : getLogger()->debug("RssWorldModelCreation::appendStructuredConstellations[{}->{}:{}]>> no intersections on "
500 : : "opposing connecting "
501 : : "route found: create "
502 : : "opposite direction use-case\n {}",
503 [ + - + - ]: 14 : egoObject->getId(),
504 [ + - ]: 28 : otherObject->getId(),
505 : 14 : mRouteId,
506 : : connectingRoute);
507 : 28 : auto const appendResult = constellationCreator.appendNonIntersectionConstellation(
508 [ + - ]: 14 : connectingRoute, world::ConstellationType::OppositeDirection, egoObject, otherObject);
509 : 14 : result = appendResult;
510 : 14 : constellationAppended = appendResult;
511 : : }
512 : : else
513 : : {
514 : : // ConnectingRouteType::Merging
515 : : // no intersection in between, but merging
516 : : // we interpret this now as intersection case
517 : : // @todo: analyze in more detail how such constellations have to be handled, keep as info message for now
518 [ # # # # ]: 0 : getLogger()->info("RssWorldModelCreation::appendStructuredConstellations[{}->{}:{}]>> no intersections on "
519 : : "merging connecting "
520 : : "route found: create "
521 : : "merging-intersection same prio use-case\n {}",
522 [ # # # # ]: 0 : egoObject->getId(),
523 [ # # ]: 0 : otherObject->getId(),
524 : 0 : mRouteId,
525 : : connectingRoute);
526 : 0 : auto const appendResult = constellationCreator.appendMergingConstellation(
527 [ # # ]: 0 : connectingRoute, world::ConstellationType::IntersectionSamePriority, egoObject, otherObject);
528 : 0 : result = appendResult;
529 : 0 : constellationAppended = appendResult;
530 : : }
531 : : }
532 : : else
533 : : {
534 : 95 : result = true;
535 : : // case: ConnectingRouteType::Following with intersections
536 : : // case: ConnectingRouteType::Opposing with intersections
537 : : // or: ConnectingRouteType::Merging with intersections
538 : 95 : bool objectPredictedRoutesProcessed = false;
539 : 95 : ::ad::map::route::FullRouteList objectPredictedRoutes;
540 [ + + ]: 198 : for (auto const &egoRoute : egoPredictedRoutes)
541 : : {
542 [ + - + - ]: 206 : getLogger()->trace(
543 : : "RssWorldModelCreation::appendStructuredConstellations[{}->{}:{}]>> investigate egoRoute({})",
544 [ + - + - ]: 103 : egoObject->getId(),
545 [ + - ]: 206 : otherObject->getId(),
546 : 103 : mRouteId,
547 : : egoRoute);
548 : : // @todo: intersection creation has to support also creation of already entered intersections
549 : : // to get intersections currently driving in also considered appropriate!!
550 : : // The below is calculated wrong if there is -- besides the currently ignored entered intersection another
551 : : // one
552 : : // ...
553 : : //
554 [ + - ]: 103 : auto intersectionsOnEgoRoute = ::ad::map::intersection::Intersection::getIntersectionsForRoute(egoRoute);
555 : 103 : if (intersectionsOnEgoRoute.empty()
556 [ + + - + : 103 : && (connectingRoute.type == ::ad::map::route::ConnectingRouteType::Merging))
- + ]
557 : : {
558 [ # # # # ]: 0 : getLogger()->debug(
559 : : "RssWorldModelCreation::appendStructuredConstellations[{}->{}:{}]>> no intersections on "
560 : : "route found: create"
561 : : "merging-intersection same prio use-case:\n egoRoute({})",
562 [ # # # # ]: 0 : egoObject->getId(),
563 [ # # ]: 0 : otherObject->getId(),
564 : 0 : mRouteId,
565 : : egoRoute);
566 : 0 : auto const appendResult = constellationCreator.appendMergingConstellation(
567 [ # # ]: 0 : connectingRoute, world::ConstellationType::IntersectionSamePriority, egoObject, otherObject);
568 [ # # # # ]: 0 : result = result && appendResult;
569 : 0 : constellationAppended |= appendResult;
570 : : }
571 : 103 : else if (intersectionsOnEgoRoute.empty()
572 [ + + - + : 103 : && (connectingRoute.type == ::ad::map::route::ConnectingRouteType::Following))
+ - ]
573 : : {
574 : : // no intersection for the following use-case, then irrelevant
575 : : }
576 : : else
577 : : {
578 [ + + ]: 103 : if (!objectPredictedRoutesProcessed)
579 : : {
580 : 95 : objectPredictedRoutesProcessed = true;
581 : : // ensure the prediction consideres only the same driving direction, otherwhise also mainly opposite
582 : : // use-cases (e.g. ego-turning-right, other-from-right) will lead to actual intersection use-cases! Be
583 : : // aware: if the object is located outside of its nominal driving lane, the computation might ignore it
584 : : // by this, because predictions might be incomplete
585 : : // @todo: add support for objects outside nominal routes
586 [ + + ]: 95 : if (otherObjectPredictionHints.empty())
587 : : {
588 : : // no hints, so predict on our own
589 [ + - ]: 168 : objectPredictedRoutes = ::ad::map::route::planning::predictRoutesOnDistance(
590 : : objectMatchObject,
591 : : predictionLength,
592 : : ::ad::map::route::RouteCreationMode::SameDrivingDirection,
593 : : ::ad::map::route::planning::FilterDuplicatesMode::SubRoutesPreferLongerOnes,
594 : 84 : relevantLanes);
595 : : }
596 : : else
597 : : {
598 : : objectPredictedRoutes
599 [ + - ]: 22 : = resizeRoutesIfRequired(otherObjectPredictionHints,
600 : : predictionLength,
601 : : ::ad::map::route::RouteCreationMode::SameDrivingDirection,
602 : 11 : relevantLanes);
603 : : }
604 : : }
605 [ + + ]: 314 : for (auto const &objectRoute : objectPredictedRoutes)
606 : : {
607 [ + - + - ]: 422 : getLogger()->trace(
608 : : "RssWorldModelCreation::appendStructuredConstellations[{}->{}:{}]>> investigate objectRoute({})",
609 [ + - + - ]: 211 : egoObject->getId(),
610 [ + - ]: 422 : otherObject->getId(),
611 : 211 : mRouteId,
612 : : objectRoute);
613 [ + - ]: 211 : auto intersectionsOnRoute = intersectionsOnEgoRoute;
614 : 211 : auto const *intersectionOtherRoute = &objectRoute;
615 [ + + ]: 211 : if (intersectionsOnRoute.empty())
616 : : {
617 : : // no intersection on ego route, but within opposing connectingRoute
618 : : // Currently this can happen if the ego vehicle route starts inside the intersection,
619 : : // and the object is outside, let's then take the intersections on the object route
620 [ + - ]: 16 : intersectionsOnRoute = ::ad::map::intersection::Intersection::getIntersectionsForRoute(objectRoute);
621 [ - + ]: 16 : if (intersectionsOnRoute.empty())
622 : : {
623 : : // looks like not a straight forward road layout, but
624 : : // need to ensure we don't miss this constellation
625 : : // @todo: analyze when this can happen and if the created constellation is appropriate
626 [ # # # # ]: 0 : getLogger()->debug(
627 : : "RssWorldModelCreation::appendStructuredConstellations[{}->{}:{}]>> no intersections "
628 : : "on route found: create "
629 : : "opposite direction use-case:\n"
630 : : " egoRoute({})",
631 [ # # # # ]: 0 : egoObject->getId(),
632 [ # # ]: 0 : otherObject->getId(),
633 : 0 : mRouteId,
634 : : egoRoute);
635 : 0 : auto const appendResult = constellationCreator.appendNonIntersectionConstellation(
636 [ # # ]: 0 : connectingRoute, world::ConstellationType::OppositeDirection, egoObject, otherObject);
637 [ # # # # ]: 0 : result = result && appendResult;
638 : 0 : constellationAppended |= appendResult;
639 : : }
640 : : else
641 : : {
642 : 16 : intersectionOtherRoute = &egoRoute;
643 [ + - + - ]: 48 : getLogger()->trace("RssWorldModelCreation::appendStructuredConstellations[{}->{}:{}]>> no "
644 : : "intersection on ego route, but"
645 : : "opposite connecting route; checking intersections in object route:\n"
646 : : " egoRoute({})\n objectRoute({})",
647 [ + - + - ]: 16 : egoObject->getId(),
648 [ + - ]: 32 : otherObject->getId(),
649 : 16 : mRouteId,
650 : : egoRoute,
651 : : objectRoute);
652 : : }
653 : : }
654 : :
655 [ + + ]: 422 : for (auto const &intersection : intersectionsOnRoute)
656 : : {
657 [ + - + + ]: 211 : if (!intersection->objectRouteCrossesIntersection(*intersectionOtherRoute))
658 : : {
659 [ + - + - ]: 90 : getLogger()->trace(
660 : : "RssWorldModelCreation::appendStructuredConstellations[{}->{}:{}]>> found object "
661 : : "route not crossing "
662 : : "intersection on route:\n"
663 : : " egoRoute({})\n objectRoute({})\n intersection({})",
664 [ + - + - ]: 30 : egoObject->getId(),
665 [ + - ]: 60 : otherObject->getId(),
666 : 30 : mRouteId,
667 : : egoRoute,
668 : : objectRoute,
669 [ + - ]: 30 : intersection->entryParaPoints());
670 : : }
671 [ + - + + ]: 181 : else if (intersection->objectRouteFromSameArmAsIntersectionRoute(*intersectionOtherRoute))
672 : : {
673 [ + + ]: 16 : if (connectingRoute.type == ::ad::map::route::ConnectingRouteType::Opposing)
674 : : {
675 : : // This happens e.g. if the object has an opposite connecting route, but the current considered
676 : : // prediction starts on ego intersection arm and the object route is still touching the ego's
677 : : // route could be modeled as following case, but then we would require the actual following route.
678 : : // And the object should still be safely considered by the actual opposite case predictions.
679 : : // -> this case is just ignored for now: requires more thorough analysis in future
680 [ + - + - ]: 9 : getLogger()->trace(
681 : : "RssWorldModelCreation::appendStructuredConstellations[{}->{}:{}]>> object route from "
682 : : "same arm as intersection and opposing connecting route: ignore\n"
683 : : " egoRoute({})\n objectRoute({})\n intersection({})\n {}",
684 [ + - + - ]: 3 : egoObject->getId(),
685 [ + - ]: 6 : otherObject->getId(),
686 : 3 : mRouteId,
687 : : egoRoute,
688 : : objectRoute,
689 [ + - ]: 3 : intersection->entryParaPoints(),
690 : : connectingRoute);
691 : : }
692 [ - + ]: 13 : else if (connectingRoute.type == ::ad::map::route::ConnectingRouteType::Merging)
693 : : {
694 : : // This can happen if the intersection is not necessarily the first intersection of both vehicles
695 : : // and so being a secondary merging use-case at that far intersection
696 : : // Therefore, later this constellation might switch to an actual merging connected route or
697 : : // following use case from above. Now: Create an intersection constellation
698 : : // @todo: find out more here: is it required now to search the two routes for a merge even before
699 : : // the intersection?
700 [ # # # # ]: 0 : getLogger()->debug(
701 : : "RssWorldModelCreation::appendStructuredConstellations[{}->{}:{}]>> object route from "
702 : : "same arm as intersection "
703 : : "looks like merging constellation far in future: create intersection constellation\n"
704 : : " egoRoute({})\n objectRoute({})\n intersection({})\n {}",
705 [ # # # # ]: 0 : egoObject->getId(),
706 [ # # ]: 0 : otherObject->getId(),
707 : 0 : mRouteId,
708 : : egoRoute,
709 : : objectRoute,
710 [ # # ]: 0 : intersection->entryParaPoints(),
711 : : connectingRoute);
712 [ # # ]: 0 : auto const appendResult = constellationCreator.appendIntersectionConstellation(
713 : : intersection, egoRoute, objectRoute, *intersectionOtherRoute, egoObject, otherObject);
714 [ # # # # ]: 0 : result = result && appendResult;
715 : 0 : constellationAppended |= appendResult;
716 : : }
717 : : else
718 : : {
719 : : // intersection following use-case, should have been found in the following use case above
720 : : // ignore for now
721 [ + - + - ]: 39 : getLogger()->trace(
722 : : "RssWorldModelCreation::appendStructuredConstellations[{}->{}:{}]>> object route from "
723 : : "same arm as intersection and follwing connecting route: ignore\n"
724 : : " egoRoute({})\n objectRoute({})\n intersection({})\n {}",
725 [ + - + - ]: 13 : egoObject->getId(),
726 [ + - ]: 26 : otherObject->getId(),
727 : 13 : mRouteId,
728 : : egoRoute,
729 : : objectRoute,
730 [ + - ]: 13 : intersection->entryParaPoints(),
731 : : connectingRoute);
732 : : }
733 : : }
734 [ + - ]: 165 : else if (intersection->objectRouteOppositeToIntersectionRoute(*intersectionOtherRoute)
735 [ + + + - : 165 : || !intersection->objectRouteCrossesIntersectionRoute(*intersectionOtherRoute))
+ + + + ]
736 : : {
737 [ + + ]: 134 : if (connectingRoute.type == ::ad::map::route::ConnectingRouteType::Opposing)
738 : : {
739 : : // here, the connecting route should be correct; otherwise we would not oppose to each other
740 [ + - + - ]: 242 : getLogger()->debug(
741 : : "RssWorldModelCreation::appendStructuredConstellations[{}->{}:{}]>> object route "
742 : : "opposite and not crossing: "
743 : : "opposite direction constellation:\n"
744 : : " egoRoute({})\n objectRoute({})\n intersection({})",
745 [ + - + - ]: 121 : egoObject->getId(),
746 [ + - ]: 242 : otherObject->getId(),
747 : 121 : mRouteId,
748 : : egoRoute,
749 : : objectRoute,
750 [ + - ]: 121 : intersection->entryParaPoints());
751 : 242 : auto const appendResult = constellationCreator.appendNonIntersectionConstellation(
752 [ + - ]: 121 : connectingRoute, world::ConstellationType::OppositeDirection, egoObject, otherObject);
753 [ + - + - ]: 121 : result = result && appendResult;
754 : 121 : constellationAppended |= appendResult;
755 : : }
756 [ - + ]: 13 : else if (connectingRoute.type == ::ad::map::route::ConnectingRouteType::Merging)
757 : : {
758 : : // This can happen if the cars are too far away from each other for creating an opposing
759 : : // connecting route, but we find two predictions of the two vehicles ending at the same
760 : : // intersection
761 : : // Therefore, later this constellation might switch to the opposite type above.
762 : : // Now: Create an intersection constellation
763 [ # # # # ]: 0 : getLogger()->debug(
764 : : "RssWorldModelCreation::appendStructuredConstellations[{}->{}:{}]>> object route "
765 : : "opposite and not crossing, but "
766 : : "merging constellation: create "
767 : : "intersection constellation\n"
768 : : " egoRoute({})\n objectRoute({})\n intersection({})\n {}",
769 [ # # # # ]: 0 : egoObject->getId(),
770 [ # # ]: 0 : otherObject->getId(),
771 : 0 : mRouteId,
772 : : egoRoute,
773 : : objectRoute,
774 [ # # ]: 0 : intersection->entryParaPoints(),
775 : : connectingRoute);
776 [ # # ]: 0 : auto const appendResult = constellationCreator.appendIntersectionConstellation(
777 : : intersection, egoRoute, objectRoute, *intersectionOtherRoute, egoObject, otherObject);
778 [ # # # # ]: 0 : result = result && appendResult;
779 : 0 : constellationAppended |= appendResult;
780 : : }
781 : : else
782 : : {
783 : : // intersection following use-case, but object route doesn not cross, strange.
784 : : // ignore for now
785 [ + - + - ]: 39 : getLogger()->trace(
786 : : "RssWorldModelCreation::appendStructuredConstellations[{}->{}:{}]>> object route "
787 : : "opposite and not crossing: ignore"
788 : : " egoRoute({})\n objectRoute({})\n intersection({})\n {}",
789 [ + - + - ]: 13 : egoObject->getId(),
790 [ + - ]: 26 : otherObject->getId(),
791 : 13 : mRouteId,
792 : : egoRoute,
793 : : objectRoute,
794 [ + - ]: 13 : intersection->entryParaPoints(),
795 : : connectingRoute);
796 : : }
797 : : }
798 : : else
799 : : {
800 [ + - + - ]: 62 : getLogger()->debug(
801 : : "RssWorldModelCreation::appendStructuredConstellations[{}->{}:{}]>> object route "
802 : : "crosses intersection route: "
803 : : "intersection constellation\n"
804 : : " egoRoute({})\n objectRoute({})\n intersectionLanesOnRoute({})\n intersection({})",
805 [ + - + - ]: 31 : egoObject->getId(),
806 [ + - ]: 62 : otherObject->getId(),
807 : 31 : mRouteId,
808 : : egoRoute,
809 : : objectRoute,
810 [ + - ]: 31 : intersection->paraPointsOnRoute(),
811 [ + - ]: 31 : intersection->entryParaPoints());
812 [ + - ]: 31 : auto const appendResult = constellationCreator.appendIntersectionConstellation(
813 : : intersection, egoRoute, objectRoute, *intersectionOtherRoute, egoObject, otherObject);
814 [ + - + - ]: 31 : result = result && appendResult;
815 : 31 : constellationAppended |= appendResult;
816 : : }
817 : : }
818 : 211 : }
819 : : }
820 : 103 : }
821 : 95 : }
822 : : }
823 : : }
824 [ + + ]: 256 : }
825 [ - - ]: 0 : catch (std::exception const &e)
826 : : {
827 [ - - - - ]: 0 : getLogger()->error(
828 : : "RssWorldModelCreation::appendStructuredConstellations[{}->{}:{}]>> Failed. Catched exception: {}",
829 [ - - - - ]: 0 : egoObject->getId(),
830 [ - - ]: 0 : otherObject->getId(),
831 : 0 : mRouteId,
832 : 0 : e.what());
833 : 0 : }
834 : :
835 [ + + ]: 218 : if (!constellationAppended)
836 : : {
837 [ + - ]: 27 : if (result)
838 : : {
839 [ + - ]: 81 : getLogger()->debug("RssWorldModelCreation::appendStructuredConstellations[{}->{}:{}]>> detailed analysis did not "
840 : : "find any relevant "
841 : : "constellation. Append non relevant constellation\n"
842 : : " objectMatchObject({})\n egoMatchObject({})\n egoRoute({})",
843 [ + - + - ]: 27 : egoObject->getId(),
844 [ + - ]: 54 : otherObject->getId(),
845 : 27 : mRouteId,
846 : : objectMatchObject,
847 : : egoMatchObject,
848 : : egoRouteInput);
849 : : }
850 : : else
851 : : {
852 [ # # ]: 0 : getLogger()->info("RssWorldModelCreation::appendStructuredConstellations[{}->{}:{}]>> an operation failed and no "
853 : : "constellation could have been "
854 : : "created. Try to append non relevant constellation\n"
855 : : " objectMatchObject({})\n egoMatchObject({})\n egoRoute({})",
856 [ # # # # ]: 0 : egoObject->getId(),
857 [ # # ]: 0 : otherObject->getId(),
858 : 0 : mRouteId,
859 : : objectMatchObject,
860 : : egoMatchObject,
861 : : egoRouteInput);
862 : : }
863 : : auto const appendResult
864 [ + - ]: 27 : = constellationCreator.appendNotRelevantConstellation(egoRouteInput, egoObject, otherObject);
865 [ + - + - ]: 27 : result = result && appendResult;
866 : : }
867 : :
868 : 218 : return result;
869 : : }
870 : :
871 : 3 : ::ad::map::route::FullRoute shortenRoadBoundariesRouteIfRequired(::ad::map::route::FullRoute const &route,
872 : : ::ad::map::match::Object const &match_object,
873 : : physics::Distance const expectedRoutePreviewDistance)
874 : : {
875 [ + - ]: 3 : ::ad::map::route::FullRoute shortenedRoute = route;
876 [ + - ]: 3 : auto findWaypointResult = ::ad::map::route::findCenterWaypoint(match_object, shortenedRoute);
877 [ + - ]: 3 : if (findWaypointResult.isValid())
878 : : {
879 [ + - ]: 3 : auto const progress_on_route = ::ad::map::route::calcLength(findWaypointResult);
880 : : // remove potential intersection prefix
881 [ + - ]: 3 : ::ad::map::route::shortenRoute(progress_on_route, shortenedRoute);
882 : : // and shorten if it's too long
883 [ + - ]: 3 : ::ad::map::route::shortenRouteToDistance(
884 : : shortenedRoute, expectedRoutePreviewDistance, ::ad::map::route::ShortenRouteToDistanceMode::AllowCutIntersection);
885 : : }
886 : 6 : return shortenedRoute;
887 : 0 : }
888 : :
889 : 34 : bool RssWorldModelCreation::appendRoadBoundaries(RssObjectData const &egoObjectData,
890 : : ::ad::map::route::FullRoute const &inputRoute,
891 : : RssAppendRoadBoundariesMode const operationMode)
892 : : {
893 [ - + ]: 34 : if (mFinalized)
894 : : {
895 [ # # ]: 0 : getLogger()->error("RssWorldModelCreation::appendRoadBoundaries[{}:{}]>> error world model already finalized.",
896 : 0 : egoObjectData.id,
897 [ # # ]: 0 : mRouteId);
898 : 0 : return false;
899 : : }
900 [ + + ]: 34 : if (operationMode == RssAppendRoadBoundariesMode::Off)
901 : : {
902 : 28 : return true;
903 : : }
904 [ + + ]: 6 : if (egoObjectData.match_object.map_matched_bounding_box.lane_occupied_regions.empty())
905 : : {
906 [ + - ]: 2 : getLogger()->warn("RssWorldModelCreation::appendRoadBoundaries[{}:{}]>> ego without occupied regions skipping.",
907 : 1 : egoObjectData.id,
908 [ + - ]: 1 : mRouteId);
909 : 1 : return false;
910 : : }
911 : :
912 [ + + ]: 5 : if (inputRoute.road_segments.empty())
913 : : {
914 : 1 : return false;
915 : : }
916 : 4 : bool result = false;
917 : : try
918 : : {
919 [ + + ]: 4 : auto egoObject = std::make_shared<RssObjectConversion const>(egoObjectData);
920 [ - + ]: 3 : if (!bool(egoObject))
921 : : {
922 [ # # # # ]: 0 : getLogger()->error("RssWorldModelCreation::appendRoadBoundaries[{}:{}]>> error allocating object data.",
923 : 0 : egoObjectData.id,
924 [ # # ]: 0 : mRouteId);
925 : 0 : return false;
926 : : }
927 : :
928 : : // for the analysis we are only interested in the near term route
929 : : ::ad::map::route::FullRoute route = shortenRoadBoundariesRouteIfRequired(
930 [ + - ]: 3 : inputRoute, *egoObject->getObjectMapMatchedPosition(), cNearTermRoadBoundariesRouteLength);
931 : :
932 [ + + + - ]: 3 : switch (operationMode)
933 : : {
934 : 1 : case RssAppendRoadBoundariesMode::RouteOnly:
935 : : {
936 [ - + ]: 1 : if (route.route_creation_mode != ::ad::map::route::RouteCreationMode::SameDrivingDirection)
937 : : {
938 [ # # ]: 0 : route = ::ad::map::route::getRouteExpandedToSameDrivingDirectionLanesOnly(route);
939 : : }
940 : 1 : break;
941 : : }
942 : 1 : case RssAppendRoadBoundariesMode::ExpandRouteToOppositeLanes:
943 : : {
944 [ + - ]: 1 : if (route.route_creation_mode != ::ad::map::route::RouteCreationMode::AllRoutableLanes)
945 : : {
946 [ + - ]: 1 : route = ::ad::map::route::getRouteExpandedToOppositeLanes(route);
947 : : }
948 : 1 : break;
949 : : }
950 : 1 : case RssAppendRoadBoundariesMode::ExpandRouteToAllNeighbors:
951 : : {
952 [ + - ]: 1 : if (route.route_creation_mode != ::ad::map::route::RouteCreationMode::AllNeighborLanes)
953 : : {
954 [ + - ]: 1 : route = ::ad::map::route::getRouteExpandedToAllNeighborLanes(route);
955 : : }
956 : 1 : break;
957 : : }
958 : 0 : default:
959 : 0 : break;
960 : : }
961 : :
962 [ + - ]: 3 : RssWorldModelCreator constellationCreator(*this);
963 [ + - + - ]: 6 : getLogger()->debug("RssWorldModelCreation::appendRoadBoundaries[{}:{}]>>\n"
964 : : " egoRoute({})\n egoObject({})",
965 : 3 : egoObjectData.id,
966 [ + - ]: 3 : mRouteId,
967 : : route,
968 : : egoObjectData);
969 [ + - ]: 3 : result = constellationCreator.appendRoadBoundaryConstellations(route, egoObject);
970 [ + - ]: 3 : }
971 [ - + ]: 1 : catch (std::exception const &e)
972 : : {
973 [ + - + - ]: 2 : getLogger()->error("RssWorldModelCreation::appendRoadBoundaries[{}:{}]>> Failed. Catched exception: {}",
974 : 1 : egoObjectData.id,
975 : 1 : mRouteId,
976 [ + - ]: 2 : e.what());
977 : 1 : }
978 : 4 : return result;
979 : : }
980 : :
981 : 435 : bool RssWorldModelCreation::appendConstellationToWorldModel(world::Constellation const &constellation)
982 : : {
983 [ + - ]: 435 : const std::lock_guard<std::mutex> lock(mWorldModelLock);
984 [ - + ]: 435 : if (mFinalized)
985 : : {
986 [ # # # # ]: 0 : getLogger()->error(
987 [ # # ]: 0 : "RssWorldModelCreation::appendConstellationToWorldModel(:{})>> error world model already finalized.", mRouteId);
988 : 0 : return false;
989 : : }
990 : :
991 [ + - ]: 435 : mWorldModel.constellations.push_back(constellation);
992 : 435 : return true;
993 : 435 : }
994 : :
995 : : } // namespace map
996 : : } // namespace rss
997 : : } // namespace ad
|
