Branch data Line data Source code
1 : : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
2 : : //
3 : : // Copyright (C) 2019-2021 Intel Corporation
4 : : //
5 : : // SPDX-License-Identifier: LGPL-2.1-only
6 : : //
7 : : // ----------------- END LICENSE BLOCK -----------------------------------
8 : : #pragma once
9 : :
10 : : #include <ad/rss/map/RssWorldModelCreation.hpp>
11 : : #include <ad/rss/map/test_support/RssMapIntegrationTestBase.hpp>
12 : : #include <ad/rss/world/WorldModelValidInputRange.hpp>
13 : : #include <algorithm>
14 : :
15 : : namespace ad {
16 : : namespace rss {
17 : : namespace map {
18 : : namespace test_support {
19 : :
20 : : struct RssWorldModelCreationTest : public virtual RssMapIntegrationTestBase
21 : : {
22 : 219 : virtual ~RssWorldModelCreationTest() = default;
23 : :
24 : : typedef std::tuple<::ad::rss::world::ConstellationType, size_t, size_t, ::ad::physics::Speed> ExpectedResultTuple;
25 : :
26 : 212 : void checkConstellationResults(::ad::rss::world::WorldModel const &world_model,
27 : : ::ad::rss::world::ObjectId const &otherVehicleId,
28 : : std::vector<ExpectedResultTuple> const &expectedResults)
29 : : {
30 [ + - - + : 212 : EXPECT_TRUE(withinValidInputRange(world_model));
- - - - -
- - - ]
31 [ + - - + : 212 : ASSERT_EQ(expectedResults.size(), world_model.constellations.size());
- - - - -
- + - ]
32 : :
33 [ + - ]: 424 : std::vector<ExpectedResultTuple> notMatchedResults = expectedResults;
34 [ + + ]: 584 : for (auto i = 0u; i < world_model.constellations.size(); ++i)
35 : : {
36 : 372 : auto &constellation = world_model.constellations[i];
37 [ + - - + : 372 : EXPECT_TRUE(withinValidInputRange(constellation)) << i;
- - - - -
- - - -
- ]
38 [ + - - + : 372 : EXPECT_EQ(::ad::rss::world::ObjectType::OtherVehicle, constellation.object.object_type) << i;
- - - - -
- - - ]
39 [ + - - + : 372 : EXPECT_EQ(otherVehicleId, constellation.object.object_id) << i;
- - - - -
- - - ]
40 : : auto findResult
41 [ + - + - ]: 372 : = std::find_if(notMatchedResults.begin(),
42 : : notMatchedResults.end(),
43 : 476 : [constellation](ExpectedResultTuple const &expectedResultTuple) {
44 : 476 : return (std::get<0>(expectedResultTuple) == constellation.constellation_type)
45 [ + + ]: 470 : && (std::get<1>(expectedResultTuple) == constellation.ego_vehicle_road.size())
46 [ + - ]: 392 : && (std::get<2>(expectedResultTuple) == constellation.intersecting_road.size())
47 [ + + + - ]: 1338 : && (mCheckerControlTest->getObjectVehicleDynamics(std::get<3>(expectedResultTuple))
48 [ + - + + ]: 1260 : == constellation.object_rss_dynamics);
49 : : });
50 [ + - ]: 372 : if (findResult != notMatchedResults.end())
51 : : {
52 [ + - ]: 372 : notMatchedResults.erase(findResult);
53 : : }
54 : : else
55 : : {
56 [ # # # # : 0 : EXPECT_TRUE(false) << " constellation index: " << i << " not found" << constellation;
# # # # #
# # # # #
# # # # ]
57 : : }
58 : : }
59 [ - + - - : 212 : EXPECT_TRUE(notMatchedResults.empty())
- - - - ]
60 [ # # # # : 0 : << " not all expected results observed: " << std::get<0>(notMatchedResults[0]) << ", "
# # ]
61 [ # # # # : 0 : << std::get<1>(notMatchedResults[0]) << ", " << std::get<2>(notMatchedResults[0]) << ", "
# # # # ]
62 [ - - - - : 212 : << mCheckerControlTest->getObjectVehicleDynamics(std::get<3>(notMatchedResults[0]));
- - ]
63 : : }
64 : :
65 : : virtual ObjectGeoLocationTuple getEgoVehicleStartLocation() = 0;
66 : :
67 : : virtual ::ad::map::point::GeoPoint getEgoVehicleTarget() = 0;
68 : :
69 : 212 : void performConstellationTest(ObjectGeoLocationTuple const &egoLocation,
70 : : ObjectGeoLocationTuple const &otherLocation,
71 : : std::vector<ExpectedResultTuple> const &expectedResults)
72 : : {
73 [ + - + - ]: 212 : ::ad::rss::map::RssWorldModelCreation constellationCreation(1u, mCheckerControlTest->getEgoVehicleDynamics());
74 : :
75 : : auto ego_vehicle = createEgoVehicle(
76 [ + - + - : 212 : ::ad::rss::world::ObjectId(123), getEgoVehicleStartLocation(), getEgoVehicleTarget(), egoLocation);
+ - ]
77 [ + - ]: 212 : auto otherVehicle = createOtherVehicle(::ad::rss::world::ObjectId(10), otherLocation);
78 [ + - ]: 212 : auto const egoObjectData = ego_vehicle->getTestRssObjecData();
79 [ + - ]: 212 : auto const otherObjectData = otherVehicle->getTestRssObjecData();
80 : :
81 [ + - + - : 212 : EXPECT_TRUE(
+ - - + -
- - - - -
- - ]
82 : : constellationCreation.appendConstellations(egoObjectData,
83 : : ego_vehicle->mTestRoute,
84 : : otherObjectData,
85 : : mCheckerControlTest->mRestrictSpeedLimitMode,
86 : : ego_vehicle->getCheckerControl()->getCurrentGreenTrafficLights(),
87 : 212 : mCheckerControlTest->mVehicleWorldModelCreationMode));
88 : :
89 [ + - ]: 212 : auto const world_model = constellationCreation.getWorldModel();
90 [ + - ]: 212 : checkConstellationResults(world_model, otherObjectData.id, expectedResults);
91 : 212 : }
92 : : };
93 : :
94 : : struct RssWorldModelCreationTestTown01 : RssWorldModelCreationTest
95 : : {
96 : 218 : MapToLoad getMapToLoad() override
97 : : {
98 : 218 : return MapToLoad::Town01;
99 : : }
100 : :
101 : 218 : ObjectGeoLocationTuple getEgoVehicleStartLocation() override
102 : : {
103 : : // lane_id: offset 240151:0.8 (far away from intersection starting at 240151:0.0)
104 : : auto egoGeoLocation
105 [ + - ]: 218 : = std::make_tuple(::ad::map::point::Longitude(8.0012621), ::ad::map::point::Latitude(48.9972518), M_PI_2);
106 : :
107 : 218 : return egoGeoLocation;
108 : : }
109 : :
110 : 213 : ::ad::map::point::GeoPoint getEgoVehicleTarget() override
111 : : {
112 : : // lane_id: offset 120149:0.52 (ego turn right)
113 : 213 : auto positionEndGeo = ::ad::map::point::createGeoPoint(::ad::map::point::Longitude(8.003),
114 : : ::ad::map::point::Latitude(48.99821051747871792),
115 : : ::ad::map::point::AltitudeUnknown);
116 : 213 : return positionEndGeo;
117 : : }
118 : :
119 : : /*
120 : : * | NorthIncoming NorthOutgoing |
121 : : * | |
122 : : * | |
123 : : * | NorthEntering NorthExiting |______________________
124 : : * |
125 : : * | EastEntering EastIncoming
126 : : * |
127 : : * | EastExiting EastOutgoing
128 : : * | _______________________
129 : : * | SouthExiting SouthEntering |
130 : : * | |
131 : : * | |
132 : : * | SouthOutgoing SouthIncoming |
133 : : *
134 : : */
135 : :
136 : 76 : ObjectGeoLocationTuple locationSouthIncoming()
137 : : {
138 : : // lane_id: 240151 directly in front of intersection
139 : : return std::make_tuple(
140 [ + - ]: 152 : ::ad::map::point::Longitude(8.00125444865324766), ::ad::map::point::Latitude(48.9980), M_PI_2);
141 : : }
142 : :
143 : 63 : ObjectGeoLocationTuple locationSouthEntering()
144 : : {
145 : : // lane_id: 240151:0. & 1290149:0. (right turn lane) & 1360149:0. (straight lane)
146 [ + - ]: 126 : return std::make_tuple(::ad::map::point::Longitude(8.0012619), ::ad::map::point::Latitude(48.9981266), M_PI_2);
147 : : }
148 : :
149 : 54 : ObjectGeoLocationTuple locationSouth2East()
150 : : {
151 : : // lane_id: 1290149:0.3, starting right turn
152 [ + - ]: 108 : return std::make_tuple(::ad::map::point::Longitude(8.0012731), ::ad::map::point::Latitude(48.9981734), M_PI_2 / 2.);
153 : : }
154 : :
155 : 55 : ObjectGeoLocationTuple locationEastExiting()
156 : : {
157 : : // lane_id: 120149:0 & 1290149:1. right turn around the intersection
158 [ + - ]: 110 : return std::make_tuple(::ad::map::point::Longitude(8.0013843), ::ad::map::point::Latitude(48.9982095), 0.);
159 : : }
160 : :
161 : 56 : ObjectGeoLocationTuple locationEastOutgoing()
162 : : {
163 : : // lane_id: 120149:0.16 right turn around the intersection
164 : : return std::make_tuple(
165 [ + - ]: 112 : ::ad::map::point::Longitude(8.00188527300496979), ::ad::map::point::Latitude(48.99821051747871792), 0.);
166 : : }
167 : :
168 : 14 : ObjectGeoLocationTuple locationEastIncoming()
169 : : {
170 : : // lane_id: 120151:0.16
171 : : return std::make_tuple(
172 [ + - ]: 28 : ::ad::map::point::Longitude(8.00188527300496979), ::ad::map::point::Latitude(48.99824), M_PI);
173 : : }
174 : :
175 : 10 : ObjectGeoLocationTuple locationEastEntering()
176 : : {
177 : : // lane_id: 120151:0.16
178 [ + - ]: 20 : return std::make_tuple(::ad::map::point::Longitude(8.0013870), ::ad::map::point::Latitude(48.9982463), M_PI);
179 : : }
180 : :
181 : 5 : ObjectGeoLocationTuple locationEast2North()
182 : : {
183 : : // lane_id: 1370149:0.5
184 : : return std::make_tuple(
185 [ + - ]: 10 : ::ad::map::point::Longitude(8.0012889), ::ad::map::point::Latitude(48.9982608), 3. * M_PI_2 / 2.);
186 : : }
187 : :
188 : 10 : ObjectGeoLocationTuple locationSouth2North()
189 : : {
190 : : // lane_id: 13690249:0.5, second half of intersection when driving straight
191 [ + - ]: 20 : return std::make_tuple(::ad::map::point::Longitude(8.0012631), ::ad::map::point::Latitude(48.9982781), M_PI_2);
192 : : }
193 : :
194 : 18 : ObjectGeoLocationTuple locationNorthExiting()
195 : : {
196 : : // lane_id: 13690249:1. & 230151:1., outgoing border of the intersection when driving straight
197 [ + - ]: 36 : return std::make_tuple(::ad::map::point::Longitude(8.0012623), ::ad::map::point::Latitude(48.9983242), M_PI_2);
198 : : }
199 : :
200 : 20 : ObjectGeoLocationTuple locationNorthOutgoing()
201 : : {
202 : : // lane_id: 230151:0.94 after the intersection when driving straight
203 [ + - ]: 40 : return std::make_tuple(::ad::map::point::Longitude(8.0012635), ::ad::map::point::Latitude(48.9984330), M_PI_2);
204 : : }
205 : :
206 : 14 : ObjectGeoLocationTuple locationNorthIncoming()
207 : : {
208 : : // lane_id: 230149, straight through the intersection in opposite direction
209 [ + - ]: 28 : return std::make_tuple(::ad::map::point::Longitude(8.00120), ::ad::map::point::Latitude(48.9985), -M_PI_2);
210 : : }
211 : :
212 : 10 : ObjectGeoLocationTuple locationNorthEntering()
213 : : {
214 : : // lane_id: 230149:1.0 & 1350251:1.0 touching straight through the intersection in opposite direction
215 [ + - ]: 20 : return std::make_tuple(::ad::map::point::Longitude(8.0012081), ::ad::map::point::Latitude(48.9983242), -M_PI_2);
216 : : }
217 : :
218 : 5 : ObjectGeoLocationTuple locationNorth2East()
219 : : {
220 : : // lane_id: 1380149:0.5
221 : : return std::make_tuple(
222 [ + - ]: 10 : ::ad::map::point::Longitude(8.0012391), ::ad::map::point::Latitude(48.9982440), -M_PI_2 / 2.);
223 : : }
224 : :
225 : 5 : ObjectGeoLocationTuple locationNorth2South()
226 : : {
227 : : // lane_id: 1350151:0.5, mid straight through the intersection in opposite direction
228 [ + - ]: 10 : return std::make_tuple(::ad::map::point::Longitude(8.0012077), ::ad::map::point::Latitude(48.9981817), -M_PI_2);
229 : : }
230 : :
231 : 5 : ObjectGeoLocationTuple locationEast2South()
232 : : {
233 : : // lane_id: 1300149:0.5
234 : : return std::make_tuple(
235 [ + - ]: 10 : ::ad::map::point::Longitude(8.0012368), ::ad::map::point::Latitude(48.9982053), 5. * M_PI_2 / 2.);
236 : : }
237 : :
238 : 10 : ObjectGeoLocationTuple locationSouthExiting()
239 : : {
240 : : // lane_id: 240149:0. opposite direction in front across intersection
241 [ + - ]: 20 : return std::make_tuple(::ad::map::point::Longitude(8.0012069), ::ad::map::point::Latitude(48.9981252), -M_PI_2);
242 : : }
243 : :
244 : 12 : ObjectGeoLocationTuple locationSouthOutgoing()
245 : : {
246 : : // lane_id: 240149, opposite direction no intersection in between
247 [ + - ]: 24 : return std::make_tuple(::ad::map::point::Longitude(8.00120), ::ad::map::point::Latitude(48.9980), -M_PI_2);
248 : : }
249 : : };
250 : :
251 : : struct RssWorldModelCreationTestWithRoute : public RssWorldModelCreationTestTown01
252 : : {
253 : 150 : TestMode getTestMode() override
254 : : {
255 : 150 : return TestMode::withRouteWithSpeedLimit;
256 : : }
257 : : };
258 : :
259 : : struct RssWorldModelCreationTestWithoutRoute : public RssWorldModelCreationTestTown01
260 : : {
261 : 69 : TestMode getTestMode() override
262 : : {
263 : 69 : return TestMode::withoutRouteWithoutSpeedLimit;
264 : : }
265 : : };
266 : :
267 : : } // namespace test_support
268 : : } // namespace map
269 : : } // namespace rss
270 : : } // namespace ad
|
