Branch data Line data Source code
1 : : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
2 : : //
3 : : // Copyright (C) 2018-2021 Intel Corporation
4 : : //
5 : : // SPDX-License-Identifier: LGPL-2.1-only
6 : : //
7 : : // ----------------- END LICENSE BLOCK -----------------------------------
8 : :
9 : : #pragma once
10 : :
11 : : #include <algorithm>
12 : : #include <limits>
13 : : #include <vector>
14 : :
15 : : #include "TestSupport.hpp"
16 : : #include "ad/rss/core/RssCheck.hpp"
17 : : #include "wrap_new.hpp"
18 : :
19 : : #define RSS_CHECK_TEST_DEBUG_OUT 0
20 : :
21 : : namespace ad {
22 : : namespace rss {
23 : : namespace core {
24 : :
25 : : template <class TESTBASE> class RssCheckTestBaseT : public TESTBASE
26 : : {
27 : : protected:
28 : 731 : virtual ~RssCheckTestBaseT() = default;
29 : :
30 : 731 : virtual void SetUp()
31 : : {
32 : 731 : objectOnSegment0 = createObject(10., 0.);
33 : 731 : objectOnSegment0.objectId = 0u;
34 : : {
35 : 731 : world::OccupiedRegion occupiedRegion;
36 : 731 : occupiedRegion.lonRange.minimum = ParametricValue(0.);
37 : 731 : occupiedRegion.lonRange.maximum = ParametricValue(0.1);
38 : 731 : occupiedRegion.segmentId = 0;
39 : 731 : occupiedRegion.latRange.minimum = ParametricValue(0.8);
40 : 731 : occupiedRegion.latRange.maximum = ParametricValue(0.9);
41 [ + - ]: 731 : objectOnSegment0.occupiedRegions.push_back(occupiedRegion);
42 : : }
43 : :
44 : 731 : objectOnSegment1 = createObject(10., 0.);
45 : 731 : objectOnSegment1.objectId = 1u;
46 : : {
47 : 731 : world::OccupiedRegion occupiedRegion;
48 : 731 : occupiedRegion.lonRange.minimum = ParametricValue(0.);
49 : 731 : occupiedRegion.lonRange.maximum = ParametricValue(0.1);
50 : 731 : occupiedRegion.segmentId = 1;
51 : 731 : occupiedRegion.latRange.minimum = ParametricValue(0.8);
52 : 731 : occupiedRegion.latRange.maximum = ParametricValue(0.9);
53 [ + - ]: 731 : objectOnSegment1.occupiedRegions.push_back(occupiedRegion);
54 : : }
55 : :
56 : 731 : objectOnSegment3 = createObject(10., 3.);
57 : 731 : objectOnSegment3.objectId = 3u;
58 : : {
59 : 731 : world::OccupiedRegion occupiedRegion;
60 : 731 : occupiedRegion.lonRange.minimum = ParametricValue(0.);
61 : 731 : occupiedRegion.lonRange.maximum = ParametricValue(0.1);
62 : 731 : occupiedRegion.segmentId = 3;
63 : 731 : occupiedRegion.latRange.minimum = ParametricValue(0.);
64 : 731 : occupiedRegion.latRange.maximum = ParametricValue(0.1);
65 [ + - ]: 731 : objectOnSegment3.occupiedRegions.push_back(occupiedRegion);
66 : : }
67 : :
68 : 731 : objectOnSegment4 = createObject(10., 0.);
69 : 731 : objectOnSegment4.objectId = 4u;
70 : : {
71 : 731 : world::OccupiedRegion occupiedRegion;
72 : 731 : occupiedRegion.lonRange.minimum = ParametricValue(0.);
73 : 731 : occupiedRegion.lonRange.maximum = ParametricValue(0.1);
74 : 731 : occupiedRegion.segmentId = 4;
75 : 731 : occupiedRegion.latRange.minimum = ParametricValue(0.45);
76 : 731 : occupiedRegion.latRange.maximum = ParametricValue(0.55);
77 [ + - ]: 731 : objectOnSegment4.occupiedRegions.push_back(occupiedRegion);
78 : : }
79 : :
80 : 731 : objectOnSegment5 = createObject(10., -3.);
81 : 731 : objectOnSegment5.objectId = 5u;
82 : : {
83 : 731 : world::OccupiedRegion occupiedRegion;
84 : 731 : occupiedRegion.lonRange.minimum = ParametricValue(0.);
85 : 731 : occupiedRegion.lonRange.maximum = ParametricValue(0.1);
86 : 731 : occupiedRegion.segmentId = 5;
87 : 731 : occupiedRegion.latRange.minimum = ParametricValue(0.8);
88 : 731 : occupiedRegion.latRange.maximum = ParametricValue(0.9);
89 [ + - ]: 731 : objectOnSegment5.occupiedRegions.push_back(occupiedRegion);
90 : : }
91 : :
92 : 731 : objectOnSegment6 = createObject(10., 0.);
93 : 731 : objectOnSegment6.objectId = 6u;
94 : : {
95 : 731 : world::OccupiedRegion occupiedRegion;
96 : 731 : occupiedRegion.lonRange.minimum = ParametricValue(0.4);
97 : 731 : occupiedRegion.lonRange.maximum = ParametricValue(0.5);
98 : 731 : occupiedRegion.segmentId = 6;
99 : 731 : occupiedRegion.latRange.minimum = ParametricValue(0.8);
100 : 731 : occupiedRegion.latRange.maximum = ParametricValue(0.9);
101 [ + - ]: 731 : objectOnSegment6.occupiedRegions.push_back(occupiedRegion);
102 : : }
103 : :
104 : 731 : objectOnSegment7 = createObject(10., 0.);
105 : 731 : objectOnSegment7.objectId = 7u;
106 : : {
107 : 731 : world::OccupiedRegion occupiedRegion;
108 : 731 : occupiedRegion.lonRange.minimum = ParametricValue(0.);
109 : 731 : occupiedRegion.lonRange.maximum = ParametricValue(0.1);
110 : 731 : occupiedRegion.segmentId = 7;
111 : 731 : occupiedRegion.latRange.minimum = ParametricValue(0.8);
112 : 731 : occupiedRegion.latRange.maximum = ParametricValue(0.9);
113 [ + - ]: 731 : objectOnSegment7.occupiedRegions.push_back(occupiedRegion);
114 : : }
115 : :
116 : 731 : objectOnSegment8 = createObject(10., 0.);
117 : 731 : objectOnSegment8.objectId = 8u;
118 : : {
119 : 731 : world::OccupiedRegion occupiedRegion;
120 : 731 : occupiedRegion.lonRange.minimum = ParametricValue(0.3);
121 : 731 : occupiedRegion.lonRange.maximum = ParametricValue(0.4);
122 : 731 : occupiedRegion.segmentId = 8;
123 : 731 : occupiedRegion.latRange.minimum = ParametricValue(0.8);
124 : 731 : occupiedRegion.latRange.maximum = ParametricValue(0.9);
125 [ + - ]: 731 : objectOnSegment8.occupiedRegions.push_back(occupiedRegion);
126 : : }
127 : :
128 : 731 : if ((getSituationType() == situation::SituationType::IntersectionEgoHasPriority)
129 [ + + ]: 669 : || (getSituationType() == situation::SituationType::IntersectionObjectHasPriority)
130 [ + + + + : 1400 : || (getSituationType() == situation::SituationType::IntersectionSamePriority))
+ + ]
131 : : {
132 : 164 : createRoadAreaIntersection();
133 : : }
134 : : else
135 : : {
136 : 567 : createRoadAreaNonIntersection();
137 : : }
138 : :
139 : 731 : worldModel.defaultEgoVehicleRssDynamics = getEgoRssDynamics();
140 [ + + ]: 1567 : for (uint32_t index = 0u; index < getNumberOfSceneObjects(); index++)
141 : : {
142 : 1672 : world::Scene scene;
143 [ + - ]: 836 : scene.situationType = getSituationType();
144 [ + - + - ]: 836 : scene.object = getSceneObject(index);
145 [ + - ]: 836 : scene.objectRssDynamics = getObjectRssDynamics();
146 [ + - ]: 836 : scene.egoVehicleRssDynamics = getEgoRssDynamics();
147 [ + - + - ]: 836 : scene.egoVehicle = objectAsEgo(getEgoObject());
148 [ + - ]: 836 : scene.egoVehicleRoad = roadArea;
149 [ + - ]: 836 : scene.intersectingRoad = otherRoadArea;
150 [ + - ]: 836 : worldModel.scenes.push_back(scene);
151 : : }
152 : 731 : worldModel.timeIndex = 1;
153 : 731 : }
154 : :
155 : 38 : virtual world::Object &getEgoObject()
156 : : {
157 : 38 : return objectOnSegment1;
158 : : }
159 : :
160 : 1252 : virtual uint32_t getNumberOfSceneObjects()
161 : : {
162 : 1252 : return 1u;
163 : : }
164 : :
165 : : /**
166 : : * @brief This function is called for every index [0; getNumberOfSceneObjects())
167 : : *
168 : : * If you overload the getNumberOfSceneObjects() you have to overload this, too.
169 : : */
170 : 38 : virtual world::Object &getSceneObject(uint32_t objectIndex)
171 : : {
172 [ + - ]: 38 : if (objectIndex == 0u)
173 : : {
174 : 38 : return objectOnSegment7;
175 : : }
176 : : else
177 : : {
178 [ # # ]: 0 : throw std::out_of_range("Test setup out of range");
179 : : }
180 : : }
181 : :
182 : 731 : virtual void TearDown()
183 : : {
184 : 731 : gNewThrowCounter = 0;
185 : 731 : }
186 : :
187 : 2136 : virtual situation::SituationType getSituationType()
188 : : {
189 : 2136 : return situation::SituationType::SameDirection;
190 : : }
191 : :
192 : 2330 : virtual world::LaneDrivingDirection getDrivingDirection()
193 : : {
194 : 2330 : return world::LaneDrivingDirection::Positive;
195 : : }
196 : :
197 : 3621 : virtual Distance getMiddleRoadSegmentLength()
198 : : {
199 : 3621 : return Distance(12);
200 : : }
201 : :
202 : 1480 : world::RoadSegment const &getRoadSegment(world::LaneSegmentId landSegmentId)
203 : : {
204 [ + - ]: 1796 : for (auto const &roadSegment : roadArea)
205 : : {
206 [ + + ]: 4308 : for (auto const &laneSegment : roadSegment)
207 : : {
208 [ + + ]: 3992 : if (laneSegment.id == landSegmentId)
209 : : {
210 : 1480 : return roadSegment;
211 : : }
212 : : }
213 : : }
214 [ # # ]: 0 : for (auto const &roadSegment : otherRoadArea)
215 : : {
216 [ # # ]: 0 : for (auto const &laneSegment : roadSegment)
217 : : {
218 [ # # ]: 0 : if (laneSegment.id == landSegmentId)
219 : : {
220 : 0 : return roadSegment;
221 : : }
222 : : }
223 : : }
224 [ # # # # : 0 : EXPECT_TRUE(false);
# # # # #
# ]
225 : 0 : return roadArea[0];
226 : : }
227 : :
228 : 2960 : world::LaneSegment const &getLaneSegment(world::LaneSegmentId landSegmentId)
229 : : {
230 [ + - ]: 6236 : for (auto const &roadSegment : roadArea)
231 : : {
232 [ + + ]: 18792 : for (auto const &laneSegment : roadSegment)
233 : : {
234 [ + + ]: 15516 : if (laneSegment.id == landSegmentId)
235 : : {
236 : 2960 : return laneSegment;
237 : : }
238 : : }
239 : : }
240 [ # # ]: 0 : for (auto const &roadSegment : otherRoadArea)
241 : : {
242 [ # # ]: 0 : for (auto const &laneSegment : roadSegment)
243 : : {
244 [ # # ]: 0 : if (laneSegment.id == landSegmentId)
245 : : {
246 : 0 : return laneSegment;
247 : : }
248 : : }
249 : : }
250 [ # # # # : 0 : EXPECT_TRUE(false);
# # # # #
# ]
251 : 0 : return roadArea[0][0];
252 : : }
253 : :
254 : 1480 : world::LaneSegment getMergedRoadSegment(world::LaneSegmentId landSegmentId)
255 : : {
256 : 1480 : world::RoadSegment const &roadSegment = getRoadSegment(landSegmentId);
257 : 1480 : world::LaneSegment mergedSegment = roadSegment[0];
258 [ + + ]: 5920 : for (auto const &laneSegment : roadSegment)
259 : : {
260 [ + - ]: 4440 : mergedSegment.length.minimum = std::min(mergedSegment.length.minimum, laneSegment.length.minimum);
261 [ + - ]: 4440 : mergedSegment.length.maximum = std::max(mergedSegment.length.maximum, laneSegment.length.maximum);
262 [ + - ]: 4440 : mergedSegment.width.minimum = std::min(mergedSegment.width.minimum, laneSegment.width.minimum);
263 [ + - ]: 4440 : mergedSegment.width.maximum = std::max(mergedSegment.width.maximum, laneSegment.width.maximum);
264 : : }
265 : 1480 : return mergedSegment;
266 : : }
267 : :
268 : 567 : void createRoadAreaNonIntersection()
269 : : {
270 : : // Road with 3 lanes, each with 3 segments
271 : : // | 6 | 7 | 8 |
272 : : // | 3 | 4 | 5 |
273 : : // | 0 | 1 | 2 |
274 : :
275 : : {
276 : 1134 : world::RoadSegment roadSegment;
277 : 567 : world::LaneSegment laneSegment;
278 [ + - ]: 567 : laneSegment.drivingDirection = getDrivingDirection();
279 : :
280 : 567 : laneSegment.id = 0;
281 : 567 : laneSegment.length.minimum = Distance(50);
282 : 567 : laneSegment.length.maximum = Distance(55);
283 : 567 : laneSegment.width.minimum = Distance(5);
284 : 567 : laneSegment.width.maximum = Distance(5);
285 [ + - ]: 567 : roadSegment.push_back(laneSegment);
286 : 567 : laneSegment.id = 1;
287 : 567 : laneSegment.length.minimum = Distance(55);
288 : 567 : laneSegment.length.maximum = Distance(60);
289 : 567 : laneSegment.width.minimum = Distance(5);
290 : 567 : laneSegment.width.maximum = Distance(5);
291 [ + - ]: 567 : roadSegment.push_back(laneSegment);
292 : 567 : laneSegment.id = 2;
293 : 567 : laneSegment.length.minimum = Distance(60);
294 : 567 : laneSegment.length.maximum = Distance(65);
295 : 567 : laneSegment.width.minimum = Distance(5);
296 : 567 : laneSegment.width.maximum = Distance(5);
297 [ + - ]: 567 : roadSegment.push_back(laneSegment);
298 : :
299 [ + - ]: 567 : roadArea.push_back(roadSegment);
300 : : }
301 : :
302 : : {
303 : 1134 : world::RoadSegment roadSegment;
304 : 567 : world::LaneSegment laneSegment;
305 [ + - ]: 567 : laneSegment.drivingDirection = getDrivingDirection();
306 : :
307 : 567 : laneSegment.id = 3;
308 [ + - ]: 567 : laneSegment.length.minimum = getMiddleRoadSegmentLength();
309 [ + - ]: 567 : laneSegment.length.maximum = getMiddleRoadSegmentLength();
310 : 567 : laneSegment.width.minimum = Distance(5);
311 : 567 : laneSegment.width.maximum = Distance(5);
312 [ + - ]: 567 : roadSegment.push_back(laneSegment);
313 : 567 : laneSegment.id = 4;
314 [ + - ]: 567 : laneSegment.length.minimum = getMiddleRoadSegmentLength();
315 [ + - ]: 567 : laneSegment.length.maximum = getMiddleRoadSegmentLength();
316 : 567 : laneSegment.width.minimum = Distance(5);
317 : 567 : laneSegment.width.maximum = Distance(5);
318 [ + - ]: 567 : roadSegment.push_back(laneSegment);
319 : 567 : laneSegment.id = 5;
320 [ + - ]: 567 : laneSegment.length.minimum = getMiddleRoadSegmentLength();
321 [ + - ]: 567 : laneSegment.length.maximum = getMiddleRoadSegmentLength();
322 : 567 : laneSegment.width.minimum = Distance(5);
323 : 567 : laneSegment.width.maximum = Distance(5);
324 [ + - ]: 567 : roadSegment.push_back(laneSegment);
325 : :
326 [ + - ]: 567 : roadArea.push_back(roadSegment);
327 : : }
328 : :
329 : : {
330 : 1134 : world::RoadSegment roadSegment;
331 : 567 : world::LaneSegment laneSegment;
332 [ + - ]: 567 : laneSegment.drivingDirection = getDrivingDirection();
333 : :
334 : 567 : laneSegment.id = 6;
335 : 567 : laneSegment.length.minimum = Distance(50);
336 : 567 : laneSegment.length.maximum = Distance(55);
337 : 567 : laneSegment.width.minimum = Distance(3);
338 : 567 : laneSegment.width.maximum = Distance(5);
339 [ + - ]: 567 : roadSegment.push_back(laneSegment);
340 : 567 : laneSegment.id = 7;
341 : 567 : laneSegment.length.minimum = Distance(55);
342 : 567 : laneSegment.length.maximum = Distance(60);
343 : 567 : laneSegment.width.minimum = Distance(3);
344 : 567 : laneSegment.width.maximum = Distance(5);
345 [ + - ]: 567 : roadSegment.push_back(laneSegment);
346 : 567 : laneSegment.id = 8;
347 : 567 : laneSegment.length.minimum = Distance(60);
348 : 567 : laneSegment.length.maximum = Distance(65);
349 : 567 : laneSegment.width.minimum = Distance(3);
350 : 567 : laneSegment.width.maximum = Distance(5);
351 [ + - ]: 567 : roadSegment.push_back(laneSegment);
352 : :
353 [ + - ]: 567 : roadArea.push_back(roadSegment);
354 : : }
355 : 567 : }
356 : :
357 : 164 : void createRoadAreaIntersection()
358 : : {
359 : : // Road with 1 lane, intersection at 6
360 : : // | 6 |- 7 - 8 -
361 : : // | 3 |
362 : : // | 0 |
363 : : {
364 : 328 : world::RoadSegment roadSegment;
365 : 164 : world::LaneSegment laneSegment;
366 [ + - ]: 164 : laneSegment.drivingDirection = getDrivingDirection();
367 : :
368 : 164 : laneSegment.id = 0;
369 : 164 : laneSegment.length.minimum = Distance(50);
370 : 164 : laneSegment.length.maximum = Distance(55);
371 : 164 : laneSegment.width.minimum = Distance(5);
372 : 164 : laneSegment.width.maximum = Distance(5);
373 [ + - ]: 164 : roadSegment.push_back(laneSegment);
374 [ + - ]: 164 : roadArea.push_back(roadSegment);
375 : : }
376 : :
377 : : {
378 : 328 : world::RoadSegment roadSegment;
379 : 164 : world::LaneSegment laneSegment;
380 [ + - ]: 164 : laneSegment.drivingDirection = getDrivingDirection();
381 : :
382 : 164 : laneSegment.id = 3;
383 : 164 : laneSegment.length.minimum = Distance(50);
384 : 164 : laneSegment.length.maximum = Distance(55);
385 : 164 : laneSegment.width.minimum = Distance(5);
386 : 164 : laneSegment.width.maximum = Distance(5);
387 [ + - ]: 164 : roadSegment.push_back(laneSegment);
388 [ + - ]: 164 : roadArea.push_back(roadSegment);
389 : : }
390 : :
391 : : {
392 : 328 : world::RoadSegment roadSegment;
393 : 164 : world::LaneSegment laneSegment;
394 [ + - ]: 164 : laneSegment.drivingDirection = getDrivingDirection();
395 : :
396 : 164 : laneSegment.id = 7;
397 : 164 : laneSegment.length.minimum = Distance(15);
398 : 164 : laneSegment.length.maximum = Distance(17);
399 : 164 : laneSegment.width.minimum = Distance(5);
400 : 164 : laneSegment.width.maximum = Distance(5);
401 [ + - ]: 164 : roadSegment.push_back(laneSegment);
402 [ + - ]: 164 : otherRoadArea.push_back(roadSegment);
403 : : }
404 : :
405 : : {
406 : 328 : world::RoadSegment roadSegment;
407 : 164 : world::LaneSegment laneSegment;
408 [ + - ]: 164 : laneSegment.drivingDirection = getDrivingDirection();
409 : :
410 : 164 : laneSegment.id = 8;
411 : 164 : laneSegment.length.minimum = Distance(35);
412 : 164 : laneSegment.length.maximum = Distance(38);
413 : 164 : laneSegment.width.minimum = Distance(5);
414 : 164 : laneSegment.width.maximum = Distance(5);
415 [ + - ]: 164 : roadSegment.push_back(laneSegment);
416 [ + - ]: 164 : otherRoadArea.push_back(roadSegment);
417 : : }
418 : :
419 : : {
420 : 328 : world::RoadSegment roadSegment;
421 : 164 : world::LaneSegment laneSegment;
422 : :
423 : 164 : laneSegment.id = 6;
424 : 164 : laneSegment.length.minimum = Distance(5);
425 : 164 : laneSegment.length.maximum = Distance(6);
426 : 164 : laneSegment.width.minimum = Distance(5);
427 : 164 : laneSegment.width.maximum = Distance(5);
428 : 164 : laneSegment.type = world::LaneSegmentType::Intersection;
429 [ + - ]: 164 : roadSegment.push_back(laneSegment);
430 [ + - ]: 164 : roadArea.push_back(roadSegment);
431 [ + - ]: 164 : otherRoadArea.push_back(roadSegment);
432 : : }
433 : 164 : }
434 : :
435 : 3839 : void testRestriction(physics::AccelerationRange const &longitudinalRange,
436 : : state::LongitudinalResponse expectedLonResponse)
437 : : {
438 [ + - - + : 3839 : EXPECT_EQ(longitudinalRange.minimum, worldModel.defaultEgoVehicleRssDynamics.alphaLon.brakeMax);
- - - - -
- ]
439 [ + + + - ]: 3839 : switch (expectedLonResponse)
440 : : {
441 : 2802 : case state::LongitudinalResponse::None:
442 [ + - - + : 2802 : EXPECT_EQ(longitudinalRange.maximum, worldModel.defaultEgoVehicleRssDynamics.alphaLon.accelMax);
- - - - -
- ]
443 : 2802 : break;
444 : 639 : case state::LongitudinalResponse::BrakeMin:
445 [ + - - + : 639 : EXPECT_EQ(longitudinalRange.maximum, worldModel.defaultEgoVehicleRssDynamics.alphaLon.brakeMin);
- - - - -
- ]
446 : 639 : break;
447 : 398 : case state::LongitudinalResponse::BrakeMinCorrect:
448 [ + - - + : 398 : EXPECT_EQ(longitudinalRange.maximum, worldModel.defaultEgoVehicleRssDynamics.alphaLon.brakeMinCorrect);
- - - - -
- ]
449 : 398 : break;
450 : 0 : default:
451 [ # # # # : 0 : EXPECT_TRUE(false);
# # # # #
# ]
452 : 0 : break;
453 : : }
454 : 3839 : }
455 : :
456 : 7678 : void testRestriction(physics::AccelerationRange const &lateralRange, state::LateralResponse expectedLatResponse)
457 : : {
458 [ + - - + : 7678 : EXPECT_EQ(lateralRange.minimum, std::numeric_limits<physics::Acceleration>::lowest());
- - - - -
- ]
459 [ + + - ]: 7678 : switch (expectedLatResponse)
460 : : {
461 : 7430 : case state::LateralResponse::None:
462 [ + - - + : 7430 : EXPECT_EQ(lateralRange.maximum, worldModel.defaultEgoVehicleRssDynamics.alphaLat.accelMax);
- - - - -
- ]
463 : 7430 : break;
464 : 248 : case state::LateralResponse::BrakeMin:
465 [ + - - + : 248 : EXPECT_EQ(lateralRange.maximum, worldModel.defaultEgoVehicleRssDynamics.alphaLat.brakeMin);
- - - - -
- ]
466 : 248 : break;
467 : 0 : default:
468 [ # # # # : 0 : EXPECT_TRUE(false);
# # # # #
# ]
469 : 0 : break;
470 : : }
471 : 7678 : }
472 : :
473 : 3839 : void testRestrictions(state::AccelerationRestriction const &accelerationRestriction,
474 : : state::LongitudinalResponse expectedLonResponse = state::LongitudinalResponse::None,
475 : : state::LateralResponse expectedLatResponseLeft = state::LateralResponse::None,
476 : : state::LateralResponse expectedLatResponseRight = state::LateralResponse::None)
477 : : {
478 : 3839 : testRestriction(accelerationRestriction.longitudinalRange, expectedLonResponse);
479 : 3839 : testRestriction(accelerationRestriction.lateralLeftRange, expectedLatResponseLeft);
480 : 3839 : testRestriction(accelerationRestriction.lateralRightRange, expectedLatResponseRight);
481 : 3839 : }
482 : :
483 : 1480 : Distance getDistanceToSegmentEnd(world::Object const &object)
484 : : {
485 [ + - ]: 1480 : world::LaneSegment objectRoadSegment = getMergedRoadSegment(object.occupiedRegions[0].segmentId);
486 : : // in our tests the vehicle only spans over one segment
487 [ + - ]: 1480 : world::LaneSegment objectLaneSegment = getLaneSegment(object.occupiedRegions[0].segmentId);
488 : : Distance const objectMaxDistanceWithinSegment
489 [ + - ]: 1480 : = object.occupiedRegions[0].lonRange.maximum * objectLaneSegment.length.maximum;
490 : :
491 [ + - ]: 2960 : return objectRoadSegment.length.minimum - objectMaxDistanceWithinSegment;
492 : : }
493 : :
494 : 1480 : Distance getFrontObjectDistanceFromSegmentBegin()
495 : : {
496 [ + - ]: 1796 : for (auto const &scene : worldModel.scenes)
497 : : {
498 [ + + + + : 1796 : if ((scene.object.objectId == 6u) || (scene.object.objectId == 7u) || (scene.object.objectId == 8u))
- + ]
499 : : {
500 : : // in our tests the vehicle only spans over one segment
501 [ + - ]: 1480 : world::LaneSegment objectLaneSegment = getLaneSegment(scene.object.occupiedRegions[0].segmentId);
502 : : Distance const objectMinDistanceWithinSegment
503 [ + - ]: 1480 : = scene.object.occupiedRegions[0].lonRange.minimum * objectLaneSegment.length.minimum;
504 : :
505 : 1480 : return objectMinDistanceWithinSegment;
506 : : }
507 : : }
508 : 0 : return Distance(0.);
509 : : }
510 : :
511 : 1355 : bool isDistanceSafeLongitudinal()
512 : : {
513 [ + + ]: 2033 : for (auto const &scene : worldModel.scenes)
514 : : {
515 : 1480 : Distance dMin;
516 [ + - + + : 1480 : switch (getSituationType())
- ]
517 : : {
518 : 607 : case situation::SituationType::SameDirection:
519 : 607 : dMin = calculateLongitudinalMinSafeDistance(scene.egoVehicle.velocity.speedLonMin,
520 : 607 : scene.egoVehicleRssDynamics,
521 : 607 : scene.object.velocity.speedLonMax,
522 [ + - ]: 607 : scene.objectRssDynamics);
523 : 607 : break;
524 : 873 : case situation::SituationType::OppositeDirection:
525 [ + - + + ]: 873 : if (getDrivingDirection() == world::LaneDrivingDirection::Negative)
526 : : {
527 : 291 : dMin = calculateLongitudinalMinSafeDistanceOppositeDirection(scene.object.velocity.speedLonMax,
528 : 291 : scene.objectRssDynamics,
529 : 291 : scene.egoVehicle.velocity.speedLonMax,
530 [ + - ]: 291 : scene.egoVehicleRssDynamics);
531 : : }
532 : : else
533 : : {
534 : 582 : dMin = calculateLongitudinalMinSafeDistanceOppositeDirection(scene.egoVehicle.velocity.speedLonMax,
535 : 582 : scene.egoVehicleRssDynamics,
536 : 582 : scene.object.velocity.speedLonMax,
537 [ + - ]: 582 : scene.objectRssDynamics);
538 : : }
539 : 873 : break;
540 : 0 : default:
541 [ # # # # : 0 : EXPECT_TRUE(false);
# # # # #
# ]
542 : 0 : break;
543 : : }
544 : :
545 [ + - ]: 1480 : Distance egoDistanceToSegmentEnd = getDistanceToSegmentEnd(scene.egoVehicle);
546 [ + - ]: 1480 : Distance objectDistanceFromSegmentBegin = getFrontObjectDistanceFromSegmentBegin();
547 : 1480 : Distance additionalLength{0u};
548 [ + + - + : 1480 : if ((scene.egoVehicle.occupiedRegions[0].segmentId < 3) || (scene.egoVehicle.occupiedRegions[0].segmentId > 5))
+ + ]
549 : : {
550 : : // ego in front or in the back, then the middle segment is relevant
551 [ + - ]: 1164 : additionalLength = getMiddleRoadSegmentLength();
552 : : }
553 : : #if RSS_CHECK_TEST_DEBUG_OUT
554 : : std::cout << "isDistanceSafeLongitudinal: dMin=" << static_cast<double>(dMin)
555 : : << " | additionalLength=" << static_cast<double>(additionalLength)
556 : : << " egoDistanceToSegmentEnd=" << static_cast<double>(egoDistanceToSegmentEnd)
557 : : << " objectDistanceFromSegmentBegin=" << static_cast<double>(objectDistanceFromSegmentBegin)
558 : : << std::endl;
559 : : #endif
560 [ + - + - : 1480 : if (dMin >= additionalLength + egoDistanceToSegmentEnd + objectDistanceFromSegmentBegin)
+ - + + ]
561 : : {
562 : 802 : return false;
563 : : }
564 : : }
565 : 553 : return true;
566 : : }
567 : :
568 : 9 : virtual void performDifferentVelocitiesTest(state::LongitudinalResponse expectedLonResponse)
569 : : {
570 : 9 : state::ProperResponse properResponse;
571 [ + - ]: 9 : core::RssCheck rssCheck;
572 : :
573 [ + + ]: 909 : for (uint32_t i = 0; i < 100; i++)
574 : : {
575 [ + + ]: 1900 : for (auto &scene : worldModel.scenes)
576 : : {
577 : 1000 : scene.egoVehicle.velocity.speedLonMin = kmhToMeterPerSec(i);
578 : 1000 : scene.egoVehicle.velocity.speedLonMax = scene.egoVehicle.velocity.speedLonMin;
579 : : }
580 : 900 : worldModel.timeIndex++;
581 : :
582 [ + - - + : 900 : ASSERT_TRUE(rssCheck.calculateProperResponse(worldModel, properResponse));
- - - - -
- - - ]
583 : :
584 : : #if RSS_CHECK_TEST_DEBUG_OUT
585 : : std::cout << "TestingVelocity[i=" << i << "]: lonSafe=" << isDistanceSafeLongitudinal() << std::endl;
586 : : #endif
587 [ + - + + ]: 900 : if (isDistanceSafeLongitudinal())
588 : : {
589 [ + - ]: 301 : testRestrictions(properResponse.accelerationRestrictions);
590 : : }
591 : : else
592 : : {
593 [ + - ]: 599 : testRestrictions(properResponse.accelerationRestrictions, expectedLonResponse);
594 : : }
595 : : }
596 : : }
597 : :
598 : 5 : virtual void performDifferentDistancesTest(state::LongitudinalResponse expectedLonResponse)
599 : : {
600 : 5 : state::ProperResponse properResponse;
601 [ + - ]: 5 : core::RssCheck rssCheck;
602 : :
603 [ + + ]: 460 : for (uint32_t i = 0; i <= 90; i++)
604 : : {
605 [ + + ]: 1001 : for (auto &scene : worldModel.scenes)
606 : : {
607 : 546 : scene.egoVehicle.occupiedRegions[0].lonRange.minimum = ParametricValue(0.01 * i);
608 : 546 : scene.egoVehicle.occupiedRegions[0].lonRange.maximum = ParametricValue(0.01 * i + 0.1);
609 : : }
610 : 455 : worldModel.timeIndex++;
611 : :
612 [ + - - + : 455 : ASSERT_TRUE(rssCheck.calculateProperResponse(worldModel, properResponse));
- - - - -
- - - ]
613 : :
614 : : #if RSS_CHECK_TEST_DEBUG_OUT
615 : : std::cout << "TestingDistance[i=" << i << "]: lonSafe=" << isDistanceSafeLongitudinal() << std::endl;
616 : : #endif
617 [ + - + + ]: 455 : if (isDistanceSafeLongitudinal())
618 : : {
619 [ + - ]: 252 : testRestrictions(properResponse.accelerationRestrictions);
620 : : }
621 : : else
622 : : {
623 [ + - ]: 203 : testRestrictions(properResponse.accelerationRestrictions, expectedLonResponse);
624 : : }
625 : : }
626 : : }
627 : :
628 : : world::WorldModel worldModel;
629 : : world::Object objectOnSegment0;
630 : : world::Object objectOnSegment1;
631 : : world::Object objectOnSegment3;
632 : : world::Object objectOnSegment4;
633 : : world::Object objectOnSegment5;
634 : : world::Object objectOnSegment6;
635 : : world::Object objectOnSegment7;
636 : : world::Object objectOnSegment8;
637 : : world::RoadArea roadArea;
638 : : world::RoadArea otherRoadArea;
639 : : };
640 : :
641 : : using RssCheckTestBase = RssCheckTestBaseT<testing::Test>;
642 : :
643 : : class RssCheckOutOfMemoryTestBase : public RssCheckTestBaseT<testing::TestWithParam<uint64_t>>
644 : : {
645 : : protected:
646 : 650 : void performOutOfMemoryTest(std::vector<uint64_t> additionalSucceessResults = {})
647 : : {
648 [ + - ]: 650 : gNewThrowCounter = GetParam();
649 : 650 : state::ProperResponse properResponse;
650 [ + - ]: 650 : core::RssCheck rssCheck;
651 : :
652 [ + - ]: 650 : bool const checkResult = rssCheck.calculateProperResponse(worldModel, properResponse);
653 [ + - + + ]: 1287 : if ((GetParam() == 0) || (gNewThrowCounter > 0u)
654 [ + + - + ]: 1652 : || (additionalSucceessResults.end()
655 [ + - + - : 1380 : != std::find(additionalSucceessResults.begin(), additionalSucceessResults.end(), GetParam())))
+ + ]
656 : : {
657 : : // for 0 there is no out of memory
658 : : // as there are not more than a certain amount of allocations while running, from a certain border on
659 : : // the test returns also true
660 [ - + - - : 285 : ASSERT_TRUE(checkResult);
- - - - -
- ]
661 [ + - ]: 285 : testRestrictions(properResponse.accelerationRestrictions);
662 : : }
663 : : else
664 : : {
665 [ - + - - : 365 : ASSERT_FALSE(checkResult);
- - - - -
- ]
666 : : }
667 : : }
668 : : };
669 : :
670 : : } // namespace core
671 : : } // namespace rss
672 : : } // namespace ad
|
