Branch data Line data Source code
1 : : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
2 : : //
3 : : // Copyright (C) 2018-2022 Intel Corporation
4 : : //
5 : : // SPDX-License-Identifier: LGPL-2.1-only
6 : : //
7 : : // ----------------- END LICENSE BLOCK -----------------------------------
8 : :
9 : : #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 : 730 : virtual ~RssCheckTestBaseT() = default;
29 : :
30 : 730 : virtual void SetUp()
31 : : {
32 : 730 : objectOnSegment0 = createObject(10., 0.);
33 : 730 : objectOnSegment0.object_id = 0u;
34 : : {
35 : 730 : world::OccupiedRegion occupiedRegion;
36 : 730 : occupiedRegion.lon_range.minimum = ParametricValue(0.);
37 : 730 : occupiedRegion.lon_range.maximum = ParametricValue(0.1);
38 : 730 : occupiedRegion.segment_id = 0;
39 : 730 : occupiedRegion.lat_range.minimum = ParametricValue(0.8);
40 : 730 : occupiedRegion.lat_range.maximum = ParametricValue(0.9);
41 [ + - ]: 730 : objectOnSegment0.occupied_regions.push_back(occupiedRegion);
42 : : }
43 : :
44 : 730 : objectOnSegment1 = createObject(10., 0.);
45 : 730 : objectOnSegment1.object_id = 1u;
46 : : {
47 : 730 : world::OccupiedRegion occupiedRegion;
48 : 730 : occupiedRegion.lon_range.minimum = ParametricValue(0.);
49 : 730 : occupiedRegion.lon_range.maximum = ParametricValue(0.1);
50 : 730 : occupiedRegion.segment_id = 1;
51 : 730 : occupiedRegion.lat_range.minimum = ParametricValue(0.8);
52 : 730 : occupiedRegion.lat_range.maximum = ParametricValue(0.9);
53 [ + - ]: 730 : objectOnSegment1.occupied_regions.push_back(occupiedRegion);
54 : : }
55 : :
56 : 730 : objectOnSegment3 = createObject(10., 3.);
57 : 730 : objectOnSegment3.object_id = 3u;
58 : : {
59 : 730 : world::OccupiedRegion occupiedRegion;
60 : 730 : occupiedRegion.lon_range.minimum = ParametricValue(0.);
61 : 730 : occupiedRegion.lon_range.maximum = ParametricValue(0.1);
62 : 730 : occupiedRegion.segment_id = 3;
63 : 730 : occupiedRegion.lat_range.minimum = ParametricValue(0.);
64 : 730 : occupiedRegion.lat_range.maximum = ParametricValue(0.1);
65 [ + - ]: 730 : objectOnSegment3.occupied_regions.push_back(occupiedRegion);
66 : : }
67 : :
68 : 730 : objectOnSegment4 = createObject(10., 0.);
69 : 730 : objectOnSegment4.object_id = 4u;
70 : : {
71 : 730 : world::OccupiedRegion occupiedRegion;
72 : 730 : occupiedRegion.lon_range.minimum = ParametricValue(0.);
73 : 730 : occupiedRegion.lon_range.maximum = ParametricValue(0.1);
74 : 730 : occupiedRegion.segment_id = 4;
75 : 730 : occupiedRegion.lat_range.minimum = ParametricValue(0.45);
76 : 730 : occupiedRegion.lat_range.maximum = ParametricValue(0.55);
77 [ + - ]: 730 : objectOnSegment4.occupied_regions.push_back(occupiedRegion);
78 : : }
79 : :
80 : 730 : objectOnSegment5 = createObject(10., -3.);
81 : 730 : objectOnSegment5.object_id = 5u;
82 : : {
83 : 730 : world::OccupiedRegion occupiedRegion;
84 : 730 : occupiedRegion.lon_range.minimum = ParametricValue(0.);
85 : 730 : occupiedRegion.lon_range.maximum = ParametricValue(0.1);
86 : 730 : occupiedRegion.segment_id = 5;
87 : 730 : occupiedRegion.lat_range.minimum = ParametricValue(0.8);
88 : 730 : occupiedRegion.lat_range.maximum = ParametricValue(0.9);
89 [ + - ]: 730 : objectOnSegment5.occupied_regions.push_back(occupiedRegion);
90 : : }
91 : :
92 : 730 : objectOnSegment6 = createObject(10., 0.);
93 : 730 : objectOnSegment6.object_id = 6u;
94 : : {
95 : 730 : world::OccupiedRegion occupiedRegion;
96 : 730 : occupiedRegion.lon_range.minimum = ParametricValue(0.4);
97 : 730 : occupiedRegion.lon_range.maximum = ParametricValue(0.5);
98 : 730 : occupiedRegion.segment_id = 6;
99 : 730 : occupiedRegion.lat_range.minimum = ParametricValue(0.8);
100 : 730 : occupiedRegion.lat_range.maximum = ParametricValue(0.9);
101 [ + - ]: 730 : objectOnSegment6.occupied_regions.push_back(occupiedRegion);
102 : : }
103 : :
104 : 730 : objectOnSegment7 = createObject(10., 0.);
105 : 730 : objectOnSegment7.object_id = 7u;
106 : : {
107 : 730 : world::OccupiedRegion occupiedRegion;
108 : 730 : occupiedRegion.lon_range.minimum = ParametricValue(0.);
109 : 730 : occupiedRegion.lon_range.maximum = ParametricValue(0.1);
110 : 730 : occupiedRegion.segment_id = 7;
111 : 730 : occupiedRegion.lat_range.minimum = ParametricValue(0.8);
112 : 730 : occupiedRegion.lat_range.maximum = ParametricValue(0.9);
113 [ + - ]: 730 : objectOnSegment7.occupied_regions.push_back(occupiedRegion);
114 : : }
115 : :
116 : 730 : objectOnSegment8 = createObject(10., 0.);
117 : 730 : objectOnSegment8.object_id = 8u;
118 : : {
119 : 730 : world::OccupiedRegion occupiedRegion;
120 : 730 : occupiedRegion.lon_range.minimum = ParametricValue(0.3);
121 : 730 : occupiedRegion.lon_range.maximum = ParametricValue(0.4);
122 : 730 : occupiedRegion.segment_id = 8;
123 : 730 : occupiedRegion.lat_range.minimum = ParametricValue(0.8);
124 : 730 : occupiedRegion.lat_range.maximum = ParametricValue(0.9);
125 [ + - ]: 730 : objectOnSegment8.occupied_regions.push_back(occupiedRegion);
126 : : }
127 : :
128 : 730 : if ((getConstellationType() == world::ConstellationType::IntersectionEgoHasPriority)
129 [ + + ]: 668 : || (getConstellationType() == world::ConstellationType::IntersectionObjectHasPriority)
130 [ + + + + : 1398 : || (getConstellationType() == world::ConstellationType::IntersectionSamePriority))
+ + ]
131 : : {
132 : 164 : createRoadAreaIntersection();
133 : : }
134 : : else
135 : : {
136 : 566 : createRoadAreaNonIntersection();
137 : : }
138 : :
139 : 730 : worldModel.default_ego_vehicle_rss_dynamics = getEgoRssDynamics();
140 [ + + ]: 1565 : for (uint32_t index = 0u; index < getNumberOfConstellationObjects(); index++)
141 : : {
142 : 835 : world::Constellation constellation;
143 [ + - ]: 835 : constellation.constellation_type = getConstellationType();
144 [ + - + - ]: 835 : constellation.object = getConstellationObject(index);
145 [ + - ]: 835 : constellation.object_rss_dynamics = getObjectRssDynamics();
146 [ + - ]: 835 : constellation.ego_vehicle_rss_dynamics = getEgoRssDynamics();
147 [ + - + - ]: 835 : constellation.ego_vehicle = objectAsEgo(getEgoObject());
148 [ + - ]: 835 : constellation.ego_vehicle_road = roadArea;
149 [ + - ]: 835 : constellation.intersecting_road = otherRoadArea;
150 [ + - ]: 835 : worldModel.constellations.push_back(constellation);
151 : : }
152 : 730 : worldModel.time_index = 1;
153 : 730 : }
154 : :
155 : 37 : virtual world::Object &getEgoObject()
156 : : {
157 : 37 : return objectOnSegment1;
158 : : }
159 : :
160 : 1250 : virtual uint32_t getNumberOfConstellationObjects()
161 : : {
162 : 1250 : return 1u;
163 : : }
164 : :
165 : : /**
166 : : * @brief This function is called for every index [0; getNumberOfConstellationObjects())
167 : : *
168 : : * If you overload the getNumberOfConstellationObjects() you have to overload this, too.
169 : : */
170 : 37 : virtual world::Object &getConstellationObject(uint32_t objectIndex)
171 : : {
172 [ + - ]: 37 : if (objectIndex == 0u)
173 : : {
174 : 37 : return objectOnSegment7;
175 : : }
176 : : else
177 : : {
178 [ # # ]: 0 : throw std::out_of_range("Test setup out of range");
179 : : }
180 : : }
181 : :
182 : 730 : virtual void TearDown()
183 : : {
184 : 730 : gNewThrowCounter = 0;
185 : 730 : }
186 : :
187 : 2132 : virtual world::ConstellationType getConstellationType()
188 : : {
189 : 2132 : return world::ConstellationType::SameDirection;
190 : : }
191 : :
192 : 2327 : virtual world::LaneDrivingDirection getDrivingDirection()
193 : : {
194 : 2327 : return world::LaneDrivingDirection::Positive;
195 : : }
196 : :
197 : 3615 : virtual Distance getMiddleRoadSegmentLength()
198 : : {
199 : 3615 : 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.lane_segments)
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.lane_segments)
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.lane_segments)
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.lane_segments)
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].lane_segments[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.lane_segments[0];
258 [ + + ]: 5920 : for (auto const &laneSegment : roadSegment.lane_segments)
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 : 566 : 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 : 566 : world::RoadSegment roadSegment;
277 : 566 : world::LaneSegment laneSegment;
278 [ + - ]: 566 : laneSegment.driving_direction = getDrivingDirection();
279 : :
280 : 566 : laneSegment.id = 0;
281 : 566 : laneSegment.length.minimum = Distance(50);
282 : 566 : laneSegment.length.maximum = Distance(55);
283 : 566 : laneSegment.width.minimum = Distance(5);
284 : 566 : laneSegment.width.maximum = Distance(5);
285 [ + - ]: 566 : roadSegment.lane_segments.push_back(laneSegment);
286 : 566 : laneSegment.id = 1;
287 : 566 : laneSegment.length.minimum = Distance(55);
288 : 566 : laneSegment.length.maximum = Distance(60);
289 : 566 : laneSegment.width.minimum = Distance(5);
290 : 566 : laneSegment.width.maximum = Distance(5);
291 [ + - ]: 566 : roadSegment.lane_segments.push_back(laneSegment);
292 : 566 : laneSegment.id = 2;
293 : 566 : laneSegment.length.minimum = Distance(60);
294 : 566 : laneSegment.length.maximum = Distance(65);
295 : 566 : laneSegment.width.minimum = Distance(5);
296 : 566 : laneSegment.width.maximum = Distance(5);
297 [ + - ]: 566 : roadSegment.lane_segments.push_back(laneSegment);
298 : :
299 [ + - ]: 566 : roadArea.push_back(roadSegment);
300 : 566 : }
301 : :
302 : : {
303 : 566 : world::RoadSegment roadSegment;
304 : 566 : world::LaneSegment laneSegment;
305 [ + - ]: 566 : laneSegment.driving_direction = getDrivingDirection();
306 : :
307 : 566 : laneSegment.id = 3;
308 [ + - ]: 566 : laneSegment.length.minimum = getMiddleRoadSegmentLength();
309 [ + - ]: 566 : laneSegment.length.maximum = getMiddleRoadSegmentLength();
310 : 566 : laneSegment.width.minimum = Distance(5);
311 : 566 : laneSegment.width.maximum = Distance(5);
312 [ + - ]: 566 : roadSegment.lane_segments.push_back(laneSegment);
313 : 566 : laneSegment.id = 4;
314 [ + - ]: 566 : laneSegment.length.minimum = getMiddleRoadSegmentLength();
315 [ + - ]: 566 : laneSegment.length.maximum = getMiddleRoadSegmentLength();
316 : 566 : laneSegment.width.minimum = Distance(5);
317 : 566 : laneSegment.width.maximum = Distance(5);
318 [ + - ]: 566 : roadSegment.lane_segments.push_back(laneSegment);
319 : 566 : laneSegment.id = 5;
320 [ + - ]: 566 : laneSegment.length.minimum = getMiddleRoadSegmentLength();
321 [ + - ]: 566 : laneSegment.length.maximum = getMiddleRoadSegmentLength();
322 : 566 : laneSegment.width.minimum = Distance(5);
323 : 566 : laneSegment.width.maximum = Distance(5);
324 [ + - ]: 566 : roadSegment.lane_segments.push_back(laneSegment);
325 : :
326 [ + - ]: 566 : roadArea.push_back(roadSegment);
327 : 566 : }
328 : :
329 : : {
330 : 566 : world::RoadSegment roadSegment;
331 : 566 : world::LaneSegment laneSegment;
332 [ + - ]: 566 : laneSegment.driving_direction = getDrivingDirection();
333 : :
334 : 566 : laneSegment.id = 6;
335 : 566 : laneSegment.length.minimum = Distance(50);
336 : 566 : laneSegment.length.maximum = Distance(55);
337 : 566 : laneSegment.width.minimum = Distance(3);
338 : 566 : laneSegment.width.maximum = Distance(5);
339 [ + - ]: 566 : roadSegment.lane_segments.push_back(laneSegment);
340 : 566 : laneSegment.id = 7;
341 : 566 : laneSegment.length.minimum = Distance(55);
342 : 566 : laneSegment.length.maximum = Distance(60);
343 : 566 : laneSegment.width.minimum = Distance(3);
344 : 566 : laneSegment.width.maximum = Distance(5);
345 [ + - ]: 566 : roadSegment.lane_segments.push_back(laneSegment);
346 : 566 : laneSegment.id = 8;
347 : 566 : laneSegment.length.minimum = Distance(60);
348 : 566 : laneSegment.length.maximum = Distance(65);
349 : 566 : laneSegment.width.minimum = Distance(3);
350 : 566 : laneSegment.width.maximum = Distance(5);
351 [ + - ]: 566 : roadSegment.lane_segments.push_back(laneSegment);
352 : :
353 [ + - ]: 566 : roadArea.push_back(roadSegment);
354 : 566 : }
355 : 566 : }
356 : :
357 : 164 : void createRoadAreaIntersection()
358 : : {
359 : : // Road with 1 lane, intersection at 6
360 : : // | 6 |- 7 - 8 -
361 : : // | 3 |
362 : : // | 0 |
363 : : {
364 : 164 : world::RoadSegment roadSegment;
365 : 164 : world::LaneSegment laneSegment;
366 [ + - ]: 164 : laneSegment.driving_direction = 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.lane_segments.push_back(laneSegment);
374 [ + - ]: 164 : roadArea.push_back(roadSegment);
375 : 164 : }
376 : :
377 : : {
378 : 164 : world::RoadSegment roadSegment;
379 : 164 : world::LaneSegment laneSegment;
380 [ + - ]: 164 : laneSegment.driving_direction = 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.lane_segments.push_back(laneSegment);
388 [ + - ]: 164 : roadArea.push_back(roadSegment);
389 : 164 : }
390 : :
391 : : {
392 : 164 : world::RoadSegment roadSegment;
393 : 164 : world::LaneSegment laneSegment;
394 [ + - ]: 164 : laneSegment.driving_direction = 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.lane_segments.push_back(laneSegment);
402 [ + - ]: 164 : otherRoadArea.push_back(roadSegment);
403 : 164 : }
404 : :
405 : : {
406 : 164 : world::RoadSegment roadSegment;
407 : 164 : world::LaneSegment laneSegment;
408 [ + - ]: 164 : laneSegment.driving_direction = 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.lane_segments.push_back(laneSegment);
416 [ + - ]: 164 : otherRoadArea.push_back(roadSegment);
417 : 164 : }
418 : :
419 : : {
420 : 164 : 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 : roadSegment.type = world::RoadSegmentType::Intersection;
429 [ + - ]: 164 : roadSegment.lane_segments.push_back(laneSegment);
430 [ + - ]: 164 : roadArea.push_back(roadSegment);
431 [ + - ]: 164 : otherRoadArea.push_back(roadSegment);
432 : 164 : }
433 : 164 : }
434 : :
435 : 3830 : void testRestriction(physics::AccelerationRange const &longitudinal_range,
436 : : state::LongitudinalResponse expectedLonResponse)
437 : : {
438 [ + - - + : 3830 : EXPECT_EQ(longitudinal_range.minimum, worldModel.default_ego_vehicle_rss_dynamics.alpha_lon.brake_max);
- - - - -
- ]
439 [ + + + - ]: 3830 : switch (expectedLonResponse)
440 : : {
441 : 2793 : case state::LongitudinalResponse::None:
442 [ + - - + : 2793 : EXPECT_EQ(longitudinal_range.maximum, worldModel.default_ego_vehicle_rss_dynamics.alpha_lon.accel_max);
- - - - -
- ]
443 : 2793 : break;
444 : 639 : case state::LongitudinalResponse::BrakeMin:
445 [ + - - + : 639 : EXPECT_EQ(longitudinal_range.maximum, worldModel.default_ego_vehicle_rss_dynamics.alpha_lon.brake_min);
- - - - -
- ]
446 : 639 : break;
447 : 398 : case state::LongitudinalResponse::BrakeMinCorrect:
448 [ + - - + : 398 : EXPECT_EQ(longitudinal_range.maximum, worldModel.default_ego_vehicle_rss_dynamics.alpha_lon.brake_min_correct);
- - - - -
- ]
449 : 398 : break;
450 : 0 : default:
451 [ # # # # : 0 : EXPECT_TRUE(false);
# # # # #
# ]
452 : 0 : break;
453 : : }
454 : 3830 : }
455 : :
456 : 7660 : void testRestriction(physics::AccelerationRange const &lateral_range, state::LateralResponse expectedLatResponse)
457 : : {
458 [ + - - + : 7660 : EXPECT_EQ(lateral_range.minimum, std::numeric_limits<physics::Acceleration>::lowest());
- - - - -
- ]
459 [ + + - ]: 7660 : switch (expectedLatResponse)
460 : : {
461 : 7412 : case state::LateralResponse::None:
462 [ + - - + : 7412 : EXPECT_EQ(lateral_range.maximum, worldModel.default_ego_vehicle_rss_dynamics.alpha_lat.accel_max);
- - - - -
- ]
463 : 7412 : break;
464 : 248 : case state::LateralResponse::BrakeMin:
465 [ + - - + : 248 : EXPECT_EQ(lateral_range.maximum, worldModel.default_ego_vehicle_rss_dynamics.alpha_lat.brake_min);
- - - - -
- ]
466 : 248 : break;
467 : 0 : default:
468 [ # # # # : 0 : EXPECT_TRUE(false);
# # # # #
# ]
469 : 0 : break;
470 : : }
471 : 7660 : }
472 : :
473 : 3830 : 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 : 3830 : testRestriction(accelerationRestriction.longitudinal_range, expectedLonResponse);
479 : 3830 : testRestriction(accelerationRestriction.lateral_left_range, expectedLatResponseLeft);
480 : 3830 : testRestriction(accelerationRestriction.lateral_right_range, expectedLatResponseRight);
481 : 3830 : }
482 : :
483 : 1480 : Distance getDistanceToSegmentEnd(world::Object const &object)
484 : : {
485 [ + - ]: 1480 : world::LaneSegment objectRoadSegment = getMergedRoadSegment(object.occupied_regions[0].segment_id);
486 : : // in our tests the vehicle only spans over one segment
487 [ + - ]: 1480 : world::LaneSegment objectLaneSegment = getLaneSegment(object.occupied_regions[0].segment_id);
488 : : Distance const objectMaxDistanceWithinSegment
489 [ + - ]: 1480 : = object.occupied_regions[0].lon_range.maximum * objectLaneSegment.length.maximum;
490 : :
491 [ + - ]: 2960 : return objectRoadSegment.length.minimum - objectMaxDistanceWithinSegment;
492 : : }
493 : :
494 : 1480 : Distance getFrontObjectDistanceFromSegmentBegin()
495 : : {
496 [ + - ]: 1796 : for (auto const &constellation : worldModel.constellations)
497 : : {
498 [ + + + + ]: 1796 : if ((constellation.object.object_id == 6u) || (constellation.object.object_id == 7u)
499 [ - + ]: 316 : || (constellation.object.object_id == 8u))
500 : : {
501 : : // in our tests the vehicle only spans over one segment
502 [ + - ]: 1480 : world::LaneSegment objectLaneSegment = getLaneSegment(constellation.object.occupied_regions[0].segment_id);
503 : : Distance const objectMinDistanceWithinSegment
504 [ + - ]: 1480 : = constellation.object.occupied_regions[0].lon_range.minimum * objectLaneSegment.length.minimum;
505 : :
506 : 1480 : return objectMinDistanceWithinSegment;
507 : : }
508 : : }
509 : 0 : return Distance(0.);
510 : : }
511 : :
512 : 1355 : bool isDistanceSafeLongitudinal()
513 : : {
514 [ + + ]: 2033 : for (auto const &constellation : worldModel.constellations)
515 : : {
516 : 1480 : Distance dMin;
517 [ + - + + : 1480 : switch (getConstellationType())
- ]
518 : : {
519 : 607 : case world::ConstellationType::SameDirection:
520 : 607 : dMin = calculateLongitudinalMinSafeDistance(constellation.ego_vehicle.velocity.speed_lon_min,
521 : 607 : constellation.ego_vehicle_rss_dynamics,
522 : 607 : constellation.object.velocity.speed_lon_max,
523 [ + - ]: 607 : constellation.object_rss_dynamics);
524 : 607 : break;
525 : 873 : case world::ConstellationType::OppositeDirection:
526 [ + - + + ]: 873 : if (getDrivingDirection() == world::LaneDrivingDirection::Negative)
527 : : {
528 : : dMin
529 : 291 : = calculateLongitudinalMinSafeDistanceOppositeDirection(constellation.object.velocity.speed_lon_max,
530 : 291 : constellation.object_rss_dynamics,
531 : 291 : constellation.ego_vehicle.velocity.speed_lon_max,
532 [ + - ]: 291 : constellation.ego_vehicle_rss_dynamics);
533 : : }
534 : : else
535 : : {
536 : : dMin
537 : 582 : = calculateLongitudinalMinSafeDistanceOppositeDirection(constellation.ego_vehicle.velocity.speed_lon_max,
538 : 582 : constellation.ego_vehicle_rss_dynamics,
539 : 582 : constellation.object.velocity.speed_lon_max,
540 [ + - ]: 582 : constellation.object_rss_dynamics);
541 : : }
542 : 873 : break;
543 : 0 : default:
544 [ # # # # : 0 : EXPECT_TRUE(false);
# # # # #
# ]
545 : 0 : break;
546 : : }
547 : :
548 [ + - ]: 1480 : Distance egoDistanceToSegmentEnd = getDistanceToSegmentEnd(constellation.ego_vehicle);
549 [ + - ]: 1480 : Distance objectDistanceFromSegmentBegin = getFrontObjectDistanceFromSegmentBegin();
550 : 1480 : Distance additionalLength{0u};
551 : 1480 : if ((constellation.ego_vehicle.occupied_regions[0].segment_id < 3)
552 [ + + - + : 1480 : || (constellation.ego_vehicle.occupied_regions[0].segment_id > 5))
+ + ]
553 : : {
554 : : // ego in front or in the back, then the middle segment is relevant
555 [ + - ]: 1164 : additionalLength = getMiddleRoadSegmentLength();
556 : : }
557 : : #if RSS_CHECK_TEST_DEBUG_OUT
558 : : std::cout << "isDistanceSafeLongitudinal: dMin=" << static_cast<double>(dMin)
559 : : << " | additionalLength=" << static_cast<double>(additionalLength)
560 : : << " egoDistanceToSegmentEnd=" << static_cast<double>(egoDistanceToSegmentEnd)
561 : : << " objectDistanceFromSegmentBegin=" << static_cast<double>(objectDistanceFromSegmentBegin)
562 : : << std::endl;
563 : : #endif
564 [ + - + - : 1480 : if (dMin >= additionalLength + egoDistanceToSegmentEnd + objectDistanceFromSegmentBegin)
+ - + + ]
565 : : {
566 : 802 : return false;
567 : : }
568 : : }
569 : 553 : return true;
570 : : }
571 : :
572 : 9 : virtual void performDifferentVelocitiesTest(state::LongitudinalResponse expectedLonResponse)
573 : : {
574 : 9 : state::ProperResponse properResponse;
575 [ + - ]: 9 : core::RssCheck rssCheck;
576 : :
577 [ + + ]: 909 : for (uint32_t i = 0; i < 100; i++)
578 : : {
579 [ + + ]: 1900 : for (auto &constellation : worldModel.constellations)
580 : : {
581 : 1000 : constellation.ego_vehicle.velocity.speed_lon_min = kmhToMeterPerSec(i);
582 : 1000 : constellation.ego_vehicle.velocity.speed_lon_max = constellation.ego_vehicle.velocity.speed_lon_min;
583 : : }
584 : 900 : worldModel.time_index++;
585 : :
586 [ + - - + : 900 : ASSERT_TRUE(rssCheck.calculateProperResponse(worldModel, properResponse));
- - - - -
- - - +
- ]
587 : :
588 : : #if RSS_CHECK_TEST_DEBUG_OUT
589 : : std::cout << "TestingVelocity[i=" << i << "]: lonSafe=" << isDistanceSafeLongitudinal() << std::endl;
590 : : #endif
591 [ + - + + ]: 900 : if (isDistanceSafeLongitudinal())
592 : : {
593 [ + - ]: 301 : testRestrictions(properResponse.acceleration_restrictions);
594 : : }
595 : : else
596 : : {
597 [ + - ]: 599 : testRestrictions(properResponse.acceleration_restrictions, expectedLonResponse);
598 : : }
599 : : }
600 [ + - + - ]: 9 : }
601 : :
602 : 5 : virtual void performDifferentDistancesTest(state::LongitudinalResponse expectedLonResponse)
603 : : {
604 : 5 : state::ProperResponse properResponse;
605 [ + - ]: 5 : core::RssCheck rssCheck;
606 : :
607 [ + + ]: 460 : for (uint32_t i = 0; i <= 90; i++)
608 : : {
609 [ + + ]: 1001 : for (auto &constellation : worldModel.constellations)
610 : : {
611 : 546 : constellation.ego_vehicle.occupied_regions[0].lon_range.minimum = ParametricValue(0.01 * i);
612 : 546 : constellation.ego_vehicle.occupied_regions[0].lon_range.maximum = ParametricValue(0.01 * i + 0.1);
613 : : }
614 : 455 : worldModel.time_index++;
615 : :
616 [ + - - + : 455 : ASSERT_TRUE(rssCheck.calculateProperResponse(worldModel, properResponse));
- - - - -
- - - +
- ]
617 : :
618 : : #if RSS_CHECK_TEST_DEBUG_OUT
619 : : std::cout << "TestingDistance[i=" << i << "]: lonSafe=" << isDistanceSafeLongitudinal() << std::endl;
620 : : #endif
621 [ + - + + ]: 455 : if (isDistanceSafeLongitudinal())
622 : : {
623 [ + - ]: 252 : testRestrictions(properResponse.acceleration_restrictions);
624 : : }
625 : : else
626 : : {
627 [ + - ]: 203 : testRestrictions(properResponse.acceleration_restrictions, expectedLonResponse);
628 : : }
629 : : }
630 [ + - + - ]: 5 : }
631 : :
632 : : world::WorldModel worldModel;
633 : : world::Object objectOnSegment0;
634 : : world::Object objectOnSegment1;
635 : : world::Object objectOnSegment3;
636 : : world::Object objectOnSegment4;
637 : : world::Object objectOnSegment5;
638 : : world::Object objectOnSegment6;
639 : : world::Object objectOnSegment7;
640 : : world::Object objectOnSegment8;
641 : : world::RoadArea roadArea;
642 : : world::RoadArea otherRoadArea;
643 : : };
644 : :
645 : : using RssCheckTestBase = RssCheckTestBaseT<testing::Test>;
646 : :
647 : : class RssCheckOutOfMemoryTestBase : public RssCheckTestBaseT<testing::TestWithParam<uint64_t>>
648 : : {
649 : : protected:
650 : 650 : void performOutOfMemoryTest(std::vector<uint64_t> additionalSucceessResults = {})
651 : : {
652 : 650 : state::ProperResponse properResponse;
653 [ + - ]: 650 : core::RssCheck rssCheck;
654 : :
655 [ + - ]: 650 : gNewThrowCounter = GetParam();
656 [ + - ]: 650 : bool const checkResult = rssCheck.calculateProperResponse(worldModel, properResponse);
657 [ + - + + ]: 1287 : if ((GetParam() == 0) || (gNewThrowCounter > 0u)
658 [ + + - + ]: 1665 : || (additionalSucceessResults.end()
659 [ + - + - : 1406 : != std::find(additionalSucceessResults.begin(), additionalSucceessResults.end(), GetParam())))
+ + ]
660 : : {
661 : : // for 0 there is no out of memory
662 : : // as there are not more than a certain amount of allocations while running, from a certain border on
663 : : // the test returns also true
664 [ - + - - : 272 : ASSERT_TRUE(checkResult);
- - - - -
- + - ]
665 [ + - ]: 272 : testRestrictions(properResponse.acceleration_restrictions);
666 : : }
667 : : else
668 : : {
669 [ - + - - : 378 : ASSERT_FALSE(checkResult);
- - - - -
- + - ]
670 : : }
671 [ + - + - ]: 650 : }
672 : : };
673 : :
674 : : } // namespace core
675 : : } // namespace rss
676 : : } // namespace ad
|
