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 "situation/RssUnstructuredSceneChecker.hpp"
11 : :
12 : : namespace ad {
13 : : namespace rss {
14 : : namespace situation {
15 : :
16 : 2 : TEST(RssSituationCheckingTestsUnstructuredSceneChecker, calculateDriveAwayAngle)
17 : : {
18 : 1 : ::ad::rss::state::HeadingRange range;
19 : 1 : RssUnstructuredSceneChecker unstructuredSceneChecker;
20 [ + - + - : 1 : ASSERT_TRUE(unstructuredSceneChecker.calculateDriveAwayAngle(
+ - + - -
+ - - - -
- - - - ]
21 : : unstructured::Point(0, 0), unstructured::Point(1, 0), physics::cPI / 4., range));
22 [ + - + - : 1 : ASSERT_EQ(range.begin, 3. / 4. * physics::cPI);
- + - - -
- - - ]
23 [ + - + - : 1 : ASSERT_EQ(range.end, -3. / 4. * physics::cPI);
- + - - -
- - - ]
24 : :
25 [ + - + - : 1 : ASSERT_TRUE(unstructuredSceneChecker.calculateDriveAwayAngle(
+ - + - -
+ - - - -
- - - - ]
26 : : unstructured::Point(0, 0), unstructured::Point(0, 1), physics::cPI / 4., range));
27 [ + - + - : 1 : ASSERT_EQ(range.begin, -3. / 4. * physics::cPI);
- + - - -
- - - ]
28 [ + - + - : 1 : ASSERT_EQ(range.end, -1. / 4. * physics::cPI);
- + - - -
- - - ]
29 : :
30 [ + - + - : 1 : ASSERT_TRUE(unstructuredSceneChecker.calculateDriveAwayAngle(
+ - + - -
+ - - - -
- - - - ]
31 : : unstructured::Point(0, 0), unstructured::Point(-1, 0), physics::cPI / 4., range));
32 [ + - + - : 1 : ASSERT_EQ(range.begin, -1. / 4. * physics::cPI);
- + - - -
- - - ]
33 [ + - + - : 1 : ASSERT_EQ(range.end, 1. / 4. * physics::cPI);
- + - - -
- - - ]
34 : :
35 [ + - + - : 1 : ASSERT_TRUE(unstructuredSceneChecker.calculateDriveAwayAngle(
+ - + - -
+ - - - -
- - - - ]
36 : : unstructured::Point(0, 0), unstructured::Point(0, -1), physics::cPI / 4., range));
37 [ + - + - : 1 : ASSERT_EQ(range.begin, 1. / 4. * physics::cPI);
- + - - -
- - - ]
38 [ + - + - : 1 : ASSERT_EQ(range.end, 3. / 4. * physics::cPI);
- + - - -
- - - ]
39 : :
40 [ + - + - : 1 : ASSERT_TRUE(unstructuredSceneChecker.calculateDriveAwayAngle(
+ - + - -
+ - - - -
- - - - ]
41 : : unstructured::Point(0, 0), unstructured::Point(1, 1), physics::cPI / 4., range));
42 [ + - - + : 1 : ASSERT_EQ(range.begin, physics::cPI);
- - - - -
- ]
43 [ + - + - : 1 : ASSERT_EQ(range.end, -physics::cPI_2);
- + - - -
- - - ]
44 : :
45 [ + - + - : 1 : ASSERT_TRUE(unstructuredSceneChecker.calculateDriveAwayAngle(
+ - + - -
+ - - - -
- - - - ]
46 : : unstructured::Point(0, 0), unstructured::Point(0, 1), 3. / 4. * physics::cPI, range));
47 [ + - + - : 1 : ASSERT_EQ(range.begin, 3. / 4. * physics::cPI);
- + - - -
- - - ]
48 [ + - + - : 1 : ASSERT_EQ(range.end, 1. / 4. * physics::cPI);
- + - - -
- - - ]
49 : :
50 [ + - + - : 1 : ASSERT_TRUE(unstructuredSceneChecker.calculateDriveAwayAngle(
+ - + - -
+ - - - -
- - - - ]
51 : : unstructured::Point(0, 0), unstructured::Point(0, 1), physics::cPI / 2., range));
52 [ + - - + : 1 : ASSERT_EQ(range.begin, physics::cPI);
- - - - -
- ]
53 [ + - - + : 1 : ASSERT_EQ(range.end, physics::Angle(0.0));
- - - - -
- ]
54 : :
55 [ + - + - : 1 : ASSERT_TRUE(unstructuredSceneChecker.calculateDriveAwayAngle(
+ - + - -
+ - - - -
- - - - ]
56 : : unstructured::Point(0, 0), unstructured::Point(0, -1), physics::cPI / 2., range));
57 [ + - - + : 1 : ASSERT_EQ(range.begin, physics::Angle(0.0));
- - - - -
- ]
58 [ + - - + : 1 : ASSERT_EQ(range.end, physics::cPI);
- - - - -
- ]
59 : : }
60 : :
61 : 2 : TEST(RssSituationCheckingTestsUnstructuredSceneChecker, calculateState_emptyTrajectorySets)
62 : : {
63 : 1 : RssUnstructuredSceneChecker unstructuredSceneChecker;
64 : 1 : state::UnstructuredSceneRssState unstructuredSceneState;
65 : 1 : Situation situation;
66 : 1 : situation.situationType = SituationType::Unstructured;
67 [ + - ]: 1 : situation.egoVehicleState = createVehicleState(world::ObjectType::EgoVehicle, 0.0, 0.0);
68 [ + - ]: 1 : situation.otherVehicleState = createVehicleState(world::ObjectType::OtherVehicle, 0.0, 0.0);
69 : 1 : state::UnstructuredSceneStateInformation egoStateInfo;
70 : 1 : state::UnstructuredSceneStateInformation otherStateInfo;
71 [ + - - + : 1 : ASSERT_FALSE(
- - - - -
- - - ]
72 : : unstructuredSceneChecker.calculateState(situation, egoStateInfo, otherStateInfo, unstructuredSceneState));
73 : : }
74 : :
75 : 2 : TEST(RssSituationCheckingTestsUnstructuredSceneChecker, calculateState_other_in_front_of_ego_stopped)
76 : : {
77 : 1 : RssUnstructuredSceneChecker unstructuredSceneChecker;
78 : 1 : state::UnstructuredSceneRssState unstructuredSceneState;
79 : 1 : Situation situation;
80 : 1 : situation.situationType = SituationType::Unstructured;
81 [ + - ]: 1 : situation.egoVehicleState = createVehicleState(world::ObjectType::EgoVehicle, 0.0, 0.0);
82 [ + - ]: 1 : situation.otherVehicleState = createVehicleState(world::ObjectType::OtherVehicle, 0.0, 0.0);
83 [ + - ]: 1 : situation.egoVehicleState.dynamics.unstructuredSettings.driveAwayMaxAngle = physics::cPI / 4.;
84 : 1 : state::UnstructuredSceneStateInformation egoStateInfo;
85 : 1 : state::UnstructuredSceneStateInformation otherStateInfo;
86 [ + - + - ]: 1 : getUnstructuredVehicle(unstructured::Point(0.5, 0.5), true, egoStateInfo, situation.egoVehicleState);
87 [ + - + - ]: 1 : getUnstructuredVehicle(unstructured::Point(0.5, 4.5), true, otherStateInfo, situation.otherVehicleState);
88 [ + - - + : 1 : ASSERT_TRUE(unstructuredSceneChecker.calculateState(situation, egoStateInfo, otherStateInfo, unstructuredSceneState));
- - - - -
- - - ]
89 [ - + - - : 1 : ASSERT_TRUE(unstructuredSceneState.isSafe);
- - - - -
- ]
90 [ + - - + : 1 : ASSERT_EQ(unstructuredSceneState.response, state::UnstructuredSceneResponse::ContinueForward);
- - - - -
- ]
91 : :
92 : : // Drive away
93 [ + - + - ]: 1 : getUnstructuredVehicle(unstructured::Point(0.5, 0.5), true, egoStateInfo, situation.egoVehicleState);
94 [ + - + - ]: 1 : getUnstructuredVehicle(unstructured::Point(0.5, 2.0), true, otherStateInfo, situation.otherVehicleState);
95 [ + - - + : 1 : ASSERT_TRUE(unstructuredSceneChecker.calculateState(situation, egoStateInfo, otherStateInfo, unstructuredSceneState));
- - - - -
- - - ]
96 [ - + - - : 1 : ASSERT_FALSE(unstructuredSceneState.isSafe);
- - - - -
- ]
97 [ + - - + : 1 : ASSERT_EQ(unstructuredSceneState.response, state::UnstructuredSceneResponse::DriveAway); // as both stopped
- - - - -
- ]
98 [ + - + - : 1 : ASSERT_EQ(unstructuredSceneState.headingRange.begin, -3. / 4. * physics::cPI);
- + - - -
- - - ]
99 [ + - + - : 1 : ASSERT_EQ(unstructuredSceneState.headingRange.end, -1. / 4. * physics::cPI);
- + - - -
- - - ]
100 : : }
101 : :
102 : 2 : TEST(RssSituationCheckingTestsUnstructuredSceneChecker, calculateState_frontal_ego_approaching)
103 : : {
104 : 1 : RssUnstructuredSceneChecker unstructuredSceneChecker;
105 : 1 : state::UnstructuredSceneRssState unstructuredSceneState;
106 : 1 : Situation situation;
107 : 1 : situation.situationType = SituationType::Unstructured;
108 [ + - ]: 1 : situation.egoVehicleState = createVehicleState(world::ObjectType::EgoVehicle, 1.0, 0.0);
109 [ + - ]: 1 : situation.otherVehicleState = createVehicleState(world::ObjectType::OtherVehicle, 0.0, 0.0);
110 [ + - ]: 1 : situation.egoVehicleState.dynamics.unstructuredSettings.driveAwayMaxAngle = physics::cPI / 4.;
111 : 1 : state::UnstructuredSceneStateInformation egoStateInfo;
112 : 1 : state::UnstructuredSceneStateInformation otherStateInfo;
113 [ + - + - ]: 1 : getUnstructuredVehicle(unstructured::Point(0.5, 0.5), true, egoStateInfo, situation.egoVehicleState);
114 [ + - + - ]: 1 : getUnstructuredVehicle(unstructured::Point(0.5, 4.5), false, otherStateInfo, situation.otherVehicleState);
115 [ + - - + : 1 : ASSERT_TRUE(unstructuredSceneChecker.calculateState(situation, egoStateInfo, otherStateInfo, unstructuredSceneState));
- - - - -
- - - ]
116 [ - + - - : 1 : ASSERT_TRUE(unstructuredSceneState.isSafe);
- - - - -
- ]
117 [ + - - + : 1 : ASSERT_EQ(unstructuredSceneState.response, state::UnstructuredSceneResponse::ContinueForward);
- - - - -
- ]
118 : :
119 : : // brake
120 [ + - + - ]: 1 : getUnstructuredVehicle(unstructured::Point(0.5, 0.5), true, egoStateInfo, situation.egoVehicleState);
121 [ + - + - ]: 1 : getUnstructuredVehicle(unstructured::Point(0.5, 1.5), false, otherStateInfo, situation.otherVehicleState);
122 [ + - - + : 1 : ASSERT_TRUE(unstructuredSceneChecker.calculateState(situation, egoStateInfo, otherStateInfo, unstructuredSceneState));
- - - - -
- - - ]
123 [ - + - - : 1 : ASSERT_FALSE(unstructuredSceneState.isSafe);
- - - - -
- ]
124 [ + - - + : 1 : ASSERT_EQ(unstructuredSceneState.response, state::UnstructuredSceneResponse::Brake);
- - - - -
- ]
125 : : }
126 : :
127 : 2 : TEST(RssSituationCheckingTestsUnstructuredSceneChecker, calculateState_ego_in_front_driving_continueforward)
128 : : {
129 : 1 : RssUnstructuredSceneChecker unstructuredSceneChecker;
130 : 1 : Situation situation;
131 : 1 : situation.situationType = SituationType::Unstructured;
132 [ + - ]: 1 : situation.egoVehicleState = createVehicleState(world::ObjectType::EgoVehicle, 1.0, 0.0);
133 : 1 : situation.egoVehicleState.objectState.centerPoint.x = physics::Distance(20.0);
134 [ + - ]: 1 : situation.otherVehicleState = createVehicleState(world::ObjectType::OtherVehicle, 1.0, 0.0);
135 : 1 : situation.otherVehicleState.objectState.centerPoint.x = physics::Distance(0.0);
136 : 1 : state::UnstructuredSceneStateInformation egoStateInfo;
137 : :
138 : 1 : state::RssState rssState;
139 [ + - - + : 1 : ASSERT_TRUE(
- - - - -
- - - ]
140 : : unstructuredSceneChecker.calculateRssStateUnstructured(world::TimeIndex(1.), situation, egoStateInfo, rssState));
141 [ - + - - : 1 : ASSERT_TRUE(rssState.unstructuredSceneState.isSafe);
- - - - -
- ]
142 [ + - - + : 1 : ASSERT_EQ(rssState.unstructuredSceneState.response, state::UnstructuredSceneResponse::ContinueForward);
- - - - -
- ]
143 : :
144 : : // other must brake, ego continue forward
145 : 1 : situation.otherVehicleState.objectState.centerPoint.x = physics::Distance(5.0);
146 [ + - - + : 1 : ASSERT_TRUE(
- - - - -
- - - ]
147 : : unstructuredSceneChecker.calculateRssStateUnstructured(world::TimeIndex(2.), situation, egoStateInfo, rssState));
148 [ - + - - : 1 : ASSERT_TRUE(rssState.unstructuredSceneState.isSafe);
- - - - -
- ]
149 [ + - - + : 1 : ASSERT_EQ(rssState.unstructuredSceneState.response, state::UnstructuredSceneResponse::ContinueForward);
- - - - -
- ]
150 : : }
151 : :
152 : 2 : TEST(RssSituationCheckingTestsUnstructuredSceneChecker, calculateState_ego_in_front_stopped_drive_away)
153 : : {
154 : 1 : RssUnstructuredSceneChecker unstructuredSceneChecker;
155 : 1 : Situation situation;
156 : 1 : situation.situationType = SituationType::Unstructured;
157 [ + - ]: 1 : situation.egoVehicleState = createVehicleState(world::ObjectType::EgoVehicle, 1.0, 0.0);
158 : 1 : situation.egoVehicleState.objectState.centerPoint.x = physics::Distance(20.0);
159 [ + - + - ]: 1 : situation.egoVehicleState.dynamics.unstructuredSettings.driveAwayMaxAngle = 3. * physics::cPI / 4.;
160 [ + - ]: 1 : situation.otherVehicleState = createVehicleState(world::ObjectType::OtherVehicle, 1.0, 0.0);
161 : 1 : situation.otherVehicleState.objectState.centerPoint.x = physics::Distance(0.0);
162 : 1 : state::UnstructuredSceneStateInformation egoStateInfo;
163 : :
164 : 1 : state::RssState rssState;
165 [ + - - + : 1 : ASSERT_TRUE(
- - - - -
- - - ]
166 : : unstructuredSceneChecker.calculateRssStateUnstructured(world::TimeIndex(1.), situation, egoStateInfo, rssState));
167 [ - + - - : 1 : ASSERT_TRUE(rssState.unstructuredSceneState.isSafe);
- - - - -
- ]
168 [ + - - + : 1 : ASSERT_EQ(rssState.unstructuredSceneState.response, state::UnstructuredSceneResponse::ContinueForward);
- - - - -
- ]
169 : :
170 : : // other must brake, ego drive away
171 : 1 : situation.otherVehicleState.objectState.centerPoint.x = physics::Distance(5.0);
172 : 1 : situation.egoVehicleState.objectState.speed = physics::Speed(0.0);
173 [ + - - + : 1 : ASSERT_TRUE(
- - - - -
- - - ]
174 : : unstructuredSceneChecker.calculateRssStateUnstructured(world::TimeIndex(2.), situation, egoStateInfo, rssState));
175 [ - + - - : 1 : ASSERT_FALSE(rssState.unstructuredSceneState.isSafe);
- - - - -
- ]
176 [ + - - + : 1 : ASSERT_EQ(rssState.unstructuredSceneState.response, state::UnstructuredSceneResponse::DriveAway);
- - - - -
- ]
177 : : }
178 : :
179 : 2 : TEST(RssSituationCheckingTestsUnstructuredSceneChecker, calculateState_other_in_front_stopped_drive_away)
180 : : {
181 : 1 : RssUnstructuredSceneChecker unstructuredSceneChecker;
182 : 1 : Situation situation;
183 : 1 : situation.situationId = 1;
184 : 1 : situation.situationType = SituationType::Unstructured;
185 [ + - ]: 1 : situation.egoVehicleState = createVehicleState(world::ObjectType::EgoVehicle, 1.0, 0.0);
186 : 1 : situation.egoVehicleState.objectState.centerPoint.x = physics::Distance(0.0);
187 [ + - + - ]: 1 : situation.egoVehicleState.dynamics.unstructuredSettings.driveAwayMaxAngle = 3. * physics::cPI / 4.;
188 [ + - ]: 1 : situation.otherVehicleState = createVehicleState(world::ObjectType::OtherVehicle, 0.0, 0.0);
189 : 1 : situation.otherVehicleState.objectState.centerPoint.x = physics::Distance(20.0);
190 : 1 : state::UnstructuredSceneStateInformation egoStateInfo;
191 : :
192 : 1 : state::RssState rssState;
193 [ + - - + : 1 : ASSERT_TRUE(
- - - - -
- - - ]
194 : : unstructuredSceneChecker.calculateRssStateUnstructured(world::TimeIndex(1.), situation, egoStateInfo, rssState));
195 [ - + - - : 1 : ASSERT_TRUE(rssState.unstructuredSceneState.isSafe);
- - - - -
- ]
196 [ + - - + : 1 : ASSERT_EQ(rssState.unstructuredSceneState.response, state::UnstructuredSceneResponse::ContinueForward);
- - - - -
- ]
197 : :
198 : : // other must brake, ego drive away
199 : 1 : situation.egoVehicleState.objectState.centerPoint.x = physics::Distance(5.0);
200 : 1 : situation.egoVehicleState.objectState.speed = physics::Speed(0.0);
201 [ + - - + : 1 : ASSERT_TRUE(
- - - - -
- - - ]
202 : : unstructuredSceneChecker.calculateRssStateUnstructured(world::TimeIndex(2.), situation, egoStateInfo, rssState));
203 [ - + - - : 1 : ASSERT_FALSE(rssState.unstructuredSceneState.isSafe);
- - - - -
- ]
204 [ + - - + : 1 : ASSERT_EQ(rssState.unstructuredSceneState.response, state::UnstructuredSceneResponse::DriveAway);
- - - - -
- ]
205 : :
206 : : // ego drives away with forbidden heading
207 : 1 : situation.egoVehicleState.objectState.centerPoint.x = physics::Distance(5.0);
208 : 1 : situation.egoVehicleState.objectState.speed = physics::Speed(5.0);
209 [ + - ]: 1 : situation.egoVehicleState.objectState.steeringAngle = physics::cPI / 5.;
210 [ + - - + : 1 : ASSERT_TRUE(
- - - - -
- - - ]
211 : : unstructuredSceneChecker.calculateRssStateUnstructured(world::TimeIndex(3.), situation, egoStateInfo, rssState));
212 [ - + - - : 1 : ASSERT_FALSE(rssState.unstructuredSceneState.isSafe);
- - - - -
- ]
213 [ + - - + : 1 : ASSERT_EQ(rssState.unstructuredSceneState.response, state::UnstructuredSceneResponse::Brake);
- - - - -
- ]
214 : :
215 : : // ego forced to stop. drive away again possible
216 : 1 : situation.egoVehicleState.objectState.centerPoint.x = physics::Distance(5.0);
217 : 1 : situation.egoVehicleState.objectState.speed = physics::Speed(0.0);
218 [ + - - + : 1 : ASSERT_TRUE(
- - - - -
- - - ]
219 : : unstructuredSceneChecker.calculateRssStateUnstructured(world::TimeIndex(4.), situation, egoStateInfo, rssState));
220 [ - + - - : 1 : ASSERT_FALSE(rssState.unstructuredSceneState.isSafe);
- - - - -
- ]
221 [ + - - + : 1 : ASSERT_EQ(rssState.unstructuredSceneState.response, state::UnstructuredSceneResponse::DriveAway);
- - - - -
- ]
222 : :
223 : : // ego drives away with allowed heading
224 : 1 : situation.egoVehicleState.objectState.centerPoint.x = physics::Distance(5.0);
225 : 1 : situation.egoVehicleState.objectState.speed = physics::Speed(5.0);
226 [ + - ]: 1 : situation.egoVehicleState.objectState.steeringAngle = physics::cPI / 4.;
227 [ + - - + : 1 : ASSERT_TRUE(
- - - - -
- - - ]
228 : : unstructuredSceneChecker.calculateRssStateUnstructured(world::TimeIndex(5.), situation, egoStateInfo, rssState));
229 [ - + - - : 1 : ASSERT_FALSE(rssState.unstructuredSceneState.isSafe);
- - - - -
- ]
230 [ + - - + : 1 : ASSERT_EQ(rssState.unstructuredSceneState.response, state::UnstructuredSceneResponse::DriveAway);
- - - - -
- ]
231 : :
232 : : // ego drives away with allowed heading but other started to move again
233 : 1 : situation.egoVehicleState.objectState.centerPoint.x = physics::Distance(5.0);
234 : 1 : situation.egoVehicleState.objectState.speed = physics::Speed(5.0);
235 [ + - ]: 1 : situation.egoVehicleState.objectState.steeringAngle = physics::cPI / 4.;
236 : 1 : situation.otherVehicleState.objectState.centerPoint.x = physics::Distance(21.0);
237 [ + - - + : 1 : ASSERT_TRUE(
- - - - -
- - - ]
238 : : unstructuredSceneChecker.calculateRssStateUnstructured(world::TimeIndex(6.), situation, egoStateInfo, rssState));
239 [ - + - - : 1 : ASSERT_FALSE(rssState.unstructuredSceneState.isSafe);
- - - - -
- ]
240 [ + - - + : 1 : ASSERT_EQ(rssState.unstructuredSceneState.response, state::UnstructuredSceneResponse::Brake);
- - - - -
- ]
241 : : }
242 : :
243 : : } // namespace situation
244 : : } // namespace rss
245 : : } // namespace ad
|
