Branch data Line data Source code
1 : : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
2 : : //
3 : : // Copyright (C) 2020-2021 Intel Corporation
4 : : //
5 : : // SPDX-License-Identifier: LGPL-2.1-only
6 : : //
7 : : // ----------------- END LICENSE BLOCK -----------------------------------
8 : :
9 : : #include "TestSupport.hpp"
10 : : #include "ad/rss/structured/RssFormulas.hpp"
11 : :
12 : : namespace ad {
13 : : namespace rss {
14 : : namespace structured {
15 : :
16 : 4 : TEST(RssFormulaTestsCalculateSafeLateralDistance, invalid_vehicle_state)
17 : : {
18 : 1 : Distance safe_distance(0.);
19 : :
20 [ + - ]: 1 : core::RelativeObjectState vehicle = createRelativeVehicleStateForLateralMotion(0.);
21 : 1 : vehicle.dynamics.alpha_lat.accel_max = Acceleration(-1.);
22 : 1 : vehicle.dynamics.alpha_lat.brake_min = Acceleration(-1.);
23 [ + - ]: 1 : core::RelativeObjectState otherVehicle = createRelativeVehicleStateForLateralMotion(0.);
24 : :
25 [ + - - + : 1 : ASSERT_FALSE(calculateSafeLateralDistance(vehicle, otherVehicle, safe_distance));
- - - - -
- - - +
- ]
26 [ + - - + : 1 : ASSERT_FALSE(calculateSafeLateralDistance(otherVehicle, vehicle, safe_distance));
- - - - -
- - - +
- ]
27 [ + - - + : 1 : ASSERT_FALSE(calculateSafeLateralDistance(vehicle, vehicle, safe_distance));
- - - - -
- - - +
- ]
28 : : }
29 : :
30 : 4 : TEST(RssFormulaTestsCalculateSafeLateralDistance, same_lateral_speed)
31 : : {
32 : 1 : Distance safe_distance(0.);
33 : :
34 [ + - ]: 1 : std::vector<double> expectedSafeDistance = {1.1, 1.19, 1.19, 2.9, 2.9};
35 : 1 : uint32_t expectedSafeDistanceIndex = 0u;
36 [ + + ]: 6 : for (auto lateralSpeed : {0., 1., -1., 5., -5.})
37 : : {
38 [ + - ]: 5 : core::RelativeObjectState leftVehicle = createRelativeVehicleStateForLateralMotion(lateralSpeed);
39 [ + - ]: 5 : core::RelativeObjectState rightVehicle = createRelativeVehicleStateForLateralMotion(lateralSpeed);
40 : :
41 [ + - - + : 5 : ASSERT_TRUE(calculateSafeLateralDistance(leftVehicle, rightVehicle, safe_distance));
- - - - -
- - - +
- ]
42 [ + - - + : 5 : ASSERT_NEAR(safe_distance.mDistance, expectedSafeDistance[expectedSafeDistanceIndex], 0.01);
- - - - -
- + - ]
43 : :
44 [ + - - + : 5 : ASSERT_TRUE(calculateSafeLateralDistance(rightVehicle, leftVehicle, safe_distance));
- - - - -
- - - +
- ]
45 [ + - - + : 5 : ASSERT_NEAR(safe_distance.mDistance, expectedSafeDistance[expectedSafeDistanceIndex], 0.01);
- - - - -
- + - ]
46 : :
47 : 5 : expectedSafeDistanceIndex++;
48 : : }
49 [ + - ]: 1 : }
50 : :
51 : 4 : TEST(RssFormulaTestsCalculateSafeLateralDistance, one_zero_lateral_speed)
52 : : {
53 : 1 : Distance safe_distance(0.);
54 : :
55 [ + - ]: 1 : std::vector<double> expectedSafeDistanceLeft = {1.84, 0.45, 5.77, 0.};
56 [ + - ]: 1 : std::vector<double> expectedSafeDistanceRight = {0.45, 1.84, 0., 5.77};
57 : 1 : uint32_t expectedSafeDistanceIndex = 0u;
58 [ + + ]: 5 : for (auto lateralSpeed : {1., -1., 5., -5.})
59 : : {
60 [ + - ]: 4 : core::RelativeObjectState leftVehicle = createRelativeVehicleStateForLateralMotion(lateralSpeed);
61 [ + - ]: 4 : core::RelativeObjectState rightVehicle = createRelativeVehicleStateForLateralMotion(0.);
62 : :
63 [ + - - + : 4 : ASSERT_TRUE(calculateSafeLateralDistance(leftVehicle, rightVehicle, safe_distance));
- - - - -
- - - +
- ]
64 [ + - - + : 4 : ASSERT_NEAR(safe_distance.mDistance, expectedSafeDistanceLeft[expectedSafeDistanceIndex], 0.01);
- - - - -
- + - ]
65 : :
66 [ + - - + : 4 : ASSERT_TRUE(calculateSafeLateralDistance(rightVehicle, leftVehicle, safe_distance));
- - - - -
- - - +
- ]
67 [ + - - + : 4 : ASSERT_NEAR(safe_distance.mDistance, expectedSafeDistanceRight[expectedSafeDistanceIndex], 0.01);
- - - - -
- + - ]
68 : :
69 : 4 : expectedSafeDistanceIndex++;
70 : : }
71 [ + - + - ]: 1 : }
72 : :
73 : 4 : TEST(RssFormulaTestsCalculateSafeLateralDistance, lateral_fluctuation_margin_is_considered)
74 : : {
75 : 1 : Distance safe_distance(0.);
76 : :
77 [ + - ]: 1 : std::vector<double> expectedSafeDistance = {1.1, 1.19, 1.19, 2.9, 2.9};
78 : 1 : uint32_t expectedSafeDistanceIndex = 0u;
79 [ + + ]: 6 : for (auto lateralSpeed : {0., 1., -1., 5., -5.})
80 : : {
81 [ + - ]: 5 : core::RelativeObjectState leftVehicle = createRelativeVehicleStateForLateralMotion(lateralSpeed);
82 [ + - ]: 5 : core::RelativeObjectState rightVehicle = createRelativeVehicleStateForLateralMotion(lateralSpeed);
83 : :
84 [ + - - + : 5 : ASSERT_TRUE(calculateSafeLateralDistance(leftVehicle, rightVehicle, safe_distance));
- - - - -
- - - +
- ]
85 [ + - - + : 5 : ASSERT_NEAR(safe_distance.mDistance, expectedSafeDistance[expectedSafeDistanceIndex], 0.01);
- - - - -
- + - ]
86 : :
87 [ + - ]: 5 : leftVehicle.dynamics.lateral_fluctuation_margin += ad::physics::Distance(0.5);
88 : :
89 [ + - - + : 5 : ASSERT_TRUE(calculateSafeLateralDistance(rightVehicle, leftVehicle, safe_distance));
- - - - -
- - - +
- ]
90 [ + - - + : 5 : ASSERT_NEAR(safe_distance.mDistance, expectedSafeDistance[expectedSafeDistanceIndex] + 0.25, 0.01);
- - - - -
- + - ]
91 : :
92 [ + - ]: 5 : rightVehicle.dynamics.lateral_fluctuation_margin += ad::physics::Distance(0.5);
93 : :
94 [ + - - + : 5 : ASSERT_TRUE(calculateSafeLateralDistance(rightVehicle, leftVehicle, safe_distance));
- - - - -
- - - +
- ]
95 [ + - - + : 5 : ASSERT_NEAR(safe_distance.mDistance, expectedSafeDistance[expectedSafeDistanceIndex] + 0.5, 0.01);
- - - - -
- + - ]
96 : :
97 : 5 : expectedSafeDistanceIndex++;
98 : : }
99 [ + - ]: 1 : }
100 : :
101 : : } // namespace structured
102 : : } // namespace rss
103 : : } // namespace ad
|
