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