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 RssLongitudinalConstellationCheckerTests : public testing::Test
17 : : {
18 : : protected:
19 : 11 : virtual void SetUp()
20 : : {
21 : 11 : constellation.constellation_type = world::ConstellationType::SameDirection;
22 : 11 : constellation.constellation_id = core::RelativeConstellationId(111);
23 : 11 : }
24 : :
25 : 11 : virtual void TearDown()
26 : : {
27 : 11 : }
28 : : core::RelativeObjectState leadingVehicle;
29 : : core::RelativeObjectState followingVehicle;
30 : : core::RelativeConstellation constellation;
31 : : state::RssState rssState;
32 : : world::TimeIndex time_index{1u};
33 : : };
34 : :
35 : 4 : TEST_F(RssLongitudinalConstellationCheckerTests, same_direction_leading_ego_safe_distance)
36 : : {
37 : 1 : RssNonIntersectionConstellationChecker checker;
38 [ + - ]: 1 : leadingVehicle = createRelativeVehicleStateForLongitudinalMotion(100);
39 [ + - ]: 1 : followingVehicle = createRelativeVehicleStateForLongitudinalMotion(10);
40 : :
41 : 1 : constellation.ego_state = leadingVehicle;
42 : 1 : constellation.other_state = followingVehicle;
43 : : constellation.relative_position
44 [ + - ]: 1 : = createRelativeLongitudinalPosition(core::LongitudinalRelativePosition::InFront, Distance(95.));
45 : :
46 [ + - - + : 1 : ASSERT_TRUE(checker.calculateRssStateNonIntersection(time_index++, constellation, rssState));
- - - - -
- - - +
- ]
47 [ + - + - : 1 : ASSERT_EQ(rssState.longitudinal_state,
- + - - -
- - - ]
48 [ + - ]: 1 : TestSupport::stateWithInformation(cTestSupport.cLongitudinalSafe, constellation));
49 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_left, TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
- + - - -
- - - +
- ]
50 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_right, TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
- + - - -
- - - +
- ]
51 [ + - ]: 1 : }
52 : :
53 : 4 : TEST_F(RssLongitudinalConstellationCheckerTests, same_direction_leading_other_50kmh_safe)
54 : : {
55 : 1 : RssNonIntersectionConstellationChecker checker;
56 [ + - ]: 1 : leadingVehicle = createRelativeVehicleStateForLongitudinalMotion(50);
57 [ + - ]: 1 : followingVehicle = createRelativeVehicleStateForLongitudinalMotion(50);
58 : 1 : followingVehicle.dynamics.alpha_lon.accel_max = Acceleration(2.);
59 : 1 : followingVehicle.dynamics.alpha_lon.brake_min = Acceleration(-4.);
60 : :
61 : 1 : constellation.ego_state = followingVehicle;
62 : 1 : constellation.other_state = leadingVehicle;
63 : : constellation.relative_position
64 [ + - ]: 1 : = createRelativeLongitudinalPosition(core::LongitudinalRelativePosition::AtBack, Distance(60.));
65 : :
66 [ + - - + : 1 : ASSERT_TRUE(checker.calculateRssStateNonIntersection(time_index++, constellation, rssState));
- - - - -
- - - +
- ]
67 [ + - + - : 1 : ASSERT_EQ(rssState.longitudinal_state,
- + - - -
- - - ]
68 [ + - ]: 1 : TestSupport::stateWithInformation(cTestSupport.cLongitudinalSafe, constellation));
69 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_left, TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
- + - - -
- - - +
- ]
70 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_right, TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
- + - - -
- - - +
- ]
71 [ + - ]: 1 : }
72 : :
73 : 4 : TEST_F(RssLongitudinalConstellationCheckerTests, same_direction_leading_other_50kmh_unsafe)
74 : : {
75 : 1 : RssNonIntersectionConstellationChecker checker;
76 [ + - ]: 1 : leadingVehicle = createRelativeVehicleStateForLongitudinalMotion(50);
77 [ + - ]: 1 : followingVehicle = createRelativeVehicleStateForLongitudinalMotion(50);
78 : 1 : followingVehicle.dynamics.alpha_lon.accel_max = Acceleration(2.);
79 : 1 : followingVehicle.dynamics.alpha_lon.brake_min = Acceleration(-4.);
80 : :
81 : 1 : constellation.ego_state = followingVehicle;
82 : 1 : constellation.other_state = leadingVehicle;
83 : : constellation.relative_position
84 [ + - ]: 1 : = createRelativeLongitudinalPosition(core::LongitudinalRelativePosition::AtBack, Distance(58.));
85 : :
86 [ + - - + : 1 : ASSERT_TRUE(checker.calculateRssStateNonIntersection(time_index++, constellation, rssState));
- - - - -
- - - +
- ]
87 [ + - + - : 1 : ASSERT_EQ(rssState.longitudinal_state,
- + - - -
- - - ]
88 [ + - ]: 1 : TestSupport::stateWithInformation(cTestSupport.cLongitudinalBrakeMin, constellation));
89 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_left, TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
- + - - -
- - - +
- ]
90 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_right, TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
- + - - -
- - - +
- ]
91 [ + - ]: 1 : }
92 : :
93 : 4 : TEST_F(RssLongitudinalConstellationCheckerTests, same_direction_leading_other_50kmh_maxspeed_safe)
94 : : {
95 : 1 : RssNonIntersectionConstellationChecker checker;
96 [ + - ]: 1 : leadingVehicle = createRelativeVehicleStateForLongitudinalMotion(50);
97 [ + - ]: 1 : followingVehicle = createRelativeVehicleStateForLongitudinalMotion(50);
98 : 1 : followingVehicle.dynamics.max_speed_on_acceleration = kmhToMeterPerSec(50.);
99 : 1 : followingVehicle.dynamics.alpha_lon.accel_max = Acceleration(2.);
100 : 1 : followingVehicle.dynamics.alpha_lon.brake_min = Acceleration(-4.);
101 : :
102 : 1 : constellation.ego_state = followingVehicle;
103 : 1 : constellation.other_state = leadingVehicle;
104 : : constellation.relative_position
105 [ + - ]: 1 : = createRelativeLongitudinalPosition(core::LongitudinalRelativePosition::AtBack, Distance(40.));
106 : :
107 [ + - - + : 1 : ASSERT_TRUE(checker.calculateRssStateNonIntersection(time_index++, constellation, rssState));
- - - - -
- - - +
- ]
108 [ + - + - : 1 : ASSERT_EQ(rssState.longitudinal_state,
- + - - -
- - - ]
109 [ + - ]: 1 : TestSupport::stateWithInformation(cTestSupport.cLongitudinalSafe, constellation));
110 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_left, TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
- + - - -
- - - +
- ]
111 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_right, TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
- + - - -
- - - +
- ]
112 [ + - ]: 1 : }
113 : :
114 : 4 : TEST_F(RssLongitudinalConstellationCheckerTests, same_direction_leading_other_50kmh_maxspeed_unsafe)
115 : : {
116 : 1 : RssNonIntersectionConstellationChecker checker;
117 [ + - ]: 1 : leadingVehicle = createRelativeVehicleStateForLongitudinalMotion(50);
118 [ + - ]: 1 : followingVehicle = createRelativeVehicleStateForLongitudinalMotion(50);
119 : 1 : followingVehicle.dynamics.max_speed_on_acceleration = kmhToMeterPerSec(50.);
120 : 1 : followingVehicle.dynamics.alpha_lon.accel_max = Acceleration(2.);
121 : 1 : followingVehicle.dynamics.alpha_lon.brake_min = Acceleration(-4.);
122 : :
123 : 1 : constellation.ego_state = followingVehicle;
124 : 1 : constellation.other_state = leadingVehicle;
125 : : constellation.relative_position
126 [ + - ]: 1 : = createRelativeLongitudinalPosition(core::LongitudinalRelativePosition::AtBack, Distance(38.));
127 : :
128 [ + - - + : 1 : ASSERT_TRUE(checker.calculateRssStateNonIntersection(time_index++, constellation, rssState));
- - - - -
- - - +
- ]
129 [ + - + - : 1 : ASSERT_EQ(rssState.longitudinal_state,
- + - - -
- - - ]
130 [ + - ]: 1 : TestSupport::stateWithInformation(cTestSupport.cLongitudinalBrakeMin, constellation));
131 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_left, TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
- + - - -
- - - +
- ]
132 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_right, TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
- + - - -
- - - +
- ]
133 [ + - ]: 1 : }
134 : :
135 : 4 : TEST_F(RssLongitudinalConstellationCheckerTests, same_direction_leading_other_40kmh_maxspeed_unsafe)
136 : : {
137 : 1 : RssNonIntersectionConstellationChecker checker;
138 [ + - ]: 1 : leadingVehicle = createRelativeVehicleStateForLongitudinalMotion(50);
139 [ + - ]: 1 : followingVehicle = createRelativeVehicleStateForLongitudinalMotion(50);
140 : 1 : followingVehicle.dynamics.max_speed_on_acceleration = kmhToMeterPerSec(40.);
141 : 1 : followingVehicle.dynamics.alpha_lon.accel_max = Acceleration(2.);
142 : 1 : followingVehicle.dynamics.alpha_lon.brake_min = Acceleration(-4.);
143 : :
144 : 1 : constellation.ego_state = followingVehicle;
145 : 1 : constellation.other_state = leadingVehicle;
146 : : constellation.relative_position
147 [ + - ]: 1 : = createRelativeLongitudinalPosition(core::LongitudinalRelativePosition::AtBack, Distance(38.));
148 : :
149 [ + - - + : 1 : ASSERT_TRUE(checker.calculateRssStateNonIntersection(time_index++, constellation, rssState));
- - - - -
- - - +
- ]
150 [ + - + - : 1 : ASSERT_EQ(rssState.longitudinal_state,
- + - - -
- - - ]
151 [ + - ]: 1 : TestSupport::stateWithInformation(cTestSupport.cLongitudinalBrakeMin, constellation));
152 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_left, TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
- + - - -
- - - +
- ]
153 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_right, TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
- + - - -
- - - +
- ]
154 [ + - ]: 1 : }
155 : :
156 : 4 : TEST_F(RssLongitudinalConstellationCheckerTests, same_direction_leading_other_50kmh_no_accel_unsafe)
157 : : {
158 : 1 : RssNonIntersectionConstellationChecker checker;
159 [ + - ]: 1 : leadingVehicle = createRelativeVehicleStateForLongitudinalMotion(50);
160 [ + - ]: 1 : followingVehicle = createRelativeVehicleStateForLongitudinalMotion(50);
161 : 1 : followingVehicle.dynamics.alpha_lon.accel_max = Acceleration(0.);
162 : 1 : followingVehicle.dynamics.alpha_lon.brake_min = Acceleration(-4.);
163 : :
164 : 1 : constellation.ego_state = followingVehicle;
165 : 1 : constellation.other_state = leadingVehicle;
166 : : constellation.relative_position
167 [ + - ]: 1 : = createRelativeLongitudinalPosition(core::LongitudinalRelativePosition::AtBack, Distance(39.));
168 : :
169 [ + - - + : 1 : ASSERT_TRUE(checker.calculateRssStateNonIntersection(time_index++, constellation, rssState));
- - - - -
- - - +
- ]
170 [ + - + - : 1 : ASSERT_EQ(rssState.longitudinal_state,
- + - - -
- - - ]
171 [ + - ]: 1 : TestSupport::stateWithInformation(cTestSupport.cLongitudinalBrakeMin, constellation));
172 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_left, TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
- + - - -
- - - +
- ]
173 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_right, TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
- + - - -
- - - +
- ]
174 [ + - ]: 1 : }
175 : :
176 : 4 : TEST_F(RssLongitudinalConstellationCheckerTests, same_direction_leading_other_50kmh_other_standing)
177 : : {
178 : 1 : RssNonIntersectionConstellationChecker checker;
179 [ + - ]: 1 : leadingVehicle = createRelativeVehicleStateForLongitudinalMotion(0);
180 [ + - ]: 1 : followingVehicle = createRelativeVehicleStateForLongitudinalMotion(50);
181 : 1 : followingVehicle.dynamics.alpha_lon.accel_max = Acceleration(2.);
182 : 1 : followingVehicle.dynamics.alpha_lon.brake_min = Acceleration(-4.);
183 : :
184 : 1 : constellation.ego_state = followingVehicle;
185 : 1 : constellation.other_state = leadingVehicle;
186 : : constellation.relative_position
187 [ + - ]: 1 : = createRelativeLongitudinalPosition(core::LongitudinalRelativePosition::AtBack, Distance(71.8));
188 : :
189 [ + - - + : 1 : ASSERT_TRUE(checker.calculateRssStateNonIntersection(time_index++, constellation, rssState));
- - - - -
- - - +
- ]
190 [ + - + - : 1 : ASSERT_EQ(rssState.longitudinal_state,
- + - - -
- - - ]
191 [ + - ]: 1 : TestSupport::stateWithInformation(cTestSupport.cLongitudinalSafe, constellation));
192 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_left, TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
- + - - -
- - - +
- ]
193 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_right, TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
- + - - -
- - - +
- ]
194 : :
195 : : constellation.relative_position
196 [ + - ]: 1 : = createRelativeLongitudinalPosition(core::LongitudinalRelativePosition::AtBack, Distance(71.6));
197 : :
198 [ + - - + : 1 : ASSERT_TRUE(checker.calculateRssStateNonIntersection(time_index++, constellation, rssState));
- - - - -
- - - +
- ]
199 [ + - + - : 1 : ASSERT_EQ(rssState.longitudinal_state,
- + - - -
- - - ]
200 [ + - ]: 1 : TestSupport::stateWithInformation(cTestSupport.cLongitudinalBrakeMin, constellation));
201 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_left, TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
- + - - -
- - - +
- ]
202 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_right, TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
- + - - -
- - - +
- ]
203 [ + - ]: 1 : }
204 : :
205 : 4 : TEST_F(RssLongitudinalConstellationCheckerTests, same_direction_leading_other_50kmh_maxspeed_other_standing)
206 : : {
207 : 1 : RssNonIntersectionConstellationChecker checker;
208 [ + - ]: 1 : leadingVehicle = createRelativeVehicleStateForLongitudinalMotion(0);
209 [ + - ]: 1 : followingVehicle = createRelativeVehicleStateForLongitudinalMotion(50);
210 : 1 : followingVehicle.dynamics.max_speed_on_acceleration = kmhToMeterPerSec(50.);
211 : 1 : followingVehicle.dynamics.alpha_lon.accel_max = Acceleration(2.);
212 : 1 : followingVehicle.dynamics.alpha_lon.brake_min = Acceleration(-4.);
213 : :
214 : 1 : constellation.ego_state = followingVehicle;
215 : 1 : constellation.other_state = leadingVehicle;
216 : : constellation.relative_position
217 [ + - ]: 1 : = createRelativeLongitudinalPosition(core::LongitudinalRelativePosition::AtBack, Distance(52.));
218 : :
219 [ + - - + : 1 : ASSERT_TRUE(checker.calculateRssStateNonIntersection(time_index++, constellation, rssState));
- - - - -
- - - +
- ]
220 [ + - + - : 1 : ASSERT_EQ(rssState.longitudinal_state,
- + - - -
- - - ]
221 [ + - ]: 1 : TestSupport::stateWithInformation(cTestSupport.cLongitudinalSafe, constellation));
222 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_left, TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
- + - - -
- - - +
- ]
223 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_right, TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
- + - - -
- - - +
- ]
224 : :
225 : : constellation.relative_position
226 [ + - ]: 1 : = createRelativeLongitudinalPosition(core::LongitudinalRelativePosition::AtBack, Distance(50.));
227 : :
228 [ + - - + : 1 : ASSERT_TRUE(checker.calculateRssStateNonIntersection(time_index++, constellation, rssState));
- - - - -
- - - +
- ]
229 [ + - + - : 1 : ASSERT_EQ(rssState.longitudinal_state,
- + - - -
- - - ]
230 [ + - ]: 1 : TestSupport::stateWithInformation(cTestSupport.cLongitudinalBrakeMin, constellation));
231 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_left, TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
- + - - -
- - - +
- ]
232 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_right, TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
- + - - -
- - - +
- ]
233 [ + - ]: 1 : }
234 : :
235 : 4 : TEST_F(RssLongitudinalConstellationCheckerTests, same_direction_leading_other_0kmh_other_standing)
236 : : {
237 : 1 : RssNonIntersectionConstellationChecker checker;
238 [ + - ]: 1 : leadingVehicle = createRelativeVehicleStateForLongitudinalMotion(0);
239 [ + - ]: 1 : followingVehicle = createRelativeVehicleStateForLongitudinalMotion(0);
240 : 1 : followingVehicle.dynamics.alpha_lon.accel_max = Acceleration(2.);
241 : 1 : followingVehicle.dynamics.alpha_lon.brake_min = Acceleration(-4.);
242 : :
243 : 1 : constellation.ego_state = followingVehicle;
244 : 1 : constellation.other_state = leadingVehicle;
245 : : constellation.relative_position
246 [ + - ]: 1 : = createRelativeLongitudinalPosition(core::LongitudinalRelativePosition::AtBack, Distance(6.1));
247 : :
248 [ + - - + : 1 : ASSERT_TRUE(checker.calculateRssStateNonIntersection(time_index++, constellation, rssState));
- - - - -
- - - +
- ]
249 [ + - + - : 1 : ASSERT_EQ(rssState.longitudinal_state,
- + - - -
- - - ]
250 [ + - ]: 1 : TestSupport::stateWithInformation(cTestSupport.cLongitudinalSafe, constellation));
251 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_left, TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
- + - - -
- - - +
- ]
252 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_right, TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
- + - - -
- - - +
- ]
253 : :
254 : : constellation.relative_position
255 [ + - ]: 1 : = createRelativeLongitudinalPosition(core::LongitudinalRelativePosition::AtBack, Distance(6.));
256 : :
257 [ + - - + : 1 : ASSERT_TRUE(checker.calculateRssStateNonIntersection(time_index++, constellation, rssState));
- - - - -
- - - +
- ]
258 [ + - + - : 1 : ASSERT_EQ(rssState.longitudinal_state,
- + - - -
- - - ]
259 [ + - ]: 1 : TestSupport::stateWithInformation(cTestSupport.cLongitudinalBrakeMin, constellation));
260 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_left, TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
- + - - -
- - - +
- ]
261 [ + - + - : 1 : ASSERT_EQ(rssState.lateral_state_right, TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
- + - - -
- - - +
- ]
262 [ + - ]: 1 : }
263 : :
264 : 4 : TEST_F(RssLongitudinalConstellationCheckerTests, same_direction_both_negative_velocity)
265 : : {
266 : 1 : RssNonIntersectionConstellationChecker checker;
267 [ + - ]: 1 : leadingVehicle = createRelativeVehicleStateForLongitudinalMotion(-50);
268 [ + - ]: 1 : followingVehicle = createRelativeVehicleStateForLongitudinalMotion(-50);
269 : :
270 : 1 : constellation.ego_state = followingVehicle;
271 : 1 : constellation.other_state = leadingVehicle;
272 : : constellation.relative_position
273 [ + - ]: 1 : = createRelativeLongitudinalPosition(core::LongitudinalRelativePosition::AtBack, Distance(71.6));
274 : :
275 [ + - - + : 1 : ASSERT_FALSE(checker.calculateRssStateNonIntersection(time_index++, constellation, rssState));
- - - - -
- - - +
- ]
276 [ + - ]: 1 : }
277 : :
278 : : } // namespace structured
279 : : } // namespace rss
280 : : } // namespace ad
|
