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