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 : : #include "RssCheckTestBaseT.hpp"
10 : :
11 : : namespace ad {
12 : : namespace rss {
13 : : namespace core {
14 : :
15 : : template <class TESTBASE> class RssCheckLateralEgoRightTestBase : public TESTBASE
16 : : {
17 : 53 : world::Object &getEgoObject() override
18 : : {
19 : 53 : return TESTBASE::objectOnSegment5;
20 : : }
21 : :
22 : 53 : world::Object &getConstellationObject(uint32_t) override
23 : : {
24 : 53 : return TESTBASE::objectOnSegment3;
25 : : }
26 : : };
27 : :
28 : : using RssCheckLateralEgoRightTest = RssCheckLateralEgoRightTestBase<RssCheckTestBase>;
29 : :
30 : : using RssCheckLateralEgoRightOutOfMemoryTest = RssCheckLateralEgoRightTestBase<RssCheckOutOfMemoryTestBase>;
31 [ + - + - : 101 : TEST_P(RssCheckLateralEgoRightOutOfMemoryTest, outOfMemoryAnyTime)
+ - + - +
- + - +
- ]
32 : : {
33 [ + - ]: 50 : performOutOfMemoryTest();
34 : 50 : }
35 [ - + - - : 51 : INSTANTIATE_TEST_CASE_P(Range,
- - ]
36 : : RssCheckLateralEgoRightOutOfMemoryTest,
37 : : ::testing::Range(uint64_t(0u), uint64_t(50u), 1u));
38 : :
39 : 4 : TEST_F(RssCheckLateralEgoRightTest, Lateral_Velocity_Towards_Each_Other)
40 : : {
41 : 1 : state::ProperResponse properResponse;
42 [ + - ]: 1 : core::RssCheck rssCheck;
43 : :
44 [ + + ]: 92 : for (uint32_t i = 0; i <= 90; i++)
45 : : {
46 [ + + ]: 182 : for (auto &constellation : worldModel.constellations)
47 : : {
48 : 91 : constellation.ego_vehicle.occupied_regions[0].lat_range.maximum = ParametricValue(1 - (0.01 * i));
49 : 91 : constellation.ego_vehicle.occupied_regions[0].lat_range.minimum = ParametricValue(1 - (0.01 * i + 0.1));
50 : : }
51 : 91 : worldModel.time_index++;
52 : :
53 : : Distance const dMin
54 [ + - ]: 91 : = calculateLateralMinSafeDistance(worldModel.constellations[0].object.velocity.speed_lat_max,
55 : 91 : worldModel.constellations[0].object_rss_dynamics,
56 : 91 : worldModel.constellations[0].ego_vehicle.velocity.speed_lat_max,
57 : 91 : worldModel.constellations[0].ego_vehicle_rss_dynamics);
58 : :
59 [ + - - + : 91 : ASSERT_TRUE(rssCheck.calculateProperResponse(worldModel, properResponse));
- - - - -
- - - +
- ]
60 : :
61 : 91 : Distance const dCur = Distance(6)
62 [ + - + - : 182 : + worldModel.constellations[0].ego_vehicle.occupied_regions[0].lat_range.minimum * Distance(5) - Distance(0.5);
+ - ]
63 [ + - + - ]: 91 : if (dMin < dCur)
64 : : {
65 [ + - ]: 91 : testRestrictions(properResponse.acceleration_restrictions);
66 : : }
67 : : else
68 : : {
69 [ # # ]: 0 : testRestrictions(properResponse.acceleration_restrictions,
70 : : state::LongitudinalResponse::None,
71 : : state::LateralResponse::BrakeMin,
72 : : state::LateralResponse::None);
73 : : }
74 : : }
75 [ + - + - ]: 1 : }
76 : :
77 : 4 : TEST_F(RssCheckLateralEgoRightTest, No_Lateral_Velocity)
78 : : {
79 [ + + ]: 2 : for (auto &constellation : worldModel.constellations)
80 : : {
81 : 1 : constellation.ego_vehicle.velocity.speed_lat_min = kmhToMeterPerSec(0);
82 : 1 : constellation.ego_vehicle.velocity.speed_lat_max = kmhToMeterPerSec(0);
83 : : }
84 : 1 : state::ProperResponse properResponse;
85 [ + - ]: 1 : core::RssCheck rssCheck;
86 : :
87 [ + + ]: 92 : for (uint32_t i = 0; i <= 90; i++)
88 : : {
89 [ + + ]: 182 : for (auto &constellation : worldModel.constellations)
90 : : {
91 : 91 : constellation.ego_vehicle.occupied_regions[0].lat_range.minimum = ParametricValue(0.01 * i);
92 : 91 : constellation.ego_vehicle.occupied_regions[0].lat_range.maximum = ParametricValue(0.01 * i + 0.1);
93 : : }
94 : 91 : worldModel.time_index++;
95 : :
96 [ + - - + : 91 : ASSERT_TRUE(rssCheck.calculateProperResponse(worldModel, properResponse));
- - - - -
- - - +
- ]
97 : :
98 [ + - ]: 91 : testRestrictions(properResponse.acceleration_restrictions);
99 : : }
100 [ + - + - ]: 1 : }
101 : :
102 : 4 : TEST_F(RssCheckLateralEgoRightTest, Lateral_Velocity_Aways_From_Each_Other)
103 : : {
104 : 1 : worldModel.constellations[0].ego_vehicle.velocity.speed_lat_min = kmhToMeterPerSec(5);
105 : 1 : worldModel.constellations[0].ego_vehicle.velocity.speed_lat_max = kmhToMeterPerSec(5);
106 : 1 : worldModel.constellations[0].object.velocity.speed_lat_min = kmhToMeterPerSec(-5);
107 : 1 : worldModel.constellations[0].object.velocity.speed_lat_max = kmhToMeterPerSec(-5);
108 : :
109 : 1 : state::ProperResponse properResponse;
110 [ + - ]: 1 : core::RssCheck rssCheck;
111 : :
112 [ + + ]: 92 : for (uint32_t i = 0; i <= 90; i++)
113 : : {
114 : 91 : worldModel.constellations[0].ego_vehicle.occupied_regions[0].lat_range.maximum = ParametricValue(1 - (0.01 * i));
115 : 91 : worldModel.constellations[0].ego_vehicle.occupied_regions[0].lat_range.minimum
116 : 91 : = ParametricValue(1 - (0.01 * i + 0.1));
117 : 91 : worldModel.time_index++;
118 : :
119 [ + - - + : 91 : ASSERT_TRUE(rssCheck.calculateProperResponse(worldModel, properResponse));
- - - - -
- - - +
- ]
120 : :
121 [ + - ]: 91 : testRestrictions(properResponse.acceleration_restrictions);
122 : : }
123 [ + - + - ]: 1 : }
124 : :
125 : : template <class TESTBASE> class RssCheckLateralEgoLeftTestBase : public TESTBASE
126 : : {
127 : 51 : world::Object &getEgoObject() override
128 : : {
129 : 51 : return TESTBASE::objectOnSegment3;
130 : : }
131 : :
132 : 51 : world::Object &getConstellationObject(uint32_t) override
133 : : {
134 : 51 : return TESTBASE::objectOnSegment5;
135 : : }
136 : : };
137 : :
138 : : using RssCheckLateralEgoLeftTest = RssCheckLateralEgoLeftTestBase<RssCheckTestBase>;
139 : :
140 : : using RssCheckLateralEgoLeftOutOfMemoryTest = RssCheckLateralEgoLeftTestBase<RssCheckOutOfMemoryTestBase>;
141 [ + - + - : 101 : TEST_P(RssCheckLateralEgoLeftOutOfMemoryTest, outOfMemoryAnyTime)
+ - + - +
- + - +
- ]
142 : : {
143 [ + - ]: 50 : performOutOfMemoryTest();
144 : 50 : }
145 [ - + - - : 51 : INSTANTIATE_TEST_CASE_P(Range,
- - ]
146 : : RssCheckLateralEgoLeftOutOfMemoryTest,
147 : : ::testing::Range(uint64_t(0u), uint64_t(50u), 1u));
148 : :
149 : 4 : TEST_F(RssCheckLateralEgoLeftTest, Lateral_Velocity_Towards_Each_Other)
150 : : {
151 : 1 : state::ProperResponse properResponse;
152 [ + - ]: 1 : core::RssCheck rssCheck;
153 : :
154 : : // increase lateral speed to enforce unsafe cases
155 [ + - ]: 1 : worldModel.constellations[0].ego_vehicle.velocity.speed_lat_max += ad::physics::Speed(0.5);
156 [ + - ]: 1 : worldModel.constellations[0].object.velocity.speed_lat_min -= ad::physics::Speed(0.5);
157 [ + + ]: 92 : for (uint32_t i = 0; i <= 90; i++)
158 : : {
159 : 91 : worldModel.constellations[0].ego_vehicle.occupied_regions[0].lat_range.minimum = ParametricValue(0.01 * i);
160 : 91 : worldModel.constellations[0].ego_vehicle.occupied_regions[0].lat_range.maximum = ParametricValue(0.01 * i + 0.1);
161 : 91 : worldModel.time_index++;
162 : :
163 : : Distance const dMin
164 [ + - ]: 91 : = calculateLateralMinSafeDistance(worldModel.constellations[0].ego_vehicle.velocity.speed_lat_max,
165 : 91 : worldModel.constellations[0].ego_vehicle_rss_dynamics,
166 : 91 : worldModel.constellations[0].object.velocity.speed_lat_min,
167 : 91 : worldModel.constellations[0].object_rss_dynamics);
168 : :
169 [ + - - + : 91 : ASSERT_TRUE(rssCheck.calculateProperResponse(worldModel, properResponse));
- - - - -
- - - +
- ]
170 : :
171 : : auto const dCur
172 [ + - + - ]: 91 : = Distance(10) - worldModel.constellations[0].ego_vehicle.occupied_regions[0].lat_range.maximum * Distance(5);
173 [ + - + + ]: 91 : if (dMin < dCur)
174 : : {
175 [ + - ]: 32 : testRestrictions(properResponse.acceleration_restrictions);
176 : : }
177 : : else
178 : : {
179 [ + - ]: 59 : testRestrictions(properResponse.acceleration_restrictions,
180 : : state::LongitudinalResponse::None,
181 : : state::LateralResponse::None,
182 : : state::LateralResponse::BrakeMin);
183 : : }
184 : : }
185 [ + - + - ]: 1 : }
186 : :
187 : : template <class TESTBASE> class RssCheckLateralEgoInTheMiddleTestBase : public TESTBASE
188 : : {
189 : : protected:
190 : : using TESTBASE::testRestrictions;
191 : : using TESTBASE::worldModel;
192 : :
193 : 53 : void SetUp() override
194 : : {
195 : 53 : TESTBASE::SetUp();
196 : : // now we have to shorten the road areas for the two constellations
197 : : // to make left and right side behave the same
198 : : // otherwise we are more cautious on the left side
199 [ + - ]: 53 : worldModel.constellations[0].ego_vehicle_road.erase(worldModel.constellations[0].ego_vehicle_road.begin() + 2);
200 [ + - ]: 53 : worldModel.constellations[1].ego_vehicle_road.erase(worldModel.constellations[1].ego_vehicle_road.begin() + 2);
201 : 53 : }
202 : :
203 : 106 : world::Object &getEgoObject() override
204 : : {
205 : 106 : return TESTBASE::objectOnSegment4;
206 : : }
207 : :
208 : 159 : uint32_t getNumberOfConstellationObjects() override
209 : : {
210 : 159 : return 2u;
211 : : }
212 : :
213 : 106 : world::Object &getConstellationObject(uint32_t index) override
214 : : {
215 [ + + ]: 106 : if (index == 0u)
216 : : {
217 : 53 : return TESTBASE::objectOnSegment3;
218 : : }
219 : : else
220 : : {
221 : 53 : return TESTBASE::objectOnSegment5;
222 : : }
223 : : }
224 : :
225 : 3 : void performTest()
226 : : {
227 [ + + ]: 30 : for (uint32_t j = 1; j <= 9; j++)
228 : : {
229 [ + - + + ]: 27 : if (worldModel.constellations[0].object.occupied_regions[0].lat_range.maximum < ParametricValue(0.9))
230 : : {
231 [ + - ]: 24 : worldModel.constellations[0].object.occupied_regions[0].lat_range.maximum += ParametricValue(0.1);
232 : : }
233 [ + - + - ]: 27 : if (worldModel.constellations[1].object.occupied_regions[0].lat_range.minimum > ParametricValue(0.08))
234 : : {
235 [ + - ]: 27 : worldModel.constellations[1].object.occupied_regions[0].lat_range.minimum -= ParametricValue(0.08);
236 : : }
237 : 27 : state::ProperResponse properResponse;
238 [ + - ]: 27 : core::RssCheck rssCheck;
239 : 27 : bool safeLeftStateExists = false;
240 : 27 : bool safeRightStateExists = false;
241 [ + + ]: 297 : for (uint32_t i = 0; i <= 9; i++)
242 : : {
243 [ + + ]: 810 : for (auto &constellation : worldModel.constellations)
244 : : {
245 [ + - + + ]: 540 : if (constellation.ego_vehicle.velocity.speed_lat_max >= Speed(0.))
246 : : {
247 : 360 : constellation.ego_vehicle.occupied_regions[0].lat_range.minimum = ParametricValue(0.1 * i);
248 : 360 : constellation.ego_vehicle.occupied_regions[0].lat_range.maximum = ParametricValue(0.1 * i + 0.1);
249 : : }
250 : : else
251 : : {
252 : 180 : constellation.ego_vehicle.occupied_regions[0].lat_range.maximum = ParametricValue(1 - (0.1 * i));
253 : 180 : constellation.ego_vehicle.occupied_regions[0].lat_range.minimum = ParametricValue(1 - (0.1 * i + 0.1));
254 : : }
255 : : }
256 : 270 : worldModel.time_index++;
257 : :
258 : 270 : state::LateralResponse expectedLatResponseLeft = state::LateralResponse::None;
259 : 270 : state::LateralResponse expectedLatResponseRight = state::LateralResponse::None;
260 : : Distance const dMinLeft
261 [ + - ]: 270 : = calculateLateralMinSafeDistance(worldModel.constellations[0].object.velocity.speed_lat_max,
262 : 270 : worldModel.constellations[0].object_rss_dynamics,
263 : 270 : worldModel.constellations[0].ego_vehicle.velocity.speed_lat_max,
264 : 270 : worldModel.constellations[0].ego_vehicle_rss_dynamics);
265 : : Distance const dActualLeft
266 [ + - ]: 270 : = (ParametricValue(1) - worldModel.constellations[0].object.occupied_regions[0].lat_range.maximum
267 [ + - ]: 540 : + worldModel.constellations[0].ego_vehicle.occupied_regions[0].lat_range.minimum)
268 [ + - ]: 540 : * Distance(5);
269 [ + - + + ]: 270 : if (dMinLeft > dActualLeft)
270 : : {
271 : 94 : expectedLatResponseLeft = state::LateralResponse::BrakeMin;
272 : : }
273 : : else
274 : : {
275 : 176 : safeLeftStateExists = true;
276 : : }
277 : :
278 : : Distance const dMinRight
279 [ + - ]: 270 : = calculateLateralMinSafeDistance(worldModel.constellations[1].ego_vehicle.velocity.speed_lat_max,
280 : 270 : worldModel.constellations[1].ego_vehicle_rss_dynamics,
281 : 270 : worldModel.constellations[1].object.velocity.speed_lat_max,
282 : 270 : worldModel.constellations[1].object_rss_dynamics);
283 : : Distance const dActualRight
284 [ + - ]: 270 : = (ParametricValue(1) - worldModel.constellations[1].ego_vehicle.occupied_regions[0].lat_range.maximum
285 [ + - ]: 540 : + worldModel.constellations[1].object.occupied_regions[0].lat_range.minimum)
286 [ + - ]: 540 : * Distance(5);
287 [ + - + + ]: 270 : if (dMinRight > dActualRight)
288 : : {
289 : 95 : expectedLatResponseRight = state::LateralResponse::BrakeMin;
290 : : }
291 : : else
292 : : {
293 : 175 : safeRightStateExists = true;
294 : : }
295 : :
296 : 270 : state::LongitudinalResponse expectedLonResponse = state::LongitudinalResponse::None;
297 [ + + + + ]: 270 : if (!safeLeftStateExists || !safeRightStateExists)
298 : : {
299 : 55 : expectedLonResponse = state::LongitudinalResponse::BrakeMin;
300 : : }
301 : :
302 : : #if RSS_CHECK_TEST_DEBUG_OUT
303 : : std::cout << "Testing[j=" << j << ", i=" << i << "]: dMinLeft=" << static_cast<double>(dMinLeft)
304 : : << " dActualLeft=" << static_cast<double>(dActualLeft) << " leftResponse="
305 : : << ((expectedLatResponseLeft == state::LateralResponse::BrakeMin) ? "BrakeMin" : "None")
306 : : << " dMinRight=" << static_cast<double>(dMinRight)
307 : : << " dActualRight=" << static_cast<double>(dActualRight) << " rightResponse="
308 : : << ((expectedLatResponseRight == state::LateralResponse::BrakeMin) ? "BrakeMin" : "None")
309 : : << " lonResponse="
310 : : << ((expectedLonResponse == state::LongitudinalResponse::BrakeMin) ? "BrakeMin" : "None")
311 : : << std::endl;
312 : : #endif
313 [ + - - + : 270 : EXPECT_TRUE(rssCheck.calculateProperResponse(worldModel, properResponse));
- - - - -
- - - ]
314 [ + - ]: 270 : testRestrictions(properResponse.acceleration_restrictions,
315 : : expectedLonResponse,
316 : : expectedLatResponseLeft,
317 : : expectedLatResponseRight);
318 : : }
319 : : }
320 : 3 : }
321 : : };
322 : :
323 : : using RssCheckLateralEgoInTheMiddleTest = RssCheckLateralEgoInTheMiddleTestBase<RssCheckTestBase>;
324 : :
325 : : using RssCheckLateralEgoInTheMiddleOutOfMemoryTest = RssCheckLateralEgoInTheMiddleTestBase<RssCheckOutOfMemoryTestBase>;
326 [ + - + - : 101 : TEST_P(RssCheckLateralEgoInTheMiddleOutOfMemoryTest, outOfMemoryAnyTime)
+ - + - +
- + - +
- ]
327 : : {
328 [ + - ]: 50 : performOutOfMemoryTest();
329 : 50 : }
330 [ - + - - : 51 : INSTANTIATE_TEST_CASE_P(Range,
- - ]
331 : : RssCheckLateralEgoInTheMiddleOutOfMemoryTest,
332 : : ::testing::Range(uint64_t(0u), uint64_t(50u), 1u));
333 : :
334 : 4 : TEST_F(RssCheckLateralEgoInTheMiddleTest, No_Lateral_Velocity)
335 : : {
336 [ + + ]: 3 : for (auto &constellation : worldModel.constellations)
337 : : {
338 : 2 : constellation.ego_vehicle.velocity.speed_lat_min = kmhToMeterPerSec(0);
339 : 2 : constellation.ego_vehicle.velocity.speed_lat_max = kmhToMeterPerSec(0);
340 : : }
341 : :
342 : 1 : performTest();
343 : 1 : }
344 : :
345 : 4 : TEST_F(RssCheckLateralEgoInTheMiddleTest, Lateral_Velocity_To_The_Left)
346 : : {
347 [ + + ]: 3 : for (auto &constellation : worldModel.constellations)
348 : : {
349 : 2 : constellation.ego_vehicle.velocity.speed_lat_min = kmhToMeterPerSec(-3);
350 : 2 : constellation.ego_vehicle.velocity.speed_lat_max = kmhToMeterPerSec(-3);
351 : : }
352 : :
353 : 1 : performTest();
354 : 1 : }
355 : :
356 : 4 : TEST_F(RssCheckLateralEgoInTheMiddleTest, Lateral_Velocity_To_The_Right)
357 : : {
358 [ + + ]: 3 : for (auto &constellation : worldModel.constellations)
359 : : {
360 : 2 : constellation.ego_vehicle.velocity.speed_lat_min = kmhToMeterPerSec(3);
361 : 2 : constellation.ego_vehicle.velocity.speed_lat_max = kmhToMeterPerSec(3);
362 : : }
363 : :
364 : 1 : performTest();
365 : 1 : }
366 : :
367 : : } // namespace core
368 : : } // namespace rss
369 : : } // namespace ad
|
