Branch data Line data Source code
1 : : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
2 : : //
3 : : // Copyright (C) 2019-2022 Intel Corporation
4 : : //
5 : : // SPDX-License-Identifier: LGPL-2.1-only
6 : : //
7 : : // ----------------- END LICENSE BLOCK -----------------------------------
8 : :
9 : : #include "ad/rss/map/RssRouteChecker.hpp"
10 : :
11 : : #include <ad/rss/state/RssStateOperation.hpp>
12 : : #include <tbb/tbb.h>
13 : :
14 : : #include "ad/map/intersection/CoreIntersection.hpp"
15 : : #include "ad/map/lane/Operation.hpp"
16 : : #include "ad/map/route/Operation.hpp"
17 : : #include "ad/rss/map/Logging.hpp"
18 : : #include "ad/rss/map/RssWorldModelCreator.hpp"
19 : : #include "ad/rss/map/detail/ParallelChecker.hpp"
20 : : #include "ad/rss/map/detail/ParallelObjectUpdater.hpp"
21 : : #include "ad/rss/map/detail/ParallelRouteUpdater.hpp"
22 : : #include "ad/rss/map/detail/ParallelWorldModelCreator.hpp"
23 : :
24 : : namespace ad {
25 : : namespace rss {
26 : : namespace map {
27 : :
28 : : template <class OBJECT_INSTANCE_TYPE>
29 : 5 : void RssRouteChecker<OBJECT_INSTANCE_TYPE>::registerArtificialObjectInjectionCallback(
30 : : ::ad::physics::Distance const artificialObjectSamplingDistance,
31 : : ArtificialObjectInjectionCallbackFunctionType artificialObjectsInjectionCallback)
32 : : {
33 [ + - ]: 5 : std::lock_guard<std::shared_timed_mutex> const lock(mCallbackRwLock);
34 : 5 : mArtificialObjectSamplingDistance = artificialObjectSamplingDistance;
35 [ + - ]: 5 : mArtificialObjectsInjectionCallback = artificialObjectsInjectionCallback;
36 : 5 : }
37 : :
38 : : template <class OBJECT_INSTANCE_TYPE>
39 : : template <class Data, class Adapter>
40 : 44 : void RssRouteChecker<OBJECT_INSTANCE_TYPE>::updateObjectMap(
41 : : std::chrono::system_clock::time_point const ¤tTime,
42 : : std::map<::ad::rss::world::ObjectId, Data> &objectMap,
43 : : std::vector<typename Adapter::Ptr> const &activeObjectAdapterList)
44 : : {
45 [ + + ]: 60 : for (auto &objectEntry : objectMap)
46 : : {
47 : 16 : objectEntry.second.mActive = false;
48 : : }
49 [ + + ]: 72 : for (auto &adapter : activeObjectAdapterList)
50 : : {
51 [ + - ]: 28 : auto objectEntryIter = objectMap.find(adapter->getObjectId());
52 [ + + ]: 28 : if (objectEntryIter == objectMap.end())
53 : : {
54 : : // create new map entry for this object
55 [ + - + - : 12 : auto insertResult = objectMap.insert({adapter->getObjectId(), Data()});
+ - ]
56 [ - + ]: 12 : if (!insertResult.second)
57 : : {
58 [ # # # # ]: 0 : getLogger()->error("RssRouteChecker::updateObjectMap[{}] unexpected error on adding new object adapter entry",
59 [ # # ]: 0 : adapter->getObjectId());
60 : 0 : continue;
61 : : }
62 : 12 : objectEntryIter = insertResult.first;
63 : : }
64 : :
65 : 28 : objectEntryIter->second.mActive = true;
66 [ + - ]: 28 : objectEntryIter->second.updateAdapter(adapter);
67 : : }
68 : :
69 : : // remove outdated entries
70 [ + + ]: 72 : for (auto objectEntryIter = objectMap.begin(); objectEntryIter != objectMap.end();)
71 : : {
72 [ - + ]: 28 : if (!objectEntryIter->second.mActive)
73 : : {
74 : 0 : auto durationSinceLastUpdate = currentTime - objectEntryIter->second.mRssObjectData.last_update;
75 [ # # ]: 0 : if (std::chrono::duration_cast<std::chrono::seconds>(durationSinceLastUpdate).count()
76 [ # # ]: 0 : > (static_cast<int64_t>(mDropOutdatedObjectsDuration.mDuration) + 1))
77 : : {
78 [ # # # # ]: 0 : getLogger()->debug("RssRouteChecker::updateObjectMap[{}] Dropping outdated object adapter.",
79 [ # # ]: 0 : objectEntryIter->first);
80 [ # # ]: 0 : objectMap.erase(objectEntryIter++);
81 : 0 : continue;
82 : : }
83 : : }
84 : 28 : objectEntryIter++;
85 : : }
86 : 44 : }
87 : :
88 : 11 : template <class OBJECT_INSTANCE_TYPE> void RssRouteChecker<OBJECT_INSTANCE_TYPE>::updateRssCheckers()
89 : : {
90 [ + + ]: 28 : for (auto &egoVehicleToUpdate : mEgoVehicleMap)
91 : : {
92 : 17 : auto &egoVehicleData = egoVehicleToUpdate.second;
93 [ + - ]: 17 : if (egoVehicleData.mActive)
94 : : {
95 : 17 : RssRouteCheckMap lastRouteCheckMap;
96 : 17 : egoVehicleData.mRssRouteCheckMap.swap(lastRouteCheckMap);
97 : :
98 [ + + ]: 34 : for (auto &lastCheckerEntry : lastRouteCheckMap)
99 : : {
100 [ + + ]: 34 : for (auto &otherVehicle : mVehicleMap)
101 : : {
102 [ - + ]: 17 : if (otherVehicle.second.mClearObjectHistory)
103 : : {
104 [ # # ]: 0 : lastCheckerEntry.second->dropObjectHistory(otherVehicle.second.mRssObjectData.id);
105 : : }
106 : : }
107 : : }
108 : :
109 [ + + ]: 45 : for (auto &egoRssRoute : egoVehicleData.mRoutes)
110 : : {
111 [ + - ]: 28 : auto routeCheckFindResult = lastRouteCheckMap.find(egoRssRoute.route_id);
112 [ + + ]: 28 : if (routeCheckFindResult != lastRouteCheckMap.end())
113 : : {
114 [ + - + - ]: 28 : getLogger()->trace("RssRouteChecker::updateRssCheckers[{}:{}] taking over existing checker",
115 : 14 : egoVehicleData.mRssObjectData.id,
116 [ + - ]: 14 : egoRssRoute.route_id);
117 : 14 : RssRouteCheckMap::value_type checkerToTakeOver = *routeCheckFindResult;
118 [ + - ]: 14 : egoVehicleData.mRssRouteCheckMap.insert(checkerToTakeOver);
119 : 14 : }
120 : : else
121 : : {
122 : : // is there a parent route assigned or a default checker on RouteId(0) available?
123 [ + - ]: 14 : routeCheckFindResult = lastRouteCheckMap.find(egoRssRoute.parent_route_id);
124 [ + + ]: 14 : if (routeCheckFindResult != lastRouteCheckMap.end())
125 : : {
126 [ + - + - ]: 6 : getLogger()->trace("RssRouteChecker::updateRssCheckers[{}:{}] add copy parent checker {}",
127 : 3 : egoVehicleData.mRssObjectData.id,
128 : 3 : egoRssRoute.route_id,
129 [ + - ]: 3 : egoRssRoute.parent_route_id);
130 : : // create a copy of the parent checker to preserve the current state of the parent of
131 : : // the new variant of the extended route
132 : 3 : ::ad::rss::core::RssCheck const &parentChecker = *routeCheckFindResult->second.get();
133 [ + - + - : 3 : ::ad::rss::core::RssCheck::Ptr parentCheckerCopy(new ::ad::rss::core::RssCheck(parentChecker));
+ - ]
134 [ + - ]: 3 : auto insertResult = egoVehicleData.mRssRouteCheckMap.insert({egoRssRoute.route_id, parentCheckerCopy});
135 [ + - ]: 3 : if (insertResult.second)
136 : : {
137 [ + - ]: 3 : updateRssCheckerCallbacks(egoVehicleData, insertResult.first);
138 : : }
139 : 3 : }
140 : : else
141 : : {
142 [ + - + - ]: 22 : getLogger()->trace("RssRouteChecker::updateRssCheckers[{}:{}] no parent checker {} found; add new checker",
143 : 11 : egoVehicleData.mRssObjectData.id,
144 : 11 : egoRssRoute.route_id,
145 [ + - ]: 11 : egoRssRoute.parent_route_id);
146 : : // create a new checker
147 [ + - + - : 11 : ::ad::rss::core::RssCheck::Ptr newChecker(new ::ad::rss::core::RssCheck());
+ - ]
148 [ + - ]: 11 : auto insertResult = egoVehicleData.mRssRouteCheckMap.insert({egoRssRoute.route_id, newChecker});
149 [ + - ]: 11 : if (insertResult.second)
150 : : {
151 [ + - ]: 11 : updateRssCheckerCallbacks(egoVehicleData, insertResult.first);
152 : : }
153 : 11 : }
154 : : }
155 : : }
156 [ - + ]: 17 : if (egoVehicleData.mRssRouteCheckMap.empty())
157 : : {
158 : : // we need to ensure to have a default checker for RouteId(0)
159 [ # # ]: 0 : if (!lastRouteCheckMap.empty())
160 : : {
161 [ # # # # ]: 0 : getLogger()->trace("RssRouteChecker::updateRssCheckers[{}:0] no current route, resuse first checker {} as 0",
162 : 0 : egoVehicleData.mRssObjectData.id,
163 [ # # ]: 0 : lastRouteCheckMap.begin()->first);
164 : : // take over the first checker in the map (if there are multiple ones available,
165 : : // the one with the lowest route id is taken; but the route id is set to zero again)
166 : : auto insertResult
167 [ # # ]: 0 : = egoVehicleData.mRssRouteCheckMap.insert({RssRouteId(0), lastRouteCheckMap.begin()->second});
168 [ # # ]: 0 : if (insertResult.second)
169 : : {
170 [ # # ]: 0 : updateRssCheckerCallbacks(egoVehicleData, insertResult.first);
171 : : }
172 : : }
173 : : else
174 : : {
175 [ # # # # ]: 0 : getLogger()->trace("RssRouteChecker::updateRssCheckers[{}:0] no current route, and no checker at all "
176 : : "present; add default checker",
177 [ # # ]: 0 : egoVehicleData.mRssObjectData.id);
178 : : // and we didn't have any last time, so create new checker to be used for checks
179 [ # # # # : 0 : ::ad::rss::core::RssCheck::Ptr newChecker(new ::ad::rss::core::RssCheck());
# # ]
180 [ # # ]: 0 : auto insertResult = egoVehicleData.mRssRouteCheckMap.insert({RssRouteId(0), newChecker});
181 [ # # ]: 0 : if (insertResult.second)
182 : : {
183 [ # # ]: 0 : updateRssCheckerCallbacks(egoVehicleData, insertResult.first);
184 : : }
185 : 0 : }
186 : : }
187 : 17 : }
188 : : }
189 : 11 : }
190 : :
191 : : template <class OBJECT_INSTANCE_TYPE>
192 : 10 : void RssRouteChecker<OBJECT_INSTANCE_TYPE>::registerCalculateTrajectorySetsCallback(
193 : : world::ObjectType objectType,
194 : : RssRouteChecker<OBJECT_INSTANCE_TYPE>::CalculateTrajectorySetsCallbackFunctionType calculateTrajectorySetsCallback)
195 : : {
196 : : try
197 : : {
198 [ + - ]: 10 : std::lock_guard<std::shared_timed_mutex> const lock(mCallbackRwLock);
199 [ + - + - ]: 10 : mCalculateTrajectorySetsCallbackMap[objectType] = calculateTrajectorySetsCallback;
200 : : // update all checkers at once
201 [ - + ]: 10 : for (auto &egoVehicleToUpdate : mEgoVehicleMap)
202 : : {
203 : 0 : auto &egoVehicleData = egoVehicleToUpdate.second;
204 : 0 : for (auto checkMapIter = egoVehicleData.mRssRouteCheckMap.begin();
205 [ # # ]: 0 : checkMapIter != egoVehicleData.mRssRouteCheckMap.end();
206 : 0 : checkMapIter++)
207 : : {
208 [ # # ]: 0 : updateRssCheckerCallbacksLocked(egoVehicleData, checkMapIter);
209 : : }
210 : : }
211 : 10 : }
212 [ - - ]: 0 : catch (std::exception &e)
213 : : {
214 [ - - - - : 0 : getLogger()->critical("RssRouteChecker::registerCalculateTrajectorySetsCallback>> Exception caught'{}'", e.what());
- - ]
215 : : }
216 : 0 : catch (...)
217 : : {
218 [ - - - - ]: 0 : getLogger()->critical("RssRouteChecker::registerCalculateTrajectorySetsCallback>> Exception caught");
219 : : }
220 : 10 : }
221 : :
222 : : template <class OBJECT_INSTANCE_TYPE>
223 : 14 : void RssRouteChecker<OBJECT_INSTANCE_TYPE>::updateRssCheckerCallbacks(
224 : : RssRouteCheckerEgoVehicleData<OBJECT_INSTANCE_TYPE> const &egoVehicle, RssRouteCheckMap::iterator routeCheckerMapIter)
225 : : {
226 [ + - ]: 14 : core::shared_lock_guard const lock_shared(mCallbackRwLock);
227 [ + - ]: 14 : updateRssCheckerCallbacksLocked(egoVehicle, routeCheckerMapIter);
228 : 14 : }
229 : :
230 : : template <class OBJECT_INSTANCE_TYPE>
231 : 14 : void RssRouteChecker<OBJECT_INSTANCE_TYPE>::updateRssCheckerCallbacksLocked(
232 : : RssRouteCheckerEgoVehicleData<OBJECT_INSTANCE_TYPE> const &egoVehicle, RssRouteCheckMap::iterator routeCheckerMapIter)
233 : : {
234 : : using namespace std::placeholders;
235 [ + + ]: 42 : for (auto &callbackPair : mCalculateTrajectorySetsCallbackMap)
236 : : {
237 : 0 : auto callback = std::bind(&RssRouteChecker::callCalculateTrajectorySetsCallback,
238 : : this,
239 : 28 : callbackPair.second,
240 : 28 : egoVehicle.mRssObjectData.id,
241 [ + - ]: 28 : routeCheckerMapIter->first,
242 : : _1,
243 : : _2,
244 : : _3,
245 : : _4,
246 : : _5);
247 [ + - + - ]: 28 : routeCheckerMapIter->second->registerCalculateTrajectorySetsCallback(callbackPair.first, callback);
248 : : }
249 : 14 : }
250 : :
251 : : template <class OBJECT_INSTANCE_TYPE>
252 : 4 : bool RssRouteChecker<OBJECT_INSTANCE_TYPE>::callCalculateTrajectorySetsCallback(
253 : : RssRouteChecker<OBJECT_INSTANCE_TYPE>::CalculateTrajectorySetsCallbackFunctionType callback,
254 : : ::ad::rss::world::ObjectId const &egoVehicleId,
255 : : ::ad::rss::map::RssRouteId const &routeId,
256 : : ::ad::rss::core::RelativeConstellation const &constellation,
257 : : ::ad::rss::world::ObjectId const &objectId,
258 : : ::ad::rss::core::RelativeObjectState const &vehicleState,
259 : : ::ad::geometry::Polygon &brakePolygon,
260 : : ::ad::geometry::Polygon &continueForwardPolygon)
261 : : {
262 [ + - ]: 4 : auto findEgoVehicleDataResult = mEgoVehicleMap.find(egoVehicleId);
263 [ + - ]: 4 : if (findEgoVehicleDataResult != mEgoVehicleMap.end())
264 : : {
265 : 4 : auto &egoVehicleData = findEgoVehicleDataResult->second;
266 [ + - ]: 4 : return callback(
267 : 4 : egoVehicleData, routeId, constellation, objectId, vehicleState, brakePolygon, continueForwardPolygon);
268 : : }
269 : : else
270 : : {
271 [ # # # # ]: 0 : getLogger()->error("RssRouteChecker::callCalculateTrajectorySetsCallback[{}:{}] ego vehicle not found to process "
272 : : "callback for object type {}",
273 : : egoVehicleId,
274 : : routeId,
275 [ # # ]: 0 : std::to_string(vehicleState.object_type));
276 : : }
277 : 0 : return false;
278 : : }
279 : :
280 : : template <class OBJECT_INSTANCE_TYPE>
281 : 28 : RssRouteCheckResult RssRouteChecker<OBJECT_INSTANCE_TYPE>::createSituationForRoute(
282 : : RssRouteCheckerEgoVehicleData<OBJECT_INSTANCE_TYPE> const &ego_vehicle, RssRoute const &rss_route)
283 : : {
284 : 28 : RssRouteCheckResult situationCheckResult;
285 : 28 : situationCheckResult.is_valid = false;
286 [ + - ]: 28 : situationCheckResult.rss_route = rss_route;
287 : 28 : situationCheckResult.rss_situation_snapshot = ad::rss::core::RssSituationSnapshot();
288 : 28 : situationCheckResult.rss_state_snapshot = ad::rss::state::RssStateSnapshot();
289 : 28 : situationCheckResult.proper_response = ad::rss::state::ProperResponse();
290 : 28 : situationCheckResult.result_analysis.dangerous_state = true;
291 : 28 : situationCheckResult.result_analysis.dangerous_vehicle = true;
292 : 28 : situationCheckResult.result_analysis.dangerous_opposite_state = true;
293 : 28 : situationCheckResult.result_analysis.vehicle_crossing_road_boundaries = false;
294 : :
295 : : try
296 : : {
297 : 28 : ParallelWorldModelCreator<OBJECT_INSTANCE_TYPE> situationWorldModelCreator(
298 [ + - ]: 28 : mTimeIndex, ego_vehicle, rss_route, mConnectingRoutesCache, mMapAreaLanes);
299 [ + - ]: 28 : tbb::parallel_for_each(mEgoVehicleMap.begin(), mEgoVehicleMap.end(), situationWorldModelCreator);
300 [ + - ]: 28 : tbb::parallel_for_each(mVehicleMap.begin(), mVehicleMap.end(), situationWorldModelCreator);
301 [ + - ]: 28 : tbb::parallel_for_each(mPedestrianMap.begin(), mPedestrianMap.end(), situationWorldModelCreator);
302 [ + - ]: 28 : tbb::parallel_for_each(mArtificialObjectMap.begin(), mArtificialObjectMap.end(), situationWorldModelCreator);
303 [ + - ]: 28 : situationCheckResult.world_model = situationWorldModelCreator.getWorldModel();
304 : :
305 [ + - ]: 28 : situationCreated(ego_vehicle, situationCheckResult);
306 : 28 : }
307 [ - - ]: 0 : catch (std::exception &e)
308 : : {
309 [ - - - - ]: 0 : getLogger()->critical("RssRouteChecker::createSituationForRoute>> Exception caught'{}' {} {}",
310 [ - - ]: 0 : e.what(),
311 : 0 : ego_vehicle.mRssObjectData.id,
312 : : rss_route);
313 : 0 : situationCheckResult.is_valid = false;
314 : : }
315 : 0 : catch (...)
316 : : {
317 [ - - - - ]: 0 : getLogger()->critical(
318 [ - - ]: 0 : "RssRouteChecker::createSituationForRoute>> Exception caught {} {}", ego_vehicle.mRssObjectData.id, rss_route);
319 [ - - ]: 0 : situationCheckResult.is_valid = false;
320 : : }
321 : 28 : return situationCheckResult;
322 : 0 : }
323 : :
324 : 11 : template <class OBJECT_INSTANCE_TYPE> RssObjectResultList RssRouteChecker<OBJECT_INSTANCE_TYPE>::createSituations()
325 : : {
326 : 11 : RssObjectResultList resultList;
327 [ + - ]: 11 : mConnectingRoutesCache.clear();
328 [ + + ]: 28 : for (auto const &ego_vehicle : mEgoVehicleMap)
329 : : {
330 [ + - ]: 17 : if (ego_vehicle.second.mActive)
331 : : {
332 [ + - ]: 17 : RssObjectResult checkResult;
333 [ + - ]: 17 : checkResult.object_data = ego_vehicle.second.mRssObjectData;
334 : :
335 [ - + ]: 17 : if (ego_vehicle.second.mRoutes.empty())
336 : : {
337 : 0 : RssRoute emptyRoute;
338 : 0 : emptyRoute.likelihood = ::ad::physics::Probability(1.);
339 : 0 : emptyRoute.route_id = RssRouteId(0.);
340 : 0 : emptyRoute.route = ::ad::map::route::FullRoute();
341 : 0 : emptyRoute.parent_route_id = RssRouteId(0.);
342 [ # # ]: 0 : auto const situationCheckResult = createSituationForRoute(ego_vehicle.second, emptyRoute);
343 [ # # ]: 0 : checkResult.situation_check_results.push_back(situationCheckResult);
344 : 0 : }
345 : : else
346 : : {
347 [ + + ]: 45 : for (auto const &rss_route : ego_vehicle.second.mRoutes)
348 : : {
349 [ + - ]: 28 : auto const situationCheckResult = createSituationForRoute(ego_vehicle.second, rss_route);
350 [ + - ]: 28 : checkResult.situation_check_results.push_back(situationCheckResult);
351 : : }
352 : : }
353 : :
354 [ + - ]: 17 : situationsCreated(ego_vehicle.second, checkResult);
355 [ + - ]: 17 : resultList.push_back(checkResult);
356 : 17 : }
357 : : }
358 : :
359 [ + - ]: 11 : situationsCreated(resultList);
360 : 11 : return resultList;
361 : 0 : }
362 : :
363 : : template <class OBJECT_INSTANCE_TYPE>
364 : 11 : void RssRouteChecker<OBJECT_INSTANCE_TYPE>::appendObjectsToResults(RssObjectResultList &objectResultList)
365 : : {
366 [ + + ]: 20 : for (auto const &vehicle : mVehicleMap)
367 : : {
368 [ + - ]: 9 : if (vehicle.second.mActive)
369 : : {
370 [ + - ]: 9 : RssObjectResult objectResult;
371 [ + - ]: 9 : objectResult.object_data = vehicle.second.mRssObjectData;
372 [ + - ]: 9 : objectResultList.push_back(objectResult);
373 : 9 : }
374 : : }
375 [ - + ]: 11 : for (auto const &pedestrian : mPedestrianMap)
376 : : {
377 [ # # ]: 0 : if (pedestrian.second.mActive)
378 : : {
379 [ # # ]: 0 : RssObjectResult objectResult;
380 [ # # ]: 0 : objectResult.object_data = pedestrian.second.mRssObjectData;
381 [ # # ]: 0 : objectResultList.push_back(objectResult);
382 : 0 : }
383 : : }
384 [ + + ]: 13 : for (auto const &artificialObject : mArtificialObjectMap)
385 : : {
386 [ + - ]: 2 : if (artificialObject.second.mActive)
387 : : {
388 [ + - ]: 2 : RssObjectResult objectResult;
389 [ + - ]: 2 : objectResult.object_data = artificialObject.second.mRssObjectData;
390 [ + - ]: 2 : objectResultList.push_back(objectResult);
391 : 2 : }
392 : : }
393 : 11 : }
394 : :
395 : : template <class OBJECT_INSTANCE_TYPE>
396 : 11 : RssObjectResultList RssRouteChecker<OBJECT_INSTANCE_TYPE>::checkObjects(
397 : : std::chrono::system_clock::time_point const ¤tTime,
398 : : ::ad::physics::Distance const vehicleSamplingDistance,
399 : : RssEgoVehicleAdapterList<OBJECT_INSTANCE_TYPE> &egoVehicles,
400 : : RssVehicleAdapterList<OBJECT_INSTANCE_TYPE> &vehicles,
401 : : ::ad::physics::Distance const pedestrianSamplingDistance,
402 : : RssPedestrianAdapterList<OBJECT_INSTANCE_TYPE> &pedestrians,
403 : : typename RssRouteCheckerVehicleData<OBJECT_INSTANCE_TYPE>::RouteExtensionMode routeExtensionMode)
404 : : {
405 : 11 : mTimeIndex++;
406 : 11 : RssObjectResultList rssObjectResultList;
407 : : try
408 : : {
409 : : // update object maps
410 : 11 : updateObjectMap<RssRouteCheckerEgoVehicleData<OBJECT_INSTANCE_TYPE>, RssEgoVehicleAdapter<OBJECT_INSTANCE_TYPE>>(
411 [ + - ]: 11 : currentTime, mEgoVehicleMap, egoVehicles);
412 : 11 : updateObjectMap<RssRouteCheckerVehicleData<OBJECT_INSTANCE_TYPE>, RssVehicleAdapter<OBJECT_INSTANCE_TYPE>>(
413 [ + - ]: 11 : currentTime, mVehicleMap, vehicles);
414 : 11 : updateObjectMap<RssRouteCheckerPedestrianData<OBJECT_INSTANCE_TYPE>, RssPedestrianAdapter<OBJECT_INSTANCE_TYPE>>(
415 [ + - ]: 11 : currentTime, mPedestrianMap, pedestrians);
416 : :
417 : : // update object data
418 : 11 : ParallelObjectUpdater<OBJECT_INSTANCE_TYPE> objectUpdater(currentTime,
419 : : vehicleSamplingDistance,
420 : : pedestrianSamplingDistance,
421 : : mArtificialObjectSamplingDistance,
422 [ + - ]: 11 : mMapAreaLanes);
423 [ + - ]: 11 : tbb::parallel_for_each(mEgoVehicleMap.begin(), mEgoVehicleMap.end(), objectUpdater);
424 [ + - ]: 11 : objectDataUpdated(mEgoVehicleMap);
425 [ + - ]: 11 : tbb::parallel_for_each(mVehicleMap.begin(), mVehicleMap.end(), objectUpdater);
426 [ + - ]: 11 : objectDataUpdated(mVehicleMap);
427 [ + - ]: 11 : tbb::parallel_for_each(mPedestrianMap.begin(), mPedestrianMap.end(), objectUpdater);
428 [ + - ]: 11 : objectDataUpdated(mPedestrianMap);
429 : :
430 : : // update routes
431 : 11 : ParallelRouteUpdater<OBJECT_INSTANCE_TYPE> routeUpdater(
432 [ + - ]: 11 : objectUpdater.getMinRoutePreviewDistance(), mMapAreaLanes, routeExtensionMode);
433 [ + - ]: 11 : tbb::parallel_for_each(mEgoVehicleMap.begin(), mEgoVehicleMap.end(), routeUpdater);
434 [ + - ]: 11 : routesUpdated(mEgoVehicleMap);
435 [ + - ]: 11 : tbb::parallel_for_each(mVehicleMap.begin(), mVehicleMap.end(), routeUpdater);
436 [ + - ]: 11 : routesUpdated(mVehicleMap);
437 : :
438 : 11 : RssArtificialObjectAdapterList<OBJECT_INSTANCE_TYPE> artificialObjects;
439 : : // keep the following block as it's essential to protect the callback registration
440 : : {
441 [ + - ]: 11 : core::shared_lock_guard const lock_shared(mCallbackRwLock);
442 [ + - ]: 11 : if (mArtificialObjectsInjectionCallback != nullptr)
443 : : {
444 [ + - ]: 11 : artificialObjects = mArtificialObjectsInjectionCallback(mEgoVehicleMap, mVehicleMap, mPedestrianMap);
445 : : }
446 : 11 : }
447 : : updateObjectMap<RssRouteCheckerArtificialObjectData<OBJECT_INSTANCE_TYPE>,
448 : 11 : RssArtificialObjectAdapter<OBJECT_INSTANCE_TYPE>>(
449 [ + - ]: 11 : currentTime, mArtificialObjectMap, artificialObjects);
450 [ + - ]: 11 : tbb::parallel_for_each(mArtificialObjectMap.begin(), mArtificialObjectMap.end(), objectUpdater);
451 [ + - ]: 11 : objectDataUpdated(mArtificialObjectMap);
452 : :
453 [ + - ]: 11 : updateRssCheckers();
454 : :
455 [ + - ]: 11 : rssObjectResultList = createSituations();
456 : :
457 : 11 : ParallelChecker<OBJECT_INSTANCE_TYPE> parallelChecker(mEgoVehicleMap);
458 [ + - ]: 11 : tbb::parallel_for_each(rssObjectResultList.begin(), rssObjectResultList.end(), parallelChecker);
459 [ + - ]: 11 : checksPerformed(rssObjectResultList);
460 : :
461 [ + - ]: 11 : appendObjectsToResults(rssObjectResultList);
462 : 11 : }
463 [ - - ]: 0 : catch (std::exception &e)
464 : : {
465 [ - - - - : 0 : getLogger()->critical("RssRouteChecker::checkObjects>> Exception caught'{}'", e.what());
- - ]
466 : 0 : rssObjectResultList.clear();
467 : : }
468 [ - - ]: 0 : catch (...)
469 : : {
470 [ - - - - ]: 0 : getLogger()->critical("RssRouteChecker::checkObjects>> Exception caught");
471 : 0 : rssObjectResultList.clear();
472 : : }
473 : :
474 : 11 : return rssObjectResultList;
475 : 0 : }
476 : :
477 : : template <class OBJECT_INSTANCE_TYPE>
478 : : void RssRouteChecker<OBJECT_INSTANCE_TYPE>::restrictOperationToMapArea(
479 : : ::ad::map::lane::LaneIdSet const &lanesToConsider)
480 : : {
481 : : mMapAreaLanes = lanesToConsider;
482 : : getLogger()->debug("RssRouteChecker::restrictOperationToMapArea>> {}", mMapAreaLanes);
483 : : }
484 : :
485 : : template <class OBJECT_INSTANCE_TYPE>
486 : : bool RssRouteChecker<OBJECT_INSTANCE_TYPE>::restrictOperationToMapArea(
487 : : ::ad::map::point::BoundingSphere const &bounding_sphere)
488 : : {
489 : : mMapAreaLanes = ad::map::lane::getLanesOfMapArea(bounding_sphere);
490 : : getLogger()->debug("RssRouteChecker::restrictOperationToMapArea({})>> {}", bounding_sphere, mMapAreaLanes);
491 : : return !mMapAreaLanes.empty();
492 : : }
493 : :
494 : : template <class OBJECT_INSTANCE_TYPE>
495 : : bool RssRouteChecker<OBJECT_INSTANCE_TYPE>::restrictOperationToMapArea(
496 : : std::vector<::ad::map::lane::MapAreaPredictionStartPoint> const &predictionStartPoints)
497 : : {
498 : : mMapAreaLanes = ad::map::lane::getLanesOfMapArea(predictionStartPoints);
499 : : getLogger()->debug("RssRouteChecker::restrictOperationToMapArea(PredictionStartPoints)>> results in {}",
500 : : mMapAreaLanes);
501 : : return !mMapAreaLanes.empty();
502 : : }
503 : :
504 : : template <class OBJECT_INSTANCE_TYPE>
505 : 1 : bool RssRouteChecker<OBJECT_INSTANCE_TYPE>::restrictOperationToIntersectionArea(
506 : : ::ad::map::point::ENUPoint const &intersectionPoint, ::ad::physics::Distance const &intersectionDistance)
507 : : {
508 : 1 : mMapAreaLanes = ad::map::lane::getLanesOfMapArea(intersectionPoint, intersectionDistance);
509 [ + - ]: 2 : getLogger()->debug("RssRouteChecker::restrictOperationToIntersectionArea({}@{})>> {}",
510 : : intersectionPoint,
511 : : intersectionDistance,
512 [ + - ]: 1 : mMapAreaLanes);
513 : 1 : return !mMapAreaLanes.empty();
514 : : }
515 : :
516 : : template <class OBJECT_INSTANCE_TYPE> void RssRouteChecker<OBJECT_INSTANCE_TYPE>::clearOperationToMapAreaRestriction()
517 : : {
518 : : mMapAreaLanes.clear();
519 : : getLogger()->debug("RssRouteChecker::clearOperationToMapAreaRestriction>> {}", mMapAreaLanes);
520 : : }
521 : :
522 : : } // namespace map
523 : : } // namespace rss
524 : : } // namespace ad
|
