Branch data Line data Source code
1 : : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
2 : : //
3 : : // Copyright (C) 2019-2022 Intel Corporation
4 : : //
5 : : // SPDX-License-Identifier: LGPL-2.1-only
6 : : //
7 : : // ----------------- END LICENSE BLOCK -----------------------------------
8 : :
9 : : #include <ad/map/lane/Operation.hpp>
10 : : #include <ad/map/match/MapMatchedOperation.hpp>
11 : : #include <ad/map/route/Planning.hpp>
12 : : #include "ad/rss/map/Logging.hpp"
13 : : #include "ad/rss/map/RssObjectDataOperation.hpp"
14 : : #include "ad/rss/map/RssRouteCheckerControl.hpp"
15 : : #include "ad/rss/map/RssRouteCheckerData.hpp"
16 : :
17 : : namespace ad {
18 : : namespace rss {
19 : : namespace map {
20 : :
21 : 30 : template <class OBJECT_INSTANCE_TYPE> RssRouteCheckerVehicleData<OBJECT_INSTANCE_TYPE>::~RssRouteCheckerVehicleData()
22 : : {
23 : : // housekeeping of RssRouteCheckerControl
24 [ + + ]: 30 : if (mVehicleAdapter != nullptr)
25 : : {
26 : 10 : mVehicleAdapter->getCheckerControl()->dropVehicle(mRssObjectData.id);
27 : : }
28 : 30 : }
29 : :
30 : : enum class ReachedRoutingTargetsMode
31 : : {
32 : : eDISTANCE_ONLY,
33 : : eDISTANCE_AND_ORIENTATION
34 : : };
35 : :
36 : 26 : inline void dropReachedRoutingTargets(std::vector<::ad::map::point::ENUPoint> &routingTargets,
37 : : RssObjectData const &rssObjectData,
38 : : ad::physics::Distance const &reachedDistance,
39 : : ReachedRoutingTargetsMode reachedRoutingTargetsMode)
40 : : {
41 : : // removeReachedRoutingTargets
42 [ - + ]: 26 : while (!routingTargets.empty())
43 : : {
44 : 0 : auto const nextTarget = routingTargets.front();
45 : : auto const distanceToNextTarget
46 [ # # ]: 0 : = ad::map::point::distance(nextTarget, rssObjectData.match_object.enu_position.center_point);
47 [ # # # # ]: 0 : if (distanceToNextTarget < reachedDistance)
48 : : {
49 [ # # ]: 0 : routingTargets.erase(routingTargets.begin());
50 [ # # # # ]: 0 : getLogger()->debug(
51 : : "RssRouteCheckerVehicleData::removeReachedRoutingTargets[{}] Next target reached by distance: {}; "
52 : : "remaining targets: {}",
53 [ # # ]: 0 : rssObjectData.id,
54 : : nextTarget,
55 : : routingTargets);
56 : : }
57 : 0 : else if ((reachedRoutingTargetsMode == ReachedRoutingTargetsMode::eDISTANCE_AND_ORIENTATION)
58 [ # # # # : 0 : && (routingTargets.size() > 1u) && (distanceToNextTarget < 10. * reachedDistance))
# # # # #
# # # ]
59 : :
60 : : {
61 : : // if we have a new list of targets and there will be additional targets left if we drop this
62 : : // and the next target is already located behind the vehicle and not more than 10 time further away as
63 : : // reachedDistance, we also drop this; otherwhise the ego might try to plan more or less a round trip before
64 : : // finishing the target list
65 : : auto routingTargetHeading
66 [ # # ]: 0 : = ad::map::point::createENUHeading(rssObjectData.match_object.enu_position.center_point, nextTarget);
67 [ # # ]: 0 : auto headingDiffSigned = ad::physics::normalizeAngleSigned(ad::physics::Angle(
68 : 0 : routingTargetHeading.mENUHeading - rssObjectData.match_object.enu_position.heading.mENUHeading));
69 [ # # # # ]: 0 : if (std::fabs(headingDiffSigned) > ad::physics::cPI_2)
70 : : {
71 [ # # ]: 0 : routingTargets.erase(routingTargets.begin());
72 [ # # # # ]: 0 : getLogger()->debug(
73 : : "RssRouteCheckerVehicleData::removeReachedRoutingTargets[{}] Next target reached by orientation: {}, {}; "
74 : : "remaining targets: {}",
75 [ # # ]: 0 : rssObjectData.id,
76 : : nextTarget,
77 : : headingDiffSigned,
78 : : routingTargets);
79 : : }
80 : : else
81 : : {
82 : 0 : break;
83 : : }
84 : : }
85 : : else
86 : : {
87 : 0 : break;
88 : : }
89 : : }
90 : 26 : }
91 : :
92 : : template <class OBJECT_INSTANCE_TYPE>
93 : 26 : std::vector<::ad::map::point::ENUPoint> RssRouteCheckerVehicleData<OBJECT_INSTANCE_TYPE>::handleRoutingTargets()
94 : : {
95 [ + - ]: 26 : auto const routingTargetOperation = getAdapter()->getRoutingTargetOperation();
96 : 26 : std::vector<::ad::map::point::ENUPoint> routingTargetsToAppend;
97 [ - + ]: 26 : if (routingTargetOperation.command
98 : : == RssVehicleAdapter<OBJECT_INSTANCE_TYPE>::RssRoutingTargetCommand::ReplaceTargets)
99 : : {
100 : 0 : mRoutes.clear();
101 [ # # ]: 0 : mRoutingTargets = routingTargetOperation.routingTargets;
102 : 0 : dropReachedRoutingTargets(mRoutingTargets,
103 [ # # ]: 0 : mRssObjectData,
104 [ # # ]: 0 : getAdapter()->getRoutingTargetReachedDistance(),
105 : : ReachedRoutingTargetsMode::eDISTANCE_AND_ORIENTATION);
106 : : }
107 : : else
108 : : {
109 : 26 : dropReachedRoutingTargets(mRoutingTargets,
110 [ + - ]: 26 : mRssObjectData,
111 [ + - ]: 52 : getAdapter()->getRoutingTargetReachedDistance(),
112 : : ReachedRoutingTargetsMode::eDISTANCE_ONLY);
113 : :
114 [ - + ]: 26 : if (routingTargetOperation.command
115 : : == RssVehicleAdapter<OBJECT_INSTANCE_TYPE>::RssRoutingTargetCommand::AppendTargets)
116 : : {
117 [ # # ]: 0 : routingTargetsToAppend = routingTargetOperation.routingTargets;
118 : 0 : dropReachedRoutingTargets(routingTargetsToAppend,
119 [ # # ]: 0 : mRssObjectData,
120 [ # # ]: 0 : getAdapter()->getRoutingTargetReachedDistance(),
121 : : ReachedRoutingTargetsMode::eDISTANCE_AND_ORIENTATION);
122 : : }
123 : : }
124 : 52 : return routingTargetsToAppend;
125 : 26 : }
126 : :
127 : 16 : template <class OBJECT_INSTANCE_TYPE> void RssRouteCheckerVehicleData<OBJECT_INSTANCE_TYPE>::shortenRoutes()
128 : : {
129 : 16 : ad::map::point::ParaPointList allLaneMatches;
130 [ + + ]: 48 : for (auto const &referencePoint :
131 : : {::ad::map::match::ObjectReferencePoints::RearRight, ::ad::map::match::ObjectReferencePoints::RearLeft})
132 : : {
133 [ + + ]: 115 : for (auto const &matchPoint :
134 : : mRssObjectData.match_object.map_matched_bounding_box.reference_point_positions[size_t(referencePoint)])
135 : : {
136 : : // consider only longitudinal in lane matches here, because we want to cover the rear of the vehicle
137 : : // allow lateral outside matches to cover vehicle driving slightly outside the route
138 [ + - + - ]: 83 : if (ad::map::match::isLongitudinalInLaneMatch(matchPoint))
139 : : {
140 [ + - ]: 83 : allLaneMatches.push_back(matchPoint.lane_point.para_point);
141 : : }
142 : : }
143 : : }
144 : :
145 : 16 : auto routeIndex = 0u;
146 [ + + ]: 44 : while (routeIndex < mRoutes.size())
147 : : {
148 [ + - + - ]: 58 : getLogger()->trace("RssVehicleAdapter::shortenRoutes[{}:{}] call shorten route with {} and {}",
149 : 29 : mRssObjectData.id,
150 [ + - ]: 29 : mRoutes[routeIndex].route_id,
151 : : allLaneMatches,
152 : 29 : mRoutes[routeIndex].route);
153 : 58 : auto shortenRouteResult = ::ad::map::route::shortenRoute(
154 : : allLaneMatches,
155 [ + - ]: 29 : mRoutes[routeIndex].route,
156 : : ::ad::map::route::ShortenRouteMode::DontCutIntersectionAndPrependIfSucceededBeforeRoute);
157 : :
158 [ + + ]: 29 : if ((shortenRouteResult != ::ad::map::route::ShortenRouteResult::Succeeded)
159 [ + + ]: 6 : && (shortenRouteResult != ::ad::map::route::ShortenRouteResult::SucceededIntersectionNotCut))
160 : : {
161 [ + - + - ]: 6 : getLogger()->trace("RssVehicleAdapter::shortenRoutes[{}:{}] shorten current route failed. Dropping. {}",
162 : 3 : mRssObjectData.id,
163 [ + - ]: 3 : mRoutes[routeIndex].route_id,
164 : 3 : mRoutes[routeIndex].route);
165 [ + - ]: 3 : mRoutes.erase(mRoutes.begin() + routeIndex);
166 : : }
167 : : else
168 : : {
169 [ + - + - ]: 52 : getLogger()->trace("RssVehicleAdapter::shortenRoutes[{}:{}] shorten current route succeeded {}",
170 : 25 : mRssObjectData.id,
171 [ + - ]: 25 : mRoutes[routeIndex].route_id,
172 : 26 : mRoutes[routeIndex].route);
173 [ + - ]: 26 : auto findWaypointResult = findNearestWaypoint(allLaneMatches, mRoutes[routeIndex].route);
174 [ + - ]: 26 : mRoutes[routeIndex].progress_on_route = calcLength(findWaypointResult);
175 : 25 : routeIndex++;
176 : : }
177 : : }
178 : 15 : }
179 : :
180 : : template <class OBJECT_INSTANCE_TYPE>
181 : 16 : void RssRouteCheckerVehicleData<OBJECT_INSTANCE_TYPE>::extendRoutes(
182 : : physics::Distance const routePreviewDistance,
183 : : std::vector<::ad::map::point::ENUPoint> const &routingTargetsToAppend,
184 : : ::ad::map::lane::LaneIdSet const &mapAreaLanes)
185 : : {
186 : 16 : RssRouteList routesToAdd;
187 : 16 : auto routeIndex = 0u;
188 [ + + ]: 68 : while (routeIndex < mRoutes.size())
189 : : {
190 : 26 : std::vector<::ad::map::route::FullRoute> additionalRoutes;
191 : :
192 : 26 : bool routeValid = false;
193 [ - + ]: 26 : if (!routingTargetsToAppend.empty())
194 : : {
195 [ # # ]: 0 : routeValid = ::ad::map::route::extendRouteToDestinations(mRoutes[routeIndex].route, routingTargetsToAppend);
196 [ # # ]: 0 : mRoutingTargets.insert(mRoutingTargets.end(), routingTargetsToAppend.begin(), routingTargetsToAppend.end());
197 : : }
198 [ + - + - : 26 : else if (mRoutingTargets.empty() && (mRouteExtensionMode != RouteExtensionMode::eAllowOnlyTargetRoute))
+ - ]
199 : : {
200 : : // only extend if no routing targets are provided and not forbidden to extend by extension mode
201 [ + - ]: 26 : routeValid = ::ad::map::route::extendRouteToDistance(mRoutes[routeIndex].route,
202 [ + - ]: 52 : mRoutes[routeIndex].progress_on_route + routePreviewDistance,
203 : : additionalRoutes,
204 : : mapAreaLanes);
205 : : }
206 : : else
207 : : {
208 : 0 : routeValid = true;
209 : : }
210 : :
211 [ + - ]: 26 : if (routeValid)
212 : : {
213 [ + - + - ]: 52 : getLogger()->trace("RssVehicleAdapter::extendRoutes[{}:{}] extended current route {} {}",
214 : 26 : mRssObjectData.id,
215 [ + - ]: 26 : mRoutes[routeIndex].route_id,
216 : 26 : mRoutes[routeIndex].progress_on_route,
217 : 26 : mRoutes[routeIndex].route);
218 [ + - ]: 26 : if (mRouteExtensionMode == RouteExtensionMode::eAllowMultipleRoutes)
219 : : {
220 [ + + ]: 29 : for (auto &additionalRoute : additionalRoutes)
221 : : {
222 : 3 : bool addRoute = true;
223 [ + + ]: 6 : for (auto checkRouteIndex = 0u; checkRouteIndex < mRoutes.size(); checkRouteIndex++)
224 : : {
225 [ - + ]: 3 : if (checkRouteIndex != routeIndex)
226 : : {
227 : 0 : auto const compareResult = ::ad::map::route::planning::compareRoutesOnIntervalLevel(
228 [ # # ]: 0 : additionalRoute, mRoutes[checkRouteIndex].route);
229 [ # # ]: 0 : if (compareResult != ::ad::map::route::planning::CompareRouteResult::Differ)
230 : : {
231 [ # # # # ]: 0 : getLogger()->trace("RssVehicleAdapter::extendRoutes[{}:{}] drop additional route {} because compares "
232 : : "{} to route_id {} {}",
233 : 0 : mRssObjectData.id,
234 [ # # ]: 0 : mRoutes[routeIndex].route_id,
235 : : additionalRoute,
236 : : compareResult,
237 : 0 : mRoutes[checkRouteIndex].route_id,
238 : 0 : mRoutes[checkRouteIndex].route);
239 : 0 : addRoute = false;
240 : 0 : break;
241 : : }
242 : : }
243 : : }
244 : :
245 [ + - ]: 3 : if (addRoute)
246 : : {
247 : : // as the current point in the route is identical with the new one, we take over the current vehicle
248 : : // dynamics, too.
249 : 3 : RssRoute newRssRoute;
250 : 3 : newRssRoute.route_id = mNextRouteId++;
251 [ + - ]: 3 : newRssRoute.route = additionalRoute;
252 : 3 : newRssRoute.vehicle_dynamics_on_route = mRoutes[routeIndex].vehicle_dynamics_on_route;
253 : 3 : newRssRoute.likelihood = mRoutes[routeIndex].likelihood;
254 : 3 : newRssRoute.parent_route_id = mRoutes[routeIndex].route_id;
255 : 3 : newRssRoute.progress_on_route = mRoutes[routeIndex].progress_on_route;
256 [ + - + - ]: 6 : getLogger()->trace("RssVehicleAdapter::extendRoutes[{}:{}] additional route {}: {} {}",
257 : 3 : mRssObjectData.id,
258 [ + - ]: 3 : mRoutes[routeIndex].route_id,
259 : : newRssRoute.route_id,
260 : : newRssRoute.progress_on_route,
261 : : newRssRoute.route);
262 [ + - ]: 3 : routesToAdd.push_back(newRssRoute);
263 : 3 : }
264 : : }
265 : : }
266 [ # # ]: 0 : else if (additionalRoutes.size() > 0u)
267 : : {
268 [ # # # # ]: 0 : getLogger()->trace(
269 : : "RssVehicleAdapter::extendRoutes[{}:{}] allow only single route mode: drop additional routes {}",
270 : 0 : mRssObjectData.id,
271 [ # # ]: 0 : mRoutes[routeIndex].route_id,
272 : 0 : additionalRoutes.size());
273 : : }
274 : 26 : routeIndex++;
275 : : }
276 : : else
277 : : {
278 : : // drop this
279 [ # # # # ]: 0 : getLogger()->warn("RssVehicleAdapter::extendRoutes[{}:{}] extend current route failed, dropping {}",
280 : 0 : mRssObjectData.id,
281 [ # # ]: 0 : mRoutes[routeIndex].route_id,
282 : 0 : mRoutes[routeIndex].route);
283 [ # # ]: 0 : mRoutes.erase(mRoutes.begin() + routeIndex);
284 : : }
285 : : }
286 [ + - ]: 16 : mRoutes.insert(mRoutes.end(), routesToAdd.begin(), routesToAdd.end());
287 : 16 : }
288 : :
289 : : template <class OBJECT_INSTANCE_TYPE>
290 : : std::vector<::ad::map::route::FullRoute>
291 : 10 : RssRouteCheckerVehicleData<OBJECT_INSTANCE_TYPE>::createRoutes(physics::Distance const routePreviewDistance,
292 : : ::ad::map::lane::LaneIdSet const &mapAreaLanes)
293 : : {
294 : 10 : std::vector<::ad::map::route::FullRoute> newRoutes;
295 [ + + ]: 30 : for (const auto &refPoint :
296 : : {::ad::map::match::ObjectReferencePoints::RearLeft, ::ad::map::match::ObjectReferencePoints::RearRight})
297 : : {
298 [ + + ]: 70 : for (const auto &position :
299 : : mRssObjectData.match_object.map_matched_bounding_box.reference_point_positions[std::size_t(int32_t(refPoint))])
300 : : {
301 : : // consider only longitudinal in lane matches here, to be consistent to shortenRoute
302 : : // otherwhise the next shorten route might fail
303 [ + - - + ]: 50 : if (!ad::map::match::isLongitudinalInLaneMatch(position))
304 : : {
305 : 0 : continue;
306 : : }
307 : 50 : auto startPoint = position.lane_point.para_point;
308 : 50 : auto projectedStartPoint = startPoint;
309 [ + - + + ]: 50 : if (!::ad::map::lane::isHeadingInLaneDirection(startPoint, mRssObjectData.match_object.enu_position.heading))
310 : : {
311 [ + - + - ]: 10 : getLogger()->trace("Vehicle heading in opposite lane direction");
312 [ + - ]: 10 : if (::ad::map::lane::projectPositionToLaneInHeadingDirection(
313 [ + - ]: 10 : startPoint, mRssObjectData.match_object.enu_position.heading, projectedStartPoint))
314 : : {
315 [ + - + - ]: 20 : getLogger()->trace("Projected to lane {}", projectedStartPoint.lane_id);
316 : : }
317 : : }
318 [ + - + - ]: 100 : getLogger()->trace("Route startPoint: {}, projectedStartPoint: {}", startPoint, projectedStartPoint);
319 : 50 : auto routingStartPoint = ::ad::map::route::planning::createRoutingPoint(
320 [ + - ]: 50 : projectedStartPoint, mRssObjectData.match_object.enu_position.heading);
321 : :
322 [ - + - - : 50 : if (!mRoutingTargets.empty() && ad::map::point::isValid(mRoutingTargets))
- - - + ]
323 : : {
324 : 0 : auto newRoute = ::ad::map::route::planning::planRoute(
325 [ # # ]: 0 : routingStartPoint, mRoutingTargets, ::ad::map::route::RouteCreationMode::AllRoutableLanes);
326 [ # # ]: 0 : if (newRoute.route_planning_counter > 0u)
327 : : {
328 [ # # ]: 0 : newRoutes.push_back(newRoute);
329 [ # # # # ]: 0 : getLogger()->trace("Route found from startPoint: {}, projectedStartPoint: {}, routingTargets: {}, route: {}",
330 : : startPoint,
331 : : projectedStartPoint,
332 [ # # ]: 0 : mRoutingTargets,
333 : : newRoute);
334 : : }
335 : : else
336 : : {
337 [ # # # # ]: 0 : getLogger()->warn("No Route found from startPoint: {}, projectedStartPoint: {}, routingTargets: {}",
338 : : startPoint,
339 : : projectedStartPoint,
340 [ # # ]: 0 : mRoutingTargets);
341 : : }
342 : 0 : }
343 [ + - ]: 50 : else if (mRouteExtensionMode != RouteExtensionMode::eAllowOnlyTargetRoute)
344 : : {
345 : : // better prefer longer routes to ensure back of the vehicle is in
346 [ + - ]: 50 : auto newRoutesFromCurrentStart = ::ad::map::route::planning::predictRoutesOnDistance(
347 : : routingStartPoint,
348 : : routePreviewDistance,
349 : : ::ad::map::route::RouteCreationMode::AllRoutableLanes,
350 : : ::ad::map::route::planning::FilterDuplicatesMode::SubRoutesPreferLongerOnes,
351 : : mapAreaLanes);
352 [ + - ]: 50 : newRoutes.insert(newRoutes.end(), newRoutesFromCurrentStart.begin(), newRoutesFromCurrentStart.end());
353 [ + - + - ]: 100 : getLogger()->trace("Routes predicted from startPoint: {}, projectedStartPoint: {}, routes: {}",
354 : : startPoint,
355 : : projectedStartPoint,
356 : : newRoutesFromCurrentStart);
357 : 50 : }
358 : : }
359 : : }
360 : :
361 : : // better prefer longer routes to ensure back of the vehicle is in
362 [ + - + - ]: 10 : newRoutes = ::ad::map::route::planning::filterDuplicatedRoutes(
363 : : newRoutes, ::ad::map::route::planning::FilterDuplicatesMode::SubRoutesPreferLongerOnes);
364 : :
365 : 10 : return newRoutes;
366 : 0 : }
367 : :
368 : : template <class OBJECT_INSTANCE_TYPE>
369 : 26 : void RssRouteCheckerVehicleData<OBJECT_INSTANCE_TYPE>::updateRoutes(
370 : : ::ad::physics::Distance const routePreviewDistance,
371 : : ::ad::map::lane::LaneIdSet const &mapAreaLanes,
372 : : RssRouteCheckerVehicleData<OBJECT_INSTANCE_TYPE>::RouteExtensionMode routeExtensionMode)
373 : : {
374 : 26 : mRouteExtensionMode = routeExtensionMode;
375 [ + - ]: 26 : auto adapterPredictions = getAdapter()->getRoutePredictions();
376 [ - + ]: 26 : if (!adapterPredictions.empty())
377 : : {
378 [ # # ]: 0 : if (!mExternalRoutes)
379 : : {
380 : : // mode switch between external and internal routes
381 : 0 : mRoutes.clear();
382 : : }
383 : 0 : mExternalRoutes = true;
384 : 0 : RssRouteList oldRoutes;
385 : 0 : oldRoutes.swap(mRoutes);
386 [ # # ]: 0 : for (auto &prediction : adapterPredictions)
387 : : {
388 [ # # ]: 0 : mRoutes.push_back(prediction);
389 : : auto oldRouteIter
390 [ # # ]: 0 : = std::find_if(oldRoutes.begin(), oldRoutes.end(), [&prediction](RssRoute const &route) -> bool {
391 : 0 : return route.route_id == prediction.route_id;
392 : : });
393 [ # # ]: 0 : if (oldRouteIter != oldRoutes.end())
394 : : {
395 : : // preserve vehicle dynamics if possible
396 : 0 : mRoutes.back().vehicle_dynamics_on_route = oldRouteIter->vehicle_dynamics_on_route;
397 : : }
398 : : }
399 : 0 : }
400 : : else
401 : : {
402 [ - + ]: 26 : if (mExternalRoutes)
403 : : {
404 : : // mode switch between external and internal routes
405 : 0 : mRoutes.clear();
406 : : }
407 : 26 : mExternalRoutes = false;
408 : :
409 [ + - ]: 26 : auto const routingTargetsToAppend = handleRoutingTargets();
410 : :
411 [ + + ]: 26 : if (mRoutes.empty())
412 : : {
413 : : // try to create routes
414 [ + - ]: 10 : ::ad::map::route::FullRouteList newRoutes = createRoutes(routePreviewDistance, mapAreaLanes);
415 [ + + ]: 27 : for (auto &newRoute : newRoutes)
416 : : {
417 : 17 : RssRoute rss_route;
418 : 17 : rss_route.route_id = mNextRouteId++;
419 [ + - ]: 17 : rss_route.route = newRoute;
420 : 17 : rss_route.object_data_on_route = RssObjectDataOnRoute();
421 : 17 : rss_route.vehicle_dynamics_on_route = RssEgoVehicleDynamicsOnRoute();
422 : 17 : rss_route.likelihood = physics::Probability(1.);
423 : 17 : rss_route.parent_route_id = RssRouteId(0);
424 : 17 : rss_route.progress_on_route = physics::Distance(0.);
425 [ + - + - ]: 34 : getLogger()->trace("RssVehicleAdapter::updateRoutes[{}] new route {}: {} {}",
426 [ + - ]: 17 : mRssObjectData.id,
427 : : rss_route.route_id,
428 : : rss_route.progress_on_route,
429 : : rss_route.route);
430 [ + - ]: 17 : mRoutes.push_back(rss_route);
431 : : }
432 : 10 : }
433 : : else
434 : : {
435 : : // adapt routes if required
436 [ + - ]: 16 : shortenRoutes();
437 [ + - ]: 16 : extendRoutes(routePreviewDistance, routingTargetsToAppend, mapAreaLanes);
438 : : }
439 [ + - ]: 26 : getAdapter()->activeRoutingTargets(mRoutingTargets);
440 : 26 : }
441 : 26 : }
442 : :
443 : 26 : template <class OBJECT_INSTANCE_TYPE> void RssRouteCheckerVehicleData<OBJECT_INSTANCE_TYPE>::normalizeAndOrderRoutes()
444 : : {
445 : 26 : physics::Probability likelihoodSum(.0);
446 [ + + ]: 72 : for (auto &rss_route : mRoutes)
447 : : {
448 [ + - ]: 46 : likelihoodSum += rss_route.likelihood;
449 : : }
450 : :
451 [ + - + - ]: 26 : if (likelihoodSum != physics::Probability(0.))
452 : : {
453 : : // normalize
454 [ + + ]: 72 : for (auto &rss_route : mRoutes)
455 : : {
456 [ + - ]: 46 : rss_route.likelihood = physics::Probability(rss_route.likelihood / likelihoodSum);
457 : : }
458 : : }
459 : :
460 : : // sort
461 [ + - ]: 26 : std::sort(mRoutes.begin(), mRoutes.end(), [](RssRoute const &left, RssRoute const &right) -> bool {
462 : 40 : return left.likelihood > right.likelihood;
463 : : });
464 : :
465 [ - + - - : 26 : if ((mRouteExtensionMode == RouteExtensionMode::eAllowOnlySingleRoute) && (mRoutes.size() > 1))
- + ]
466 : : {
467 [ # # # # ]: 0 : getLogger()->trace(
468 : : "RssVehicleAdapter::normalizeAndOrderRoutes[{}] allow only single route mode: drop other routes #{}",
469 : 0 : mRssObjectData.id,
470 [ # # ]: 0 : mRoutes.size() - 1);
471 [ # # ]: 0 : mRoutes.resize(1u);
472 : : }
473 : 26 : }
474 : :
475 : : template <class OBJECT_INSTANCE_TYPE>
476 : 17 : void RssRouteCheckerEgoVehicleData<OBJECT_INSTANCE_TYPE>::updateVehicleDynamicsOnRoutes()
477 : : {
478 [ + + ]: 45 : for (auto &rss_route : mRoutes)
479 : : {
480 : : // before starting to override, store required old data
481 : 28 : physics::Duration deltaTime(0.);
482 [ + + ]: 28 : if (rss_route.vehicle_dynamics_on_route.last_update.time_since_epoch().count() != 0)
483 : : {
484 : 17 : deltaTime = physics::Duration(
485 [ + - ]: 34 : std::chrono::duration<double>(mRssObjectData.last_update - rss_route.vehicle_dynamics_on_route.last_update)
486 : : .count());
487 : : }
488 : 28 : auto const lastRouteSpeedValid = rss_route.object_data_on_route.is_valid;
489 : 28 : auto const lastRouteSpeedLat = rss_route.object_data_on_route.route_speed_lat;
490 : 28 : auto const lastRouteSpeedLon = rss_route.object_data_on_route.route_speed_lon;
491 : :
492 : : // start the update
493 [ + - ]: 28 : rss_route.object_data_on_route = calculateRssObjectDataOnRoute(rss_route.route, mRssObjectData);
494 : 28 : rss_route.vehicle_dynamics_on_route.last_update = mRssObjectData.last_update;
495 : :
496 [ + - + + : 28 : if ((deltaTime > physics::Duration(0.0001)) && lastRouteSpeedValid && rss_route.object_data_on_route.is_valid)
+ + + - +
+ ]
497 : : {
498 : : rss_route.vehicle_dynamics_on_route.route_accel_lat
499 [ + - + - ]: 14 : = (rss_route.object_data_on_route.route_speed_lat - lastRouteSpeedLat) / deltaTime;
500 : : rss_route.vehicle_dynamics_on_route.avg_route_accel_lat
501 : 0 : = ((rss_route.vehicle_dynamics_on_route.avg_route_accel_lat * 2.)
502 [ + - ]: 14 : + rss_route.vehicle_dynamics_on_route.route_accel_lat)
503 [ + - + - ]: 28 : / 3.;
504 : : rss_route.vehicle_dynamics_on_route.route_accel_lon
505 [ + - + - ]: 14 : = (rss_route.object_data_on_route.route_speed_lon - lastRouteSpeedLon) / deltaTime;
506 : : rss_route.vehicle_dynamics_on_route.avg_route_accel_lon
507 : 0 : = ((rss_route.vehicle_dynamics_on_route.avg_route_accel_lon * 2.)
508 [ + - ]: 14 : + rss_route.vehicle_dynamics_on_route.route_accel_lon)
509 [ + - + - ]: 28 : / 3.;
510 : :
511 [ + - + + ]: 14 : if (rss_route.vehicle_dynamics_on_route.avg_route_accel_lat == physics::Acceleration(0.))
512 : : {
513 : : // prevent from underrun
514 : 12 : rss_route.vehicle_dynamics_on_route.avg_route_accel_lat = physics::Acceleration(0.);
515 : : }
516 [ + - + - ]: 14 : if (rss_route.vehicle_dynamics_on_route.avg_route_accel_lon == physics::Acceleration(0.))
517 : : {
518 : : // prevent from underrun
519 : 14 : rss_route.vehicle_dynamics_on_route.avg_route_accel_lon = physics::Acceleration(0.);
520 : : }
521 : : }
522 : : }
523 : 17 : }
524 : :
525 : 17 : template <class OBJECT_INSTANCE_TYPE> void RssRouteCheckerEgoVehicleData<OBJECT_INSTANCE_TYPE>::evaluateRoutes()
526 : : {
527 [ + - ]: 17 : if (!mExternalRoutes)
528 : : {
529 : 17 : physics::Angle const maxHeadingDelta(M_PI / 4.);
530 : 17 : physics::Distance const maxCenterDistance(4.);
531 [ + + ]: 45 : for (auto &rss_route : mRoutes)
532 : : {
533 : 28 : rss_route.likelihood = physics::Probability(.05);
534 [ + - ]: 28 : if (rss_route.object_data_on_route.is_valid)
535 : : {
536 : : // distance to nominal center
537 : 28 : auto const distanceToRouteNominalCenter = ad::map::point::distance(
538 : 28 : rss_route.object_data_on_route.nominal_center_position_of_lane_in_nominal_route_direction,
539 [ + - ]: 28 : mRssObjectData.match_object.enu_position.center_point);
540 [ - + ]: 28 : if (!rss_route.object_data_on_route.object_within_route)
541 : : {
542 : 0 : rss_route.likelihood = physics::Probability(.01);
543 : : }
544 : 56 : else if ((rss_route.object_data_on_route.object_center_within_route)
545 [ + - - + ]: 28 : && (distanceToRouteNominalCenter < maxCenterDistance)
546 [ + - - - : 56 : && (std::fabs(rss_route.object_data_on_route.route_heading_delta) < maxHeadingDelta))
- - - + ]
547 : : {
548 [ # # ]: 0 : auto const distanceFactor = 1. - distanceToRouteNominalCenter / maxCenterDistance;
549 : 0 : auto const headingFactor
550 [ # # ]: 0 : = 1. - std::fabs(rss_route.object_data_on_route.route_heading_delta) / maxHeadingDelta;
551 : 0 : rss_route.likelihood = physics::Probability(distanceFactor * headingFactor);
552 : : }
553 : : else
554 : : {
555 : : // center not within route anymore, or heading diff larger than 45 degree
556 : : }
557 : : }
558 : : }
559 : : }
560 : 17 : }
561 : :
562 : : } // namespace map
563 : : } // namespace rss
564 : : } // namespace ad
|
