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/RssIntersectionConstellationChecker.hpp"
11 : :
12 : : namespace ad {
13 : : namespace rss {
14 : : namespace structured {
15 : :
16 : : class RssIntersectionNoPriorityConstellationCheckerTests : public testing::Test
17 : : {
18 : : protected:
19 : : core::RelativeObjectState leadingVehicle;
20 : : core::RelativeObjectState followingVehicle;
21 : : core::RelativeConstellation constellation;
22 : : state::RssState rssState;
23 : : world::TimeIndex time_index{1u};
24 : : };
25 : :
26 : 4 : TEST_F(RssIntersectionNoPriorityConstellationCheckerTests, ego_following_no_overlap)
27 : : {
28 : 3 : for (auto constellation_type :
29 [ + + ]: 4 : {world::ConstellationType::IntersectionSamePriority, world::ConstellationType::IntersectionObjectHasPriority})
30 : : {
31 : 2 : RssIntersectionConstellationChecker checker;
32 [ + - ]: 2 : constellation.ego_state = createRelativeVehicleStateForLongitudinalMotion(120);
33 : 2 : constellation.ego_state.structured_object_state.distance_to_enter_intersection = Distance(15);
34 : 2 : constellation.ego_state.structured_object_state.distance_to_leave_intersection = Distance(15);
35 : :
36 : 2 : constellation.ego_state.dynamics.alpha_lon.accel_max = Acceleration(2.);
37 : 2 : constellation.ego_state.dynamics.alpha_lon.brake_min = Acceleration(-4.);
38 : :
39 [ + - ]: 2 : constellation.other_state = createRelativeVehicleStateForLongitudinalMotion(10);
40 : 2 : constellation.other_state.dynamics.alpha_lon.accel_max = Acceleration(2.);
41 : 2 : constellation.other_state.dynamics.alpha_lon.brake_min = Acceleration(-4.);
42 : 2 : constellation.other_state.structured_object_state.distance_to_enter_intersection = Distance(16);
43 : 2 : constellation.other_state.structured_object_state.distance_to_leave_intersection = Distance(16);
44 : :
45 : : constellation.relative_position
46 [ + - ]: 2 : = createRelativeLongitudinalPosition(core::LongitudinalRelativePosition::AtBack, Distance(1.));
47 : 2 : constellation.constellation_type = constellation_type;
48 [ + + ]: 2 : if (constellation_type == world::ConstellationType::IntersectionObjectHasPriority)
49 : : {
50 : 1 : constellation.other_state.structured_object_state.has_priority = true;
51 : : }
52 : : else
53 : : {
54 : 1 : constellation.other_state.structured_object_state.has_priority = false;
55 : : }
56 : :
57 [ + - - + : 2 : EXPECT_TRUE(checker.calculateRssStateIntersection(time_index++, constellation, rssState));
- - - - -
- - - ]
58 [ + - + - : 2 : EXPECT_EQ(rssState.longitudinal_state,
- + - - -
- - - ]
59 : 2 : TestSupport::stateWithInformation(cTestSupport.cLongitudinalSafe, constellation));
60 [ + - + - : 2 : EXPECT_EQ(rssState.lateral_state_left, TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
- + - - -
- - - ]
61 [ + - + - : 2 : EXPECT_EQ(rssState.lateral_state_right,
- + - - -
- - - ]
62 : 2 : TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
63 : :
64 : : // next constellation we have overlap
65 : 2 : constellation.ego_state.structured_object_state.velocity.speed_lon_min = Speed(10);
66 : 2 : constellation.ego_state.structured_object_state.velocity.speed_lon_max = Speed(10);
67 : :
68 [ + - - + : 2 : EXPECT_TRUE(checker.calculateRssStateIntersection(time_index++, constellation, rssState));
- - - - -
- - - ]
69 [ + + ]: 2 : if (constellation_type == world::ConstellationType::IntersectionObjectHasPriority)
70 : : {
71 [ + - + - : 1 : EXPECT_EQ(rssState.longitudinal_state,
- + - - -
- - - ]
72 : 1 : TestSupport::stateWithInformation(cTestSupport.cLongitudinalBrakeMin, constellation));
73 : : }
74 : : else
75 : : {
76 [ + - + - : 1 : EXPECT_EQ(rssState.longitudinal_state,
- + - - -
- - - ]
77 : 1 : TestSupport::stateWithInformation(cTestSupport.cLongitudinalSafe, constellation));
78 : : }
79 [ + - + - : 2 : EXPECT_EQ(rssState.lateral_state_left, TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
- + - - -
- - - ]
80 [ + - + - : 2 : EXPECT_EQ(rssState.lateral_state_right,
- + - - -
- - - ]
81 : 2 : TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
82 : 2 : }
83 : 1 : }
84 : :
85 : 4 : TEST_F(RssIntersectionNoPriorityConstellationCheckerTests, 50kmh_safe_distance_ego_following)
86 : : {
87 : 2 : for (auto constellation_type :
88 [ + + ]: 4 : {world::ConstellationType::IntersectionSamePriority, world::ConstellationType::IntersectionObjectHasPriority})
89 : : {
90 : 2 : RssIntersectionConstellationChecker checker;
91 [ + - ]: 2 : leadingVehicle = createRelativeVehicleStateForLongitudinalMotion(120);
92 : 2 : leadingVehicle.structured_object_state.distance_to_enter_intersection = Distance(2);
93 : 2 : leadingVehicle.structured_object_state.distance_to_leave_intersection = Distance(2);
94 : 2 : leadingVehicle.dynamics.alpha_lon.accel_max = Acceleration(2.);
95 : 2 : leadingVehicle.dynamics.alpha_lon.brake_min = Acceleration(-4.);
96 [ + + ]: 2 : if (constellation_type == world::ConstellationType::IntersectionObjectHasPriority)
97 : : {
98 : 1 : leadingVehicle.structured_object_state.has_priority = true;
99 : : }
100 : : else
101 : : {
102 : 1 : leadingVehicle.structured_object_state.has_priority = false;
103 : : }
104 : :
105 [ + - ]: 2 : followingVehicle = createRelativeVehicleStateForLongitudinalMotion(30);
106 : 2 : followingVehicle.dynamics.alpha_lon.accel_max = Acceleration(2.);
107 : 2 : followingVehicle.dynamics.alpha_lon.brake_min = Acceleration(-4.);
108 : 2 : followingVehicle.structured_object_state.distance_to_enter_intersection = Distance(12.);
109 : 2 : followingVehicle.structured_object_state.distance_to_leave_intersection = Distance(12.);
110 : :
111 : 2 : constellation.ego_state = followingVehicle;
112 : 2 : constellation.other_state = leadingVehicle;
113 : : constellation.relative_position
114 [ + - ]: 2 : = createRelativeLongitudinalPosition(core::LongitudinalRelativePosition::AtBack, Distance(10.));
115 : 2 : constellation.constellation_type = constellation_type;
116 [ + + ]: 2 : if (constellation_type == world::ConstellationType::IntersectionObjectHasPriority)
117 : : {
118 : 1 : constellation.other_state.structured_object_state.has_priority = true;
119 : : }
120 : : else
121 : : {
122 : 1 : constellation.other_state.structured_object_state.has_priority = false;
123 : : }
124 : :
125 [ + - - + : 2 : ASSERT_TRUE(checker.calculateRssStateIntersection(time_index++, constellation, rssState));
- - - - -
- - - +
- ]
126 [ + - + - : 2 : EXPECT_EQ(rssState.longitudinal_state,
- + - - -
- - - ]
127 : 2 : TestSupport::stateWithInformation(cTestSupport.cLongitudinalSafe, constellation));
128 [ + - + - : 2 : EXPECT_EQ(rssState.lateral_state_left, TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
- + - - -
- - - ]
129 [ + - + - : 2 : EXPECT_EQ(rssState.lateral_state_right,
- + - - -
- - - ]
130 : 2 : TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
131 [ + - ]: 2 : }
132 : : }
133 : :
134 : 4 : TEST_F(RssIntersectionNoPriorityConstellationCheckerTests, 50kmh_safe_distance_ego_leading)
135 : : {
136 : 2 : for (auto constellation_type :
137 [ + + ]: 4 : {world::ConstellationType::IntersectionSamePriority, world::ConstellationType::IntersectionObjectHasPriority})
138 : : {
139 : 2 : RssIntersectionConstellationChecker checker;
140 [ + - ]: 2 : leadingVehicle = createRelativeVehicleStateForLongitudinalMotion(50);
141 : 2 : leadingVehicle.structured_object_state.distance_to_enter_intersection = Distance(10.);
142 : 2 : leadingVehicle.structured_object_state.distance_to_leave_intersection = Distance(10.);
143 [ + - ]: 2 : followingVehicle = createRelativeVehicleStateForLongitudinalMotion(50);
144 : 2 : followingVehicle.dynamics.alpha_lon.accel_max = Acceleration(2.);
145 : 2 : followingVehicle.dynamics.alpha_lon.brake_min = Acceleration(-4.);
146 : 2 : followingVehicle.structured_object_state.distance_to_enter_intersection = Distance(70.);
147 : 2 : followingVehicle.structured_object_state.distance_to_leave_intersection = Distance(70.);
148 : :
149 : 2 : constellation.ego_state = leadingVehicle;
150 : 2 : constellation.other_state = followingVehicle;
151 : : constellation.relative_position
152 [ + - ]: 2 : = createRelativeLongitudinalPosition(core::LongitudinalRelativePosition::InFront, Distance(60.));
153 : 2 : constellation.constellation_type = constellation_type;
154 [ + + ]: 2 : if (constellation_type == world::ConstellationType::IntersectionObjectHasPriority)
155 : : {
156 : 1 : constellation.other_state.structured_object_state.has_priority = true;
157 : : }
158 : : else
159 : : {
160 : 1 : constellation.other_state.structured_object_state.has_priority = false;
161 : : }
162 : :
163 [ + - - + : 2 : ASSERT_TRUE(checker.calculateRssStateIntersection(time_index++, constellation, rssState));
- - - - -
- - - +
- ]
164 [ - + - - : 2 : ASSERT_TRUE(rssState.longitudinal_state.is_safe);
- - - - -
- + - ]
165 [ + - + - : 2 : EXPECT_EQ(rssState.longitudinal_state,
- + - - -
- - - ]
166 : 2 : TestSupport::stateWithInformation(cTestSupport.cLongitudinalSafe, constellation));
167 [ + - + - : 2 : EXPECT_EQ(rssState.lateral_state_left, TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
- + - - -
- - - ]
168 [ + - + - : 2 : EXPECT_EQ(rssState.lateral_state_right,
- + - - -
- - - ]
169 : 2 : TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
170 [ + - ]: 2 : }
171 : : }
172 : :
173 : 4 : TEST_F(RssIntersectionNoPriorityConstellationCheckerTests, 50km_h_stop_before_intersection)
174 : : {
175 [ + + ]: 3 : for (auto initiallySafe : {true, false})
176 : : {
177 : 4 : for (auto constellation_type :
178 [ + + ]: 8 : {world::ConstellationType::IntersectionSamePriority, world::ConstellationType::IntersectionObjectHasPriority})
179 : : {
180 : 4 : RssIntersectionConstellationChecker checker;
181 [ + - ]: 4 : leadingVehicle = createRelativeVehicleStateForLongitudinalMotion(50);
182 : 4 : leadingVehicle.structured_object_state.distance_to_enter_intersection = Distance(80.);
183 : 4 : leadingVehicle.structured_object_state.distance_to_leave_intersection = Distance(80.);
184 : 4 : leadingVehicle.dynamics.alpha_lon.accel_max = Acceleration(2.);
185 : 4 : leadingVehicle.dynamics.alpha_lon.brake_min = Acceleration(-4.);
186 [ + - ]: 4 : followingVehicle = createRelativeVehicleStateForLongitudinalMotion(50);
187 : 4 : followingVehicle.dynamics.alpha_lon.accel_max = Acceleration(2.);
188 : 4 : followingVehicle.dynamics.alpha_lon.brake_min = Acceleration(-4.);
189 : 4 : followingVehicle.structured_object_state.distance_to_enter_intersection = Distance(110.);
190 : 4 : followingVehicle.structured_object_state.distance_to_leave_intersection = Distance(110.);
191 : :
192 : 4 : constellation.ego_state = leadingVehicle;
193 : 4 : constellation.other_state = followingVehicle;
194 : : constellation.relative_position
195 [ + - ]: 4 : = createRelativeLongitudinalPosition(core::LongitudinalRelativePosition::InFront, Distance(30.));
196 : 4 : constellation.constellation_type = constellation_type;
197 [ + + ]: 4 : if (constellation_type == world::ConstellationType::IntersectionObjectHasPriority)
198 : : {
199 : 2 : constellation.other_state.structured_object_state.has_priority = true;
200 : : }
201 : : else
202 : : {
203 : 2 : constellation.other_state.structured_object_state.has_priority = false;
204 : : }
205 : :
206 [ + + ]: 4 : if (initiallySafe)
207 : : {
208 : : // both vehicles can stop safely
209 [ + - - + : 2 : ASSERT_TRUE(checker.calculateRssStateIntersection(time_index++, constellation, rssState));
- - - - -
- - - +
- ]
210 [ - + - - : 2 : ASSERT_TRUE(rssState.longitudinal_state.is_safe);
- - - - -
- + - ]
211 [ + - + - : 2 : ASSERT_EQ(rssState.longitudinal_state,
- + - - -
- - - ]
212 [ + - ]: 2 : TestSupport::stateWithInformation(cTestSupport.cLongitudinalSafe, constellation));
213 [ + - + - : 2 : EXPECT_EQ(rssState.lateral_state_left,
- + - - -
- - - ]
214 : 2 : TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
215 [ + - + - : 2 : EXPECT_EQ(rssState.lateral_state_right,
- + - - -
- - - ]
216 : 2 : TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
217 : : }
218 : :
219 : : // ego vehicle cannot stop safely anymore
220 : : // but other vehicle still
221 : 4 : constellation.ego_state.structured_object_state.distance_to_enter_intersection = Distance(70);
222 : 4 : constellation.ego_state.structured_object_state.distance_to_leave_intersection = Distance(70);
223 : 4 : constellation.other_state.structured_object_state.distance_to_enter_intersection = Distance(100);
224 : 4 : constellation.other_state.structured_object_state.distance_to_leave_intersection = Distance(100);
225 : :
226 [ + - - + : 4 : ASSERT_TRUE(checker.calculateRssStateIntersection(time_index++, constellation, rssState));
- - - - -
- - - +
- ]
227 [ + + ]: 4 : if (constellation.other_state.structured_object_state.has_priority)
228 : : {
229 [ - + - - : 2 : ASSERT_FALSE(rssState.longitudinal_state.is_safe);
- - - - -
- + - ]
230 [ + - + - : 2 : ASSERT_EQ(rssState.longitudinal_state,
- + - - -
- - - ]
231 [ + - ]: 2 : TestSupport::stateWithInformation(cTestSupport.cLongitudinalBrakeMin, constellation));
232 : : }
233 : : else
234 : : {
235 [ - + - - : 2 : ASSERT_TRUE(rssState.longitudinal_state.is_safe);
- - - - -
- + - ]
236 [ + - + - : 2 : ASSERT_EQ(rssState.longitudinal_state,
- + - - -
- - - ]
237 [ + - ]: 2 : TestSupport::stateWithInformation(cTestSupport.cLongitudinalSafe, constellation));
238 : : }
239 [ + - + - : 4 : EXPECT_EQ(rssState.lateral_state_left,
- + - - -
- - - ]
240 : 4 : TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
241 [ + - + - : 4 : EXPECT_EQ(rssState.lateral_state_right,
- + - - -
- - - ]
242 : 4 : TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
243 : :
244 : : // both cannot stop safely anymore
245 : 4 : constellation.ego_state.structured_object_state.distance_to_enter_intersection = Distance(70.);
246 : 4 : constellation.ego_state.structured_object_state.distance_to_leave_intersection = Distance(70.);
247 : 4 : constellation.other_state.structured_object_state.distance_to_enter_intersection = Distance(70.);
248 : 4 : constellation.other_state.structured_object_state.distance_to_leave_intersection = Distance(70.);
249 : :
250 [ + - - + : 4 : ASSERT_TRUE(checker.calculateRssStateIntersection(time_index++, constellation, rssState));
- - - - -
- - - +
- ]
251 [ - + - - : 4 : ASSERT_FALSE(rssState.longitudinal_state.is_safe);
- - - - -
- + - ]
252 [ + - + - : 4 : ASSERT_EQ(rssState.longitudinal_state,
- + - - -
- - - ]
253 [ + - ]: 4 : TestSupport::stateWithInformation(cTestSupport.cLongitudinalBrakeMin, constellation));
254 [ + - + - : 4 : EXPECT_EQ(rssState.lateral_state_left,
- + - - -
- - - ]
255 : 4 : TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
256 [ + - + - : 4 : EXPECT_EQ(rssState.lateral_state_right,
- + - - -
- - - ]
257 : 4 : TestSupport::stateWithInformation(cTestSupport.cLateralNone, constellation));
258 [ + - ]: 4 : }
259 : : }
260 : : }
261 : :
262 : : } // namespace structured
263 : : } // namespace rss
264 : : } // namespace ad
|
