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 "TestSupport.hpp"
10 : : #include "ad/rss/structured/RssNonIntersectionConstellationChecker.hpp"
11 : :
12 : : namespace ad {
13 : : namespace rss {
14 : : namespace structured {
15 : :
16 : : class RssLateralConstellationCheckerTests : public testing::Test
17 : : {
18 : : protected:
19 : 8 : virtual void SetUp()
20 : : {
21 : 8 : constellation.constellation_type = world::ConstellationType::SameDirection;
22 : 8 : constellation.constellation_id = core::RelativeConstellationId(111);
23 : 8 : }
24 : :
25 : 8 : virtual void TearDown()
26 : : {
27 : 8 : }
28 : : core::RelativeObjectState leftVehicle;
29 : : core::RelativeObjectState rightVehicle;
30 : : core::RelativeConstellation constellation;
31 : : state::RssState rssState;
32 : : world::TimeIndex time_index{1u};
33 : : };
34 : :
35 : 4 : TEST_F(RssLateralConstellationCheckerTests, safe_left)
36 : : {
37 : 1 : RssNonIntersectionConstellationChecker checker;
38 [ + - ]: 1 : leftVehicle = createRelativeVehicleStateForLateralMotion(1);
39 [ + - ]: 1 : rightVehicle = createRelativeVehicleStateForLateralMotion(1);
40 : :
41 : 1 : constellation.ego_state = leftVehicle;
42 : 1 : constellation.other_state = rightVehicle;
43 [ + - ]: 1 : constellation.relative_position = createRelativeLateralPosition(core::LateralRelativePosition::AtLeft, Distance(95.));
44 : :
45 [ + - - + : 1 : ASSERT_TRUE(checker.calculateRssStateNonIntersection(time_index++, constellation, rssState));
- - - - -
- - - +
- ]
46 : :
47 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_right, TestSupport::stateWithInformation(cTestSupport.cLateralSafe, constellation));
- + - - -
- - - +
- ]
48 [ + - - + : 1 : ASSERT_EQ(rssState.lateral_state_left, cTestSupport.cLateralSafe);
- - - - -
- + - ]
49 [ + - + - : 1 : ASSERT_EQ(rssState.longitudinal_state,
- + - - -
- - - ]
50 [ + - ]: 1 : TestSupport::stateWithInformation(cTestSupport.cLongitudinalNone, constellation));
51 [ + - ]: 1 : }
52 : :
53 : 4 : TEST_F(RssLateralConstellationCheckerTests, not_safe_overlap_left_spontaneous)
54 : : {
55 : 1 : RssNonIntersectionConstellationChecker checker;
56 [ + - ]: 1 : leftVehicle = createRelativeVehicleStateForLateralMotion(1);
57 [ + - ]: 1 : rightVehicle = createRelativeVehicleStateForLateralMotion(1);
58 : :
59 : 1 : constellation.ego_state = leftVehicle;
60 : 1 : constellation.other_state = rightVehicle;
61 [ + - ]: 1 : constellation.relative_position = createRelativeLateralPosition(core::LateralRelativePosition::OverlapLeft);
62 : :
63 [ + - - + : 1 : ASSERT_TRUE(checker.calculateRssStateNonIntersection(time_index++, constellation, rssState));
- - - - -
- - - +
- ]
64 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_right, TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
- + - - -
- - - +
- ]
65 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_left, TestSupport::stateWithInformation(cTestSupport.cLateralSafe, constellation));
- + - - -
- - - +
- ]
66 [ + - + - : 1 : ASSERT_EQ(rssState.longitudinal_state,
- + - - -
- - - ]
67 [ + - ]: 1 : TestSupport::stateWithInformation(cTestSupport.cLongitudinalBrakeMin, constellation));
68 [ + - ]: 1 : }
69 : :
70 : 4 : TEST_F(RssLateralConstellationCheckerTests, not_safe_overlap_spontaneous)
71 : : {
72 : 1 : RssNonIntersectionConstellationChecker checker;
73 [ + - ]: 1 : leftVehicle = createRelativeVehicleStateForLateralMotion(1);
74 [ + - ]: 1 : rightVehicle = createRelativeVehicleStateForLateralMotion(1);
75 : :
76 : 1 : constellation.ego_state = leftVehicle;
77 : 1 : constellation.other_state = rightVehicle;
78 [ + - ]: 1 : constellation.relative_position = createRelativeLateralPosition(core::LateralRelativePosition::Overlap);
79 : :
80 [ + - - + : 1 : ASSERT_TRUE(checker.calculateRssStateNonIntersection(time_index++, constellation, rssState));
- - - - -
- - - +
- ]
81 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_right, TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
- + - - -
- - - +
- ]
82 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_left, TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
- + - - -
- - - +
- ]
83 [ + - + - : 1 : ASSERT_EQ(rssState.longitudinal_state,
- + - - -
- - - ]
84 [ + - ]: 1 : TestSupport::stateWithInformation(cTestSupport.cLongitudinalBrakeMin, constellation));
85 [ + - ]: 1 : }
86 : :
87 : 4 : TEST_F(RssLateralConstellationCheckerTests, not_safe_overlap_right_spontaneous)
88 : : {
89 : 1 : RssNonIntersectionConstellationChecker checker;
90 [ + - ]: 1 : leftVehicle = createRelativeVehicleStateForLateralMotion(1);
91 [ + - ]: 1 : rightVehicle = createRelativeVehicleStateForLateralMotion(1);
92 : :
93 : 1 : constellation.ego_state = leftVehicle;
94 : 1 : constellation.other_state = rightVehicle;
95 [ + - ]: 1 : constellation.relative_position = createRelativeLateralPosition(core::LateralRelativePosition::OverlapRight);
96 : :
97 [ + - - + : 1 : ASSERT_TRUE(checker.calculateRssStateNonIntersection(time_index++, constellation, rssState));
- - - - -
- - - +
- ]
98 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_right, TestSupport::stateWithInformation(cTestSupport.cLateralSafe, constellation));
- + - - -
- - - +
- ]
99 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_left, TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
- + - - -
- - - +
- ]
100 [ + - + - : 1 : ASSERT_EQ(rssState.longitudinal_state,
- + - - -
- - - ]
101 [ + - ]: 1 : TestSupport::stateWithInformation(cTestSupport.cLongitudinalBrakeMin, constellation));
102 [ + - ]: 1 : }
103 : :
104 : 4 : TEST_F(RssLateralConstellationCheckerTests, safe_right)
105 : : {
106 : 1 : RssNonIntersectionConstellationChecker checker;
107 [ + - ]: 1 : leftVehicle = createRelativeVehicleStateForLateralMotion(1);
108 [ + - ]: 1 : rightVehicle = createRelativeVehicleStateForLateralMotion(1);
109 : :
110 : 1 : constellation.ego_state = leftVehicle;
111 : 1 : constellation.other_state = rightVehicle;
112 : : constellation.relative_position
113 [ + - ]: 1 : = createRelativeLateralPosition(core::LateralRelativePosition::AtRight, Distance(95.));
114 : :
115 [ + - - + : 1 : ASSERT_TRUE(checker.calculateRssStateNonIntersection(time_index++, constellation, rssState));
- - - - -
- - - +
- ]
116 [ + - - + : 1 : ASSERT_EQ(rssState.lateral_state_right, cTestSupport.cLateralSafe);
- - - - -
- + - ]
117 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_left, TestSupport::stateWithInformation(cTestSupport.cLateralSafe, constellation));
- + - - -
- - - +
- ]
118 [ + - + - : 1 : ASSERT_EQ(rssState.longitudinal_state,
- + - - -
- - - ]
119 [ + - ]: 1 : TestSupport::stateWithInformation(cTestSupport.cLongitudinalNone, constellation));
120 [ + - ]: 1 : }
121 : :
122 : 4 : TEST_F(RssLateralConstellationCheckerTests, both_move_right)
123 : : {
124 : 1 : RssNonIntersectionConstellationChecker checker;
125 [ + - ]: 1 : leftVehicle = createRelativeVehicleStateForLateralMotion(1);
126 [ + - ]: 1 : rightVehicle = createRelativeVehicleStateForLateralMotion(1);
127 : :
128 : 1 : constellation.ego_state = leftVehicle;
129 : 1 : constellation.other_state = rightVehicle;
130 : : constellation.relative_position
131 [ + - ]: 1 : = createRelativeLateralPosition(core::LateralRelativePosition::AtLeft, Distance(0.02));
132 : :
133 [ + - - + : 1 : ASSERT_TRUE(checker.calculateRssStateNonIntersection(time_index++, constellation, rssState));
- - - - -
- - - +
- ]
134 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_right,
- + - - -
- - - ]
135 [ + - ]: 1 : TestSupport::stateWithInformation(cTestSupport.cLateralBrakeMin, constellation));
136 [ + - - + : 1 : ASSERT_EQ(rssState.lateral_state_left, cTestSupport.cLateralSafe);
- - - - -
- + - ]
137 [ + - + - : 1 : ASSERT_EQ(rssState.longitudinal_state,
- + - - -
- - - ]
138 [ + - ]: 1 : TestSupport::stateWithInformation(cTestSupport.cLongitudinalBrakeMin, constellation));
139 [ + - ]: 1 : }
140 : :
141 : 4 : TEST_F(RssLateralConstellationCheckerTests, move_towards_each_other)
142 : : {
143 : 1 : RssNonIntersectionConstellationChecker checker;
144 [ + - ]: 1 : leftVehicle = createRelativeVehicleStateForLateralMotion(1);
145 [ + - ]: 1 : rightVehicle = createRelativeVehicleStateForLateralMotion(-1);
146 : :
147 : 1 : constellation.ego_state = leftVehicle;
148 : 1 : constellation.other_state = rightVehicle;
149 : : constellation.relative_position
150 [ + - ]: 1 : = createRelativeLateralPosition(core::LateralRelativePosition::AtLeft, Distance(0.02));
151 : :
152 [ + - - + : 1 : ASSERT_TRUE(checker.calculateRssStateNonIntersection(time_index++, constellation, rssState));
- - - - -
- - - +
- ]
153 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_right,
- + - - -
- - - ]
154 [ + - ]: 1 : TestSupport::stateWithInformation(cTestSupport.cLateralBrakeMin, constellation));
155 [ + - - + : 1 : ASSERT_EQ(rssState.lateral_state_left, cTestSupport.cLateralSafe);
- - - - -
- + - ]
156 [ + - + - : 1 : ASSERT_EQ(rssState.longitudinal_state,
- + - - -
- - - ]
157 [ + - ]: 1 : TestSupport::stateWithInformation(cTestSupport.cLongitudinalBrakeMin, constellation));
158 [ + - ]: 1 : }
159 : :
160 : 4 : TEST_F(RssLateralConstellationCheckerTests, check_input_range)
161 : : {
162 : 1 : RssNonIntersectionConstellationChecker checker;
163 [ + - ]: 1 : leftVehicle = createRelativeVehicleStateForLateralMotion(0);
164 [ + - ]: 1 : rightVehicle = createRelativeVehicleStateForLateralMotion(0);
165 : :
166 : : // ego vehicle "moves" towards right vehicle from left
167 : 1 : constellation.ego_state = leftVehicle;
168 : 1 : constellation.other_state = rightVehicle;
169 [ + + ]: 10 : for (uint32_t i = 10; i > 1; i--)
170 : : {
171 [ + - ]: 9 : constellation.relative_position = createRelativeLateralPosition(core::LateralRelativePosition::AtLeft, Distance(i));
172 : :
173 [ + - - + : 9 : ASSERT_TRUE(checker.calculateRssStateNonIntersection(time_index++, constellation, rssState));
- - - - -
- - - +
- ]
174 [ + - + - : 9 : ASSERT_EQ(rssState.lateral_state_right,
- + - - -
- - - ]
175 [ + - ]: 9 : TestSupport::stateWithInformation(cTestSupport.cLateralSafe, constellation));
176 [ + - - + : 9 : ASSERT_EQ(rssState.lateral_state_left, cTestSupport.cLateralSafe);
- - - - -
- + - ]
177 [ + - + - : 9 : ASSERT_EQ(rssState.longitudinal_state,
- + - - -
- - - ]
178 [ + - ]: 9 : TestSupport::stateWithInformation(cTestSupport.cLongitudinalNone, constellation));
179 : : }
180 : :
181 : : // near enough: trigger brake
182 [ + - ]: 1 : constellation.relative_position = createRelativeLateralPosition(core::LateralRelativePosition::AtLeft, Distance(1));
183 : :
184 [ + - - + : 1 : ASSERT_TRUE(checker.calculateRssStateNonIntersection(time_index++, constellation, rssState));
- - - - -
- - - +
- ]
185 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_right,
- + - - -
- - - ]
186 [ + - ]: 1 : TestSupport::stateWithInformation(cTestSupport.cLateralBrakeMin, constellation));
187 [ + - - + : 1 : ASSERT_EQ(rssState.lateral_state_left, cTestSupport.cLateralSafe);
- - - - -
- + - ]
188 [ + - + - : 1 : ASSERT_EQ(rssState.longitudinal_state,
- + - - -
- - - ]
189 [ + - ]: 1 : TestSupport::stateWithInformation(cTestSupport.cLongitudinalNone, constellation));
190 : :
191 : : // ego vehicle overlaps on left side
192 [ + - ]: 1 : constellation.relative_position = createRelativeLateralPosition(core::LateralRelativePosition::OverlapLeft);
193 : :
194 [ + - - + : 1 : ASSERT_TRUE(checker.calculateRssStateNonIntersection(time_index++, constellation, rssState));
- - - - -
- - - +
- ]
195 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_right,
- + - - -
- - - ]
196 [ + - ]: 1 : TestSupport::stateWithInformation(cTestSupport.cLateralBrakeMin, constellation));
197 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_left, TestSupport::stateWithInformation(cTestSupport.cLateralSafe, constellation));
- + - - -
- - - +
- ]
198 [ + - + - : 1 : ASSERT_EQ(rssState.longitudinal_state,
- + - - -
- - - ]
199 [ + - ]: 1 : TestSupport::stateWithInformation(cTestSupport.cLongitudinalNone, constellation));
200 : :
201 : : // ego vehicle totally overlaps with other vehicle
202 [ + - ]: 1 : constellation.relative_position = createRelativeLateralPosition(core::LateralRelativePosition::Overlap);
203 : :
204 [ + - - + : 1 : ASSERT_TRUE(checker.calculateRssStateNonIntersection(time_index++, constellation, rssState));
- - - - -
- - - +
- ]
205 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_right,
- + - - -
- - - ]
206 [ + - ]: 1 : TestSupport::stateWithInformation(cTestSupport.cLateralBrakeMin, constellation));
207 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_left,
- + - - -
- - - ]
208 [ + - ]: 1 : TestSupport::stateWithInformation(cTestSupport.cLateralBrakeMin, constellation));
209 [ + - + - : 1 : ASSERT_EQ(rssState.longitudinal_state,
- + - - -
- - - ]
210 [ + - ]: 1 : TestSupport::stateWithInformation(cTestSupport.cLongitudinalNone, constellation));
211 : :
212 : : // ego vehicle overlaps on right side
213 [ + - ]: 1 : constellation.relative_position = createRelativeLateralPosition(core::LateralRelativePosition::OverlapRight);
214 : :
215 [ + - - + : 1 : ASSERT_TRUE(checker.calculateRssStateNonIntersection(time_index++, constellation, rssState));
- - - - -
- - - +
- ]
216 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_right, TestSupport::stateWithInformation(cTestSupport.cLateralSafe, constellation));
- + - - -
- - - +
- ]
217 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_left,
- + - - -
- - - ]
218 [ + - ]: 1 : TestSupport::stateWithInformation(cTestSupport.cLateralBrakeMin, constellation));
219 [ + - + - : 1 : ASSERT_EQ(rssState.longitudinal_state,
- + - - -
- - - ]
220 [ + - ]: 1 : TestSupport::stateWithInformation(cTestSupport.cLongitudinalNone, constellation));
221 : :
222 : : // ego vehicle still too near, but on right side
223 [ + - ]: 1 : constellation.relative_position = createRelativeLateralPosition(core::LateralRelativePosition::AtRight, Distance(1));
224 : :
225 [ + - - + : 1 : ASSERT_TRUE(checker.calculateRssStateNonIntersection(time_index++, constellation, rssState));
- - - - -
- - - +
- ]
226 [ + - - + : 1 : ASSERT_EQ(rssState.lateral_state_right, cTestSupport.cLateralSafe);
- - - - -
- + - ]
227 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_left,
- + - - -
- - - ]
228 [ + - ]: 1 : TestSupport::stateWithInformation(cTestSupport.cLateralBrakeMin, constellation));
229 [ + - + - : 1 : ASSERT_EQ(rssState.longitudinal_state,
- + - - -
- - - ]
230 [ + - ]: 1 : TestSupport::stateWithInformation(cTestSupport.cLongitudinalNone, constellation));
231 : :
232 : : // ego vehicle far enough on right side
233 [ + + ]: 9 : for (uint32_t i = 2; i < 10; i++)
234 : : {
235 : : constellation.relative_position
236 [ + - ]: 8 : = createRelativeLateralPosition(core::LateralRelativePosition::AtRight, Distance(i));
237 [ + - - + : 8 : ASSERT_TRUE(checker.calculateRssStateNonIntersection(time_index++, constellation, rssState));
- - - - -
- - - +
- ]
238 [ + - - + : 8 : ASSERT_EQ(rssState.lateral_state_right, cTestSupport.cLateralSafe);
- - - - -
- + - ]
239 [ + - + - : 8 : ASSERT_EQ(rssState.lateral_state_left, TestSupport::stateWithInformation(cTestSupport.cLateralSafe, constellation));
- + - - -
- - - +
- ]
240 [ + - + - : 8 : ASSERT_EQ(rssState.longitudinal_state,
- + - - -
- - - ]
241 [ + - ]: 8 : TestSupport::stateWithInformation(cTestSupport.cLongitudinalNone, constellation));
242 : : }
243 [ + - ]: 1 : }
244 : :
245 : : } // namespace structured
246 : : } // namespace rss
247 : : } // namespace ad
|
