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 : : #include <ad/rss/map/RssWorldModelCreation.hpp>
11 : : #include <ad/rss/map/test_support/RssWorldModelCreationTest.hpp>
12 : : #include <ad/rss/unstructured/TrajectoryPedestrian.hpp>
13 : : #include <ad/rss/unstructured/TrajectoryVehicle.hpp>
14 : : #include <functional>
15 : :
16 : : using namespace ad::rss::map::test_support;
17 : :
18 : : struct RssRouteCheckerTests : public virtual RssMapIntegrationTestBase
19 : : {
20 : 5 : void SetUp() override
21 : : {
22 : 5 : RssMapIntegrationTestBase::SetUp();
23 : :
24 [ + - ]: 5 : mRouteChecker = std::make_shared<::ad::rss::map::RssRouteChecker<void *>>(::ad::physics::Duration(2.));
25 [ + - + - ]: 10 : mRouteChecker->registerArtificialObjectInjectionCallback(
26 : : ad::physics::Distance(1.),
27 : 0 : std::bind(&RssRouteCheckerTests::injectArtificialObjectsCallback,
28 [ + - ]: 5 : this,
29 : : std::placeholders::_1,
30 : : std::placeholders::_2,
31 : : std::placeholders::_3));
32 : 5 : mMinConnectedRouteLengthOriginal = ::ad::rss::map::RssWorldModelCreation::cMinConnectedRouteLength;
33 : : // enforce shorter connected route length to be enable route expansion with parent handling
34 : 5 : *const_cast<ad::physics::Distance *>(&::ad::rss::map::RssWorldModelCreation::cMinConnectedRouteLength)
35 : 5 : = ad::physics::Distance(20.);
36 : :
37 [ + - + - ]: 10 : mRouteChecker->registerCalculateTrajectorySetsCallback(
38 : : ad::rss::world::ObjectType::EgoVehicle,
39 : 0 : std::bind(&RssRouteCheckerTests::calculateTrajectorySetsCallbackEgoVehicle,
40 [ + - ]: 5 : this,
41 : : std::placeholders::_1,
42 : : std::placeholders::_2,
43 : : std::placeholders::_3,
44 : : std::placeholders::_4,
45 : : std::placeholders::_5,
46 : : std::placeholders::_6,
47 : : std::placeholders::_7));
48 [ + - + - ]: 10 : mRouteChecker->registerCalculateTrajectorySetsCallback(
49 : : ad::rss::world::ObjectType::ArtificialPedestrian,
50 : 0 : std::bind(&RssRouteCheckerTests::calculateTrajectorySetsCallbackPedestrian,
51 [ + - ]: 5 : this,
52 : : std::placeholders::_1,
53 : : std::placeholders::_2,
54 : : std::placeholders::_3,
55 : : std::placeholders::_4,
56 : : std::placeholders::_5,
57 : : std::placeholders::_6,
58 : : std::placeholders::_7));
59 : 5 : }
60 : :
61 : 5 : void TearDown() override
62 : : {
63 : 5 : mRouteChecker.reset();
64 : 5 : *const_cast<ad::physics::Distance *>(&::ad::rss::map::RssWorldModelCreation::cMinConnectedRouteLength)
65 : 5 : = mMinConnectedRouteLengthOriginal;
66 : 5 : }
67 : :
68 : 7 : void addObject(RssEgoVehicleAdapterTest::Ptr const ego_vehicle)
69 : : {
70 [ + - ]: 7 : mEgoVehicles.push_back(ego_vehicle);
71 : 7 : }
72 : 3 : void addObject(RssVehicleAdapterTest::Ptr const vehicle)
73 : : {
74 [ + - ]: 3 : mOtherVehicles.push_back(vehicle);
75 : 3 : }
76 : : void addObject(RssPedestrianAdapterTest::Ptr const pedestrian)
77 : : {
78 : : mPedestrians.push_back(pedestrian);
79 : : }
80 : 1 : void addVehicleAsArtificialVehicle(RssVehicleAdapterTest::Ptr const vehicle)
81 : : {
82 : 1 : ::ad::rss::map::RssObjectAdapterData artificial_vehicle_data = *vehicle;
83 : 1 : artificial_vehicle_data.object_type = ::ad::rss::world::ObjectType::ArtificialVehicle;
84 : : auto artificial_vehicle = std::make_shared<::ad::rss::map::RssArtificialObjectAdapter<void *>>(
85 [ + - + - ]: 1 : artificial_vehicle_data, vehicle->getDefaultRssDynamics(), mCheckerControlTest);
86 [ + - ]: 1 : mArtificialObjects.push_back(artificial_vehicle);
87 : 1 : }
88 : :
89 : 1 : void addPedestrianAsArtificialPedestrian(RssPedestrianAdapterTest::Ptr const pedestrian)
90 : : {
91 : 1 : ::ad::rss::map::RssObjectAdapterData artificial_pedestrian_data = *pedestrian;
92 : 1 : artificial_pedestrian_data.object_type = ::ad::rss::world::ObjectType::ArtificialPedestrian;
93 : : auto artificial_pedestrian = std::make_shared<::ad::rss::map::RssArtificialObjectAdapter<void *>>(
94 [ + - + - ]: 1 : artificial_pedestrian_data, pedestrian->getDefaultRssDynamics(), mCheckerControlTest);
95 [ + - ]: 1 : mArtificialObjects.push_back(artificial_pedestrian);
96 : 1 : }
97 : :
98 : : ad::rss::map::RssArtificialObjectAdapterList<void *>
99 : 11 : injectArtificialObjectsCallback(ad::rss::map::RssRouteCheckerEgoVehicleDataMap<void *> const &egoVehicleMap,
100 : : ad::rss::map::RssRouteCheckerVehicleDataMap<void *> const &vehicleMap,
101 : : ad::rss::map::RssRouteCheckerPedestrianDataMap<void *> const &pedestrianMap)
102 : : {
103 : : (void)egoVehicleMap;
104 : : (void)vehicleMap;
105 : : (void)pedestrianMap;
106 : 11 : return mArtificialObjects;
107 : : }
108 : :
109 : : bool
110 : 2 : calculateTrajectorySetsCallbackEgoVehicle(::ad::rss::map::RssRouteCheckerEgoVehicleData<void *> const &egoVehicle,
111 : : ::ad::rss::map::RssRouteId const &routeId,
112 : : ::ad::rss::core::RelativeConstellation const &constellation,
113 : : ::ad::rss::world::ObjectId const &objectId,
114 : : ::ad::rss::core::RelativeObjectState const &vehicleState,
115 : : ::ad::geometry::Polygon &brakePolygon,
116 : : ::ad::geometry::Polygon &continueForwardPolygon)
117 : : {
118 : 2 : bool routeFound = false;
119 [ + + ]: 6 : for (auto const &route : egoVehicle.mRoutes)
120 : : {
121 [ + + ]: 4 : if (route.route_id == routeId)
122 : : {
123 : 2 : routeFound = true;
124 : : }
125 : : }
126 [ - + - - : 2 : EXPECT_TRUE(routeFound);
- - - - -
- ]
127 [ + - - + : 2 : EXPECT_EQ(egoVehicle.mRssObjectData.id, objectId);
- - - - -
- ]
128 [ + - - + : 2 : EXPECT_EQ(egoVehicle.mRssObjectData.id, constellation.ego_id);
- - - - -
- ]
129 [ + - - + : 2 : EXPECT_EQ(ad::rss::world::ObjectType::EgoVehicle, vehicleState.object_type);
- - - - -
- ]
130 : 2 : ::ad::rss::unstructured::TrajectoryVehicle trajectoryVehicle;
131 [ + - ]: 2 : bool result = trajectoryVehicle.calculateTrajectorySets(vehicleState, brakePolygon, continueForwardPolygon);
132 : 2 : return result;
133 : : }
134 : :
135 : : bool
136 : 2 : calculateTrajectorySetsCallbackPedestrian(::ad::rss::map::RssRouteCheckerEgoVehicleData<void *> const &egoVehicle,
137 : : ::ad::rss::map::RssRouteId const &routeId,
138 : : ::ad::rss::core::RelativeConstellation const &constellation,
139 : : ::ad::rss::world::ObjectId const &objectId,
140 : : ::ad::rss::core::RelativeObjectState const &vehicleState,
141 : : ::ad::geometry::Polygon &brakePolygon,
142 : : ::ad::geometry::Polygon &continueForwardPolygon)
143 : : {
144 : 2 : bool routeFound = false;
145 [ + + ]: 6 : for (auto const &route : egoVehicle.mRoutes)
146 : : {
147 [ + + ]: 4 : if (route.route_id == routeId)
148 : : {
149 : 2 : routeFound = true;
150 : : }
151 : : }
152 [ - + - - : 2 : EXPECT_TRUE(routeFound);
- - - - -
- ]
153 [ + - - + : 2 : EXPECT_NE(egoVehicle.mRssObjectData.id, objectId);
- - - - -
- ]
154 [ + - - + : 2 : EXPECT_EQ(objectId, constellation.object_id);
- - - - -
- ]
155 [ + - - + : 2 : EXPECT_EQ(ad::rss::world::ObjectType::ArtificialPedestrian, vehicleState.object_type);
- - - - -
- ]
156 : 2 : ::ad::rss::unstructured::TrajectoryPedestrian trajectoryPedestrian;
157 [ + - ]: 2 : bool result = trajectoryPedestrian.calculateTrajectorySets(vehicleState, brakePolygon, continueForwardPolygon);
158 : 2 : return result;
159 : : }
160 : :
161 : : struct ExpectedResult
162 : : {
163 : 28 : ExpectedResult(uint32_t iRouteId,
164 : : uint32_t iParentRouteId = 0u,
165 : : std::vector<uint32_t> iDangerousObjects = std::vector<uint32_t>())
166 : 28 : : route_id(iRouteId)
167 : 28 : , parent_route_id(iParentRouteId)
168 : : {
169 : 28 : dangerous_state = false;
170 [ + + ]: 41 : for (auto dangerousObject : iDangerousObjects)
171 : : {
172 : 13 : dangerous_state = true;
173 [ + - ]: 13 : dangerous_objects.push_back(ad::rss::world::ObjectId(dangerousObject));
174 : : }
175 : 28 : }
176 : :
177 : : ad::rss::map::RssRouteId route_id;
178 : : bool dangerous_state;
179 : : ad::rss::map::RssRouteId parent_route_id;
180 : : std::vector<ad::rss::world::ObjectId> dangerous_objects;
181 : : };
182 : :
183 : 11 : void runCheck(std::vector<std::vector<ExpectedResult>> const &expectedDangerousStates)
184 : : {
185 : 11 : auto const checkResultList = mRouteChecker->checkObjects(
186 : 11 : std::chrono::system_clock::now(),
187 : : ad::physics::Distance(2.),
188 : 11 : mEgoVehicles,
189 : 11 : mOtherVehicles,
190 : : ad::physics::Distance(1.),
191 : 11 : mPedestrians,
192 [ + - ]: 22 : ad::rss::map::RssRouteCheckerEgoVehicleData<void *>::RouteExtensionMode::eAllowMultipleRoutes);
193 : :
194 : : // one result per object
195 [ + - - + : 11 : ASSERT_EQ(mEgoVehicles.size() + mOtherVehicles.size() + mPedestrians.size() + mArtificialObjects.size(),
- - - - -
- ]
196 [ + - ]: 11 : checkResultList.size());
197 : : // one actual check result per ego vehicle
198 [ + - - + : 11 : ASSERT_EQ(mEgoVehicles.size(), expectedDangerousStates.size());
- - - - -
- + - ]
199 : :
200 [ + + ]: 39 : for (auto const &checkResult : checkResultList)
201 : : {
202 : 28 : ::ad::rss::map::RssEgoVehicleAdapterList<void *>::iterator egoVehicleIter = mEgoVehicles.end();
203 [ + + - + : 28 : switch (checkResult.object_data.type)
- ]
204 : : {
205 : 17 : case ::ad::rss::world::ObjectType::EgoVehicle:
206 : : {
207 : : egoVehicleIter
208 [ + - ]: 17 : = std::find_if(mEgoVehicles.begin(),
209 : : mEgoVehicles.end(),
210 : 46 : [&checkResult](ad::rss::map::RssEgoVehicleAdapter<void *>::ConstPtr ego_vehicle) {
211 : 23 : return ego_vehicle->getObjectId() == checkResult.object_data.id;
212 : : });
213 [ - + - - : 17 : ASSERT_TRUE(egoVehicleIter != mEgoVehicles.end());
- - - - -
- + - ]
214 : 17 : break;
215 : : }
216 : 9 : case ::ad::rss::world::ObjectType::OtherVehicle:
217 : : case ::ad::rss::world::ObjectType::Bicycle:
218 : : case ::ad::rss::world::ObjectType::OtherObject:
219 : : {
220 : : auto vehicleIter
221 [ + - ]: 9 : = std::find_if(mOtherVehicles.begin(),
222 : : mOtherVehicles.end(),
223 : 18 : [&checkResult](ad::rss::map::RssVehicleAdapter<void *>::ConstPtr otherVehicle) {
224 : 9 : return otherVehicle->getObjectId() == checkResult.object_data.id;
225 : : });
226 [ - + - - : 9 : ASSERT_TRUE(vehicleIter != mOtherVehicles.end());
- - - - -
- + - ]
227 : 9 : break;
228 : : }
229 : 0 : case ::ad::rss::world::ObjectType::Pedestrian:
230 : : {
231 : : auto pedestrianIter
232 [ # # ]: 0 : = std::find_if(mPedestrians.begin(),
233 : : mPedestrians.end(),
234 : 0 : [&checkResult](ad::rss::map::RssPedestrianAdapter<void *>::ConstPtr pedestrian) {
235 : 0 : return pedestrian->getObjectId() == checkResult.object_data.id;
236 : : });
237 [ # # # # : 0 : ASSERT_TRUE(pedestrianIter != mPedestrians.end());
# # # # #
# # # ]
238 : 0 : break;
239 : : }
240 : 2 : case ::ad::rss::world::ObjectType::ArtificialObject:
241 : : case ::ad::rss::world::ObjectType::ArtificialVehicle:
242 : : case ::ad::rss::world::ObjectType::ArtificialPedestrian:
243 : : {
244 : : auto artificialObjectIter
245 [ + - ]: 2 : = std::find_if(mArtificialObjects.begin(),
246 : : mArtificialObjects.end(),
247 : 4 : [&checkResult](ad::rss::map::RssArtificialObjectAdapter<void *>::ConstPtr artificialObject) {
248 : 2 : return artificialObject->getObjectId() == checkResult.object_data.id;
249 : : });
250 [ - + - - : 2 : ASSERT_TRUE(artificialObjectIter != mArtificialObjects.end());
- - - - -
- + - ]
251 : 2 : break;
252 : : }
253 : 0 : default:
254 : : {
255 [ # # # # : 0 : ASSERT_TRUE(false) << "Invalid object type";
# # # # #
# # # #
# ]
256 : : }
257 : : }
258 : :
259 [ + + ]: 28 : if (egoVehicleIter != mEgoVehicles.end())
260 : : {
261 [ + - ]: 17 : auto vehicleIndexSigned = std::distance(mEgoVehicles.begin(), egoVehicleIter);
262 [ + - - + : 17 : ASSERT_LE(0, vehicleIndexSigned);
- - - - -
- + - ]
263 : 17 : auto vehicleIndex = static_cast<std::size_t>(vehicleIndexSigned);
264 [ + - ]: 17 : auto expectedDangerousRoutes = expectedDangerousStates[vehicleIndex];
265 [ + - - + : 17 : ASSERT_EQ(expectedDangerousRoutes.size(), checkResult.situation_check_results.size())
- - - - -
- ]
266 [ - - + - ]: 17 : << egoVehicleIter->get()->getObjectId();
267 [ + + ]: 45 : for (auto j = 0u; j < expectedDangerousRoutes.size(); ++j)
268 : : {
269 [ - + - - : 28 : EXPECT_TRUE(checkResult.situation_check_results[j].is_valid);
- - - - -
- ]
270 [ + - - + : 28 : EXPECT_EQ(expectedDangerousRoutes[j].route_id, checkResult.situation_check_results[j].rss_route.route_id)
- - - - -
- ]
271 [ - - ]: 28 : << egoVehicleIter->get()->getObjectId();
272 [ + - - + : 28 : EXPECT_EQ(expectedDangerousRoutes[j].parent_route_id,
- - - - -
- ]
273 : : checkResult.situation_check_results[j].rss_route.parent_route_id)
274 [ - - ]: 28 : << egoVehicleIter->get()->getObjectId();
275 [ + - - + : 28 : EXPECT_EQ(expectedDangerousRoutes[j].dangerous_state,
- - - - -
- ]
276 : : checkResult.situation_check_results[j].result_analysis.dangerous_state)
277 [ - - ]: 28 : << egoVehicleIter->get()->getObjectId();
278 [ + - - + : 28 : EXPECT_EQ(expectedDangerousRoutes[j].dangerous_objects,
- - - - -
- ]
279 : : checkResult.situation_check_results[j].proper_response.dangerous_objects)
280 [ - - ]: 28 : << egoVehicleIter->get()->getObjectId();
281 : : }
282 [ + - ]: 17 : }
283 : : }
284 [ + - ]: 11 : }
285 : :
286 : : ::ad::rss::map::RssRouteChecker<void *>::Ptr mRouteChecker;
287 : :
288 : : ::ad::rss::map::RssPedestrianAdapterList<void *> mPedestrians;
289 : : ::ad::rss::map::RssArtificialObjectAdapterList<void *> mArtificialObjects;
290 : : ::ad::rss::map::RssVehicleAdapterList<void *> mOtherVehicles;
291 : : ::ad::rss::map::RssEgoVehicleAdapterList<void *> mEgoVehicles;
292 : : ::ad::physics::Distance mMinConnectedRouteLengthOriginal;
293 : : };
294 : :
295 : : struct RssRouteCheckerTestsNoExternalData : public RssRouteCheckerTests,
296 : : public ad::rss::map::test_support::RssWorldModelCreationTestWithoutRoute
297 : : {
298 : 2 : void run_two_egos_test()
299 : : {
300 [ + - + - : 2 : auto egoVehicleBack = createEgoVehicle(::ad::rss::world::ObjectId(123), getEgoVehicleStartLocation());
+ - ]
301 [ + - + - : 2 : auto egoVehicleFront = createEgoVehicle(::ad::rss::world::ObjectId(124), locationSouthIncoming());
+ - ]
302 : :
303 [ + - ]: 2 : addObject(egoVehicleBack);
304 [ + - ]: 2 : addObject(egoVehicleFront);
305 [ + - + - : 2 : addObject(createOtherVehicle(::ad::rss::world::ObjectId(10), locationNorthOutgoing()));
+ - ]
306 : :
307 [ + - + - : 12 : runCheck({// ego back has only one prediction
+ - + - +
- + - + -
+ + + + +
+ - - - -
- - ]
308 : : {ExpectedResult(1u)},
309 : : // ego front immediately starts with two predictions
310 : : {ExpectedResult(1u), ExpectedResult(2u)}});
311 : :
312 [ + - + - ]: 2 : egoVehicleFront->updatePosition(locationSouthEntering());
313 [ + - + - ]: 2 : egoVehicleBack->updatePosition(locationSouthIncoming());
314 : :
315 [ + - + - : 14 : runCheck({// ego back now also has two predictions, both times too near to second ego
+ - + - +
- + - + -
+ - + - +
- + - + -
+ + + + +
+ - - - -
- - ]
316 : : {ExpectedResult(1u, 0u, {124}), ExpectedResult(2u, 1u, {124})},
317 : : // ego front state also changes, because front is too near in the back
318 : : {ExpectedResult(1u, 0u, {123}), ExpectedResult(2u, 0u, {123})}});
319 : :
320 [ + - + - ]: 2 : egoVehicleFront->updatePosition(locationNorthExiting());
321 : :
322 [ + - + - : 12 : runCheck({// ego back keeps its predications, but since front is away, it's safe again
+ - + - +
- + - + -
+ - + + +
+ + + - -
- - - - ]
323 : : {ExpectedResult(1u, 0u), ExpectedResult(2u, 1u)},
324 : : // ego front route 1 dropped
325 : : // remains dangerous, but now because of the other vehicle in front
326 : : {ExpectedResult(2u, 0u, {10})}});
327 : 2 : }
328 : : };
329 : :
330 : 4 : TEST_F(RssRouteCheckerTestsNoExternalData, one_ego_one_vehicle)
331 : : {
332 [ + - + - : 1 : auto ego_vehicle = createEgoVehicle(::ad::rss::world::ObjectId(123), getEgoVehicleStartLocation());
+ - ]
333 [ + - ]: 1 : addObject(ego_vehicle);
334 [ + - + - : 1 : addObject(createOtherVehicle(::ad::rss::world::ObjectId(10), locationNorthOutgoing()));
+ - ]
335 : : // not yet at intersection
336 [ + - + - : 3 : runCheck({{ExpectedResult(1u)}});
+ - + - +
+ + + - -
- - ]
337 : :
338 : : // near enough to trigger two route predictions of the ego, second one is extended from first
339 [ + - + - ]: 1 : ego_vehicle->updatePosition(locationSouthIncoming());
340 [ + - + - : 4 : runCheck({{ExpectedResult(1u), ExpectedResult(2u, 1u)}});
+ - + - +
- + + + +
- - - - ]
341 : :
342 : : // leaving the intersection to the north, route 1 dropped
343 [ + - + - ]: 1 : ego_vehicle->updatePosition(locationNorthExiting());
344 [ + - + - : 3 : runCheck({{ExpectedResult(2u, 1u, {10})}});
+ - + - +
- + + + +
- - - - ]
345 : 1 : }
346 : :
347 : 4 : TEST_F(RssRouteCheckerTestsNoExternalData, one_ego_one_artificial_vehicle)
348 : : {
349 [ + - + - : 1 : auto ego_vehicle = createEgoVehicle(::ad::rss::world::ObjectId(123), getEgoVehicleStartLocation());
+ - ]
350 [ + - ]: 1 : addObject(ego_vehicle);
351 [ + - + - : 1 : addVehicleAsArtificialVehicle(createOtherVehicle(::ad::rss::world::ObjectId(15), locationNorthOutgoing()));
+ - ]
352 : :
353 : : // near enough to trigger two route predictions of the ego
354 [ + - + - ]: 1 : ego_vehicle->updatePosition(locationSouthIncoming());
355 [ + - + - : 4 : runCheck({{ExpectedResult(1u), ExpectedResult(2u)}});
+ - + - +
- + + + +
- - - - ]
356 : 1 : }
357 : :
358 : 4 : TEST_F(RssRouteCheckerTestsNoExternalData, one_ego_one_artificial_pedestrian)
359 : : {
360 [ + - + - : 1 : auto ego_vehicle = createEgoVehicle(::ad::rss::world::ObjectId(123), getEgoVehicleStartLocation());
+ - ]
361 [ + - ]: 1 : addObject(ego_vehicle);
362 [ + - + - : 1 : addPedestrianAsArtificialPedestrian(createPedestrian(::ad::rss::world::ObjectId(15), locationSouthIncoming()));
+ - ]
363 : :
364 : : // near enough to trigger two route predictions of the ego
365 [ + - + - ]: 1 : ego_vehicle->updatePosition(locationSouthIncoming());
366 [ + - + - : 4 : runCheck({{ExpectedResult(1u, 0u, {15}), ExpectedResult(2u, 0u, {15})}});
+ - + - +
- + - + -
+ + + + -
- - - ]
367 : 1 : }
368 : :
369 : 4 : TEST_F(RssRouteCheckerTestsNoExternalData, two_egos)
370 : : {
371 : 1 : run_two_egos_test();
372 : 1 : }
373 : :
374 : 4 : TEST_F(RssRouteCheckerTestsNoExternalData, two_egos_restrict_area)
375 : : {
376 [ + - + - ]: 1 : auto southEnteringENU = RssObjectAdapterTest::getENUPoint(locationSouthEntering());
377 [ + - ]: 1 : mRouteChecker->restrictOperationToIntersectionArea(southEnteringENU, ::ad::physics::Distance(50.));
378 [ + - ]: 1 : run_two_egos_test();
379 : 1 : }
380 : :
381 : : //@TODO extend tests by more cases: with external routes, with pedestrians, ensure time jumps are considered correctly,
382 : : //...
|
