Branch data Line data Source code
1 : : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
2 : : //
3 : : // Copyright (C) 2019-2021 Intel Corporation
4 : : //
5 : : // SPDX-License-Identifier: MIT
6 : : //
7 : : // ----------------- END LICENSE BLOCK -----------------------------------
8 : :
9 : : #include <ad/map/access/Logging.hpp>
10 : : #include <ad/map/access/Operation.hpp>
11 : : #include <ad/map/intersection/Intersection.hpp>
12 : : #include <ad/map/match/Operation.hpp>
13 : : #include <ad/map/route/Planning.hpp>
14 : : #include <ad/map/route/RoutePrediction.hpp>
15 : : #include <gtest/gtest.h>
16 : :
17 : : using namespace ::ad;
18 : : using namespace map;
19 : : using namespace map::point;
20 : : using namespace map::route;
21 : : using namespace map::route::planning;
22 : :
23 : : struct RoutePredictionTest : ::testing::Test
24 : : {
25 : 13 : RoutePredictionTest()
26 : 13 : {
27 : 13 : }
28 : :
29 : 13 : virtual void SetUp()
30 : : {
31 : 13 : access::cleanup();
32 [ + - - + ]: 13 : if (!access::init(getTestMap()))
33 : : {
34 [ # # # # : 0 : throw std::runtime_error("Unable to initialize with " + getTestMap());
# # ]
35 : : }
36 : : // access::getLogger()->set_level(spdlog::level::trace);
37 : :
38 : 13 : getPredictionStartParaPoint(predictionStart.point);
39 [ + + ]: 13 : if (lane::isLaneDirectionPositive(lane::getLane(predictionStart.point.laneId)))
40 : : {
41 : 9 : predictionStart.direction = route::planning::RoutingDirection::POSITIVE;
42 : : }
43 : : else
44 : : {
45 : 4 : predictionStart.direction = route::planning::RoutingDirection::NEGATIVE;
46 : : }
47 : 13 : }
48 : :
49 : 13 : virtual void TearDown()
50 : : {
51 : 13 : access::cleanup();
52 : 13 : }
53 : :
54 : : virtual std::string getTestMap() = 0;
55 : :
56 : 12 : virtual void getPredictionStartParaPoint(point::ParaPoint &predictionStartResult)
57 : : {
58 [ + - ]: 12 : auto predictionStartGeo = getPredictionStartGeo();
59 [ + - - + : 12 : ASSERT_TRUE(withinValidInputRange(predictionStartGeo));
- - - - -
- - - ]
60 [ + - ]: 12 : match::AdMapMatching mapMatching;
61 : : auto mapMatchingResults
62 [ + - ]: 12 : = mapMatching.getMapMatchedPositions(predictionStartGeo, physics::Distance(1.), physics::Probability(0.8));
63 : :
64 [ + - - + : 12 : ASSERT_GE(mapMatchingResults.size(), 1u);
- - - - -
- ]
65 : 12 : predictionStartResult = mapMatchingResults.front().lanePoint.paraPoint;
66 : : }
67 : :
68 : 9 : virtual point::GeoPoint getPredictionStartGeo()
69 : : {
70 : 9 : point::GeoPoint resultPoint;
71 [ + - + - ]: 18 : auto pois = access::getPointsOfInterest();
72 [ + - ]: 9 : if (pois.size() > 0u)
73 : : {
74 : 9 : resultPoint = pois.front().geoPoint;
75 : : }
76 : 18 : return resultPoint;
77 : : }
78 : :
79 : : route::planning::RoutingParaPoint predictionStart;
80 : : };
81 : :
82 : : struct RoutePredictionTestTown01 : public RoutePredictionTest
83 : : {
84 : 7 : std::string getTestMap() override
85 : : {
86 [ + - ]: 7 : return "test_files/Town01.txt";
87 : : }
88 : : };
89 : :
90 : : struct RoutePredictionTestTown03 : public RoutePredictionTest
91 : : {
92 : 5 : std::string getTestMap() override
93 : : {
94 [ + - ]: 5 : return "test_files/Town03.txt";
95 : : }
96 : : };
97 : :
98 : 2 : TEST_F(RoutePredictionTestTown01, route_prediction_positive)
99 : : {
100 : : // remain in the same lane
101 [ + - ]: 1 : auto routePredictions = route::planning::predictRoutesOnDistance(predictionStart, physics::Distance(10.));
102 [ + - - + : 1 : ASSERT_EQ(routePredictions.size(), 1u);
- - - - -
- ]
103 [ + - - + : 1 : ASSERT_EQ(1u, routePredictions[0].roadSegments.size());
- - - - -
- ]
104 : 1 : point::ParaPoint expectedRouteEnd;
105 : 1 : expectedRouteEnd.laneId = predictionStart.point.laneId;
106 : 1 : expectedRouteEnd.parametricOffset = physics::ParametricValue(1.);
107 [ + - + - ]: 1 : auto findWaypointResult = route::findNearestWaypoint({expectedRouteEnd}, routePredictions[0]);
108 [ - + - - : 1 : EXPECT_TRUE(findWaypointResult.isValid());
- - - - -
- ]
109 : :
110 : : // the next crossing leads to a split of the route
111 [ + - ]: 1 : routePredictions = route::planning::predictRoutesOnDistance(predictionStart, physics::Distance(150.));
112 [ + - - + : 1 : EXPECT_EQ(routePredictions.size(), 2u);
- - - - -
- ]
113 : : }
114 : :
115 : 2 : TEST_F(RoutePredictionTestTown01, route_prediction_negative)
116 : : {
117 : : // since standing with wrong orientation, no prediction is available
118 : 1 : predictionStart.direction = route::planning::RoutingDirection::NEGATIVE;
119 [ + - ]: 1 : auto routePredictions = route::planning::predictRoutesOnDistance(predictionStart, physics::Distance(10.));
120 [ + - - + : 1 : ASSERT_EQ(routePredictions.size(), 0u);
- - - - -
- ]
121 : :
122 : : // extending the search space doesn't help here
123 [ + - ]: 1 : routePredictions = route::planning::predictRoutesOnDistance(predictionStart, physics::Distance(150.));
124 [ + - - + : 1 : ASSERT_EQ(routePredictions.size(), 0u);
- - - - -
- ]
125 : : }
126 : :
127 : 2 : TEST_F(RoutePredictionTestTown01, route_prediction_dont_care)
128 : : {
129 : : // not knowing the start direction leads to the same result as positive here
130 : 1 : predictionStart.direction = route::planning::RoutingDirection::DONT_CARE;
131 [ + - ]: 1 : auto routePredictions = route::planning::predictRoutesOnDistance(predictionStart, physics::Distance(10.));
132 [ + - - + : 1 : ASSERT_EQ(routePredictions.size(), 1u);
- - - - -
- ]
133 [ + - - + : 1 : ASSERT_EQ(1u, routePredictions[0].roadSegments.size());
- - - - -
- ]
134 : 1 : point::ParaPoint expectedRouteEnd;
135 : 1 : expectedRouteEnd.laneId = predictionStart.point.laneId;
136 : 1 : expectedRouteEnd.parametricOffset = physics::ParametricValue(1.);
137 [ + - + - ]: 1 : auto findWaypointResult = route::findNearestWaypoint({expectedRouteEnd}, routePredictions[0]);
138 [ - + - - : 1 : EXPECT_TRUE(findWaypointResult.isValid());
- - - - -
- ]
139 : :
140 : : // the next crossing leads to a split of the route
141 [ + - ]: 1 : routePredictions = route::planning::predictRoutesOnDistance(predictionStart, physics::Distance(150.));
142 [ + - - + : 1 : EXPECT_EQ(routePredictions.size(), 2u);
- - - - -
- ]
143 : : }
144 : :
145 : 2 : TEST_F(RoutePredictionTestTown01, route_prediction_directionless)
146 : : {
147 : : // the directionless search allows to drive actually in both directions and change between lanes of different lane
148 : : // direction
149 : : auto routePredictions = route::planning::predictRoutesDirectionless(
150 [ + - ]: 1 : predictionStart.point, physics::Distance(10.), physics::Duration(100.));
151 [ + - - + : 1 : ASSERT_EQ(routePredictions.size(), 2u);
- - - - -
- ]
152 [ + - - + : 1 : ASSERT_EQ(1u, routePredictions[0].roadSegments.size());
- - - - -
- ]
153 : 1 : point::ParaPoint expectedRouteEnd;
154 : 1 : expectedRouteEnd.laneId = predictionStart.point.laneId;
155 : 1 : expectedRouteEnd.parametricOffset = physics::ParametricValue(1.);
156 [ + - + - ]: 1 : auto findWaypointResult0 = route::findNearestWaypoint({expectedRouteEnd}, routePredictions[0]);
157 [ - + - - : 1 : EXPECT_TRUE(findWaypointResult0.isValid());
- - - - -
- ]
158 [ + - - + : 1 : ASSERT_EQ(1u, routePredictions[1].roadSegments.size());
- - - - -
- ]
159 : 1 : expectedRouteEnd.parametricOffset = physics::ParametricValue(0.);
160 [ + - + - ]: 1 : auto findWaypointResult1 = route::findNearestWaypoint({expectedRouteEnd}, routePredictions[1]);
161 [ - + - - : 1 : EXPECT_TRUE(findWaypointResult1.isValid());
- - - - -
- ]
162 : :
163 : : // the next crossing leads to a split of the each of the routes
164 : 2 : routePredictions = route::planning::predictRoutesDirectionless(
165 [ + - ]: 2 : predictionStart.point, physics::Distance(150.), physics::Duration(100.));
166 [ + - - + : 1 : EXPECT_EQ(routePredictions.size(), 4u);
- - - - -
- ]
167 : : }
168 : :
169 : 2 : TEST_F(RoutePredictionTestTown01, route_prediction_relevant_lanes)
170 : : {
171 : : // larger prediction distance to have two crossings and another split
172 [ + - ]: 1 : auto routePredictions = route::planning::predictRoutesOnDistance(predictionStart, physics::Distance(300.));
173 [ + - - + : 1 : ASSERT_EQ(routePredictions.size(), 3u);
- - - - -
- ]
174 : :
175 : : // now we restrict the prediction to the lanes of and around the first intersection
176 [ + - - + : 1 : ASSERT_GE(routePredictions[0].roadSegments.size(), 2u);
- - - - -
- ]
177 : 1 : auto intersectionLaneId = routePredictions[0].roadSegments[1].drivableLaneSegments[0].laneInterval.laneId;
178 [ + - ]: 1 : auto coreIntersection = intersection::CoreIntersection::getCoreIntersectionFor(intersectionLaneId);
179 [ + - - + : 1 : ASSERT_NE(coreIntersection, nullptr);
- - - - -
- ]
180 [ + - + - ]: 1 : auto relevantLanes = coreIntersection->internalLanes();
181 [ + - + - : 1 : relevantLanes.insert(coreIntersection->entryLanes().begin(), coreIntersection->entryLanes().end());
+ - ]
182 [ + - + - : 1 : relevantLanes.insert(coreIntersection->exitLanes().begin(), coreIntersection->exitLanes().end());
+ - ]
183 : :
184 : : routePredictions
185 [ + - ]: 2 : = route::planning::predictRoutesOnDistance(predictionStart,
186 : 1 : physics::Distance(300.),
187 : : route::RouteCreationMode::SameDrivingDirection,
188 : : route::planning::FilterDuplicatesMode::SubRoutesPreferLongerOnes,
189 : 1 : relevantLanes);
190 : : // as a result the second intersection is not existing anymore and the predictions stop,
191 : : // but the first intersection is still there, so one split is expected
192 [ + - - + : 1 : ASSERT_EQ(routePredictions.size(), 2u);
- - - - -
- ]
193 : : }
194 : :
195 : 2 : TEST_F(RoutePredictionTestTown03, route_prediction_positive)
196 : : {
197 [ + - ]: 2 : auto routePredictions = route::planning::predictRoutesOnDistance(predictionStart, physics::Distance(10.));
198 [ + - - + : 1 : EXPECT_EQ(routePredictions.size(), 1u);
- - - - -
- ]
199 : :
200 [ + - ]: 1 : routePredictions = route::planning::predictRoutesOnDistance(predictionStart, physics::Distance(478.));
201 [ + - - + : 1 : EXPECT_EQ(routePredictions.size(), 18u);
- - - - -
- ]
202 : 1 : }
203 : :
204 : 2 : TEST_F(RoutePredictionTestTown03, route_prediction_dont_care)
205 : : {
206 : 1 : predictionStart.direction = route::planning::RoutingDirection::DONT_CARE;
207 [ + - ]: 2 : auto routePredictions = route::planning::predictRoutesOnDistance(predictionStart, physics::Distance(10.));
208 [ + - - + : 1 : EXPECT_EQ(routePredictions.size(), 1u);
- - - - -
- ]
209 : :
210 [ + - ]: 1 : routePredictions = route::planning::predictRoutesOnDistance(predictionStart, physics::Distance(478.));
211 [ + - - + : 1 : EXPECT_EQ(routePredictions.size(), 18u);
- - - - -
- ]
212 : 1 : }
213 : :
214 : 2 : TEST_F(RoutePredictionTestTown03, route_prediction_constructor)
215 : : {
216 : 1 : predictionStart.direction = route::planning::RoutingDirection::DONT_CARE;
217 [ + - ]: 1 : auto routePredictions = route::planning::predictRoutesOnDuration(predictionStart, physics::Duration(1.));
218 [ + - - + : 1 : ASSERT_EQ(routePredictions.size(), 1u);
- - - - -
- ]
219 : :
220 [ + - ]: 1 : routePredictions = route::planning::predictRoutes(predictionStart, physics::Distance(478.), physics::Duration(478.));
221 [ + - - + : 1 : ASSERT_EQ(routePredictions.size(), 18u);
- - - - -
- ]
222 : : }
223 : :
224 : 2 : TEST_F(RoutePredictionTestTown03, route_getBasicRoutes)
225 : : {
226 : 1 : predictionStart.direction = route::planning::RoutingDirection::DONT_CARE;
227 [ + - ]: 2 : map::route::planning::RoutePrediction routePrediction(predictionStart, physics::Distance(10.));
228 [ + - + - ]: 1 : if (routePrediction.calculate())
229 : : {
230 [ + - ]: 1 : std::vector<Route::BasicRoute> res = routePrediction.getBasicRoutes();
231 : : }
232 : 1 : }
233 : :
234 : : struct RoutePredictionTestIntersection : public RoutePredictionTestTown01
235 : : {
236 : 2 : point::GeoPoint getPredictionStartGeo() override
237 : : {
238 : 2 : return point::createGeoPoint(point::Longitude(0.00079520), point::Latitude(-0.00284188), point::Altitude(0.));
239 : : }
240 : : };
241 : :
242 : 2 : TEST_F(RoutePredictionTestIntersection, route_prediction_dont_stop_within_intersections)
243 : : {
244 [ + - ]: 1 : auto routePredictions = route::planning::predictRoutesOnDistance(predictionStart, physics::Distance(7.));
245 [ + - - + : 1 : EXPECT_EQ(routePredictions.size(), 2u);
- - - - -
- ]
246 : :
247 [ + + ]: 3 : for (auto &routePrediction : routePredictions)
248 : : {
249 [ + - ]: 2 : auto intersections = intersection::Intersection::getIntersectionsForRoute(routePrediction);
250 [ + - - + : 2 : ASSERT_EQ(1u, intersections.size());
- - - - -
- ]
251 : : }
252 : : }
253 : :
254 : 2 : TEST_F(RoutePredictionTestIntersection, route_extension_dont_stop_within_intersections)
255 : : {
256 [ + - ]: 1 : auto routePredictions = route::planning::predictRoutesOnDistance(predictionStart, physics::Distance(1.));
257 [ + - - + : 1 : EXPECT_EQ(routePredictions.size(), 1u);
- - - - -
- ]
258 : :
259 [ + - ]: 1 : route::FullRoute route = routePredictions.front();
260 : 1 : routePredictions.clear();
261 : :
262 [ + - - + : 1 : EXPECT_TRUE(route::extendRouteToDistance(route, physics::Distance(7.), routePredictions));
- - - - -
- - - ]
263 : :
264 [ + - ]: 1 : routePredictions.push_back(route);
265 : :
266 [ + + ]: 3 : for (auto &routePrediction : routePredictions)
267 : : {
268 [ + - ]: 2 : auto intersections = intersection::Intersection::getIntersectionsForRoute(routePrediction);
269 [ + - - + : 2 : ASSERT_EQ(1u, intersections.size());
- - - - -
- ]
270 : : }
271 : : }
272 : :
273 : : struct RoutePredictionTestRoundabout : public RoutePredictionTestTown03
274 : : {
275 : 1 : point::GeoPoint getPredictionStartGeo() override
276 : : {
277 : 1 : return point::createGeoPoint(point::Longitude(0.00033117), point::Latitude(0.00007233), point::Altitude(0.));
278 : : }
279 : : };
280 : :
281 : 2 : TEST_F(RoutePredictionTestRoundabout, route_prediction)
282 : : {
283 [ + - ]: 2 : auto routePredictions = route::planning::predictRoutesOnDistance(predictionStart, physics::Distance(130.));
284 : : // starting at the east entry we get:
285 : : // - to north
286 : : // - to south
287 : : // - circling just before out to east
288 : : // the route towards the west is not present in the new map!
289 [ + - - + : 1 : EXPECT_EQ(routePredictions.size(), 3u);
- - - - -
- ]
290 : 1 : }
291 : :
292 : : struct RoutePredictionTestTown04 : public RoutePredictionTest
293 : : {
294 : 1 : void getPredictionStartParaPoint(point::ParaPoint &predictionStartResult) override
295 : : {
296 : 1 : predictionStartResult.laneId = lane::LaneId(490148);
297 : 1 : predictionStartResult.parametricOffset = physics::ParametricValue(0.1);
298 : 1 : }
299 : :
300 : 1 : std::string getTestMap() override
301 : : {
302 [ + - ]: 1 : return "test_files/Town04.txt";
303 : : }
304 : : };
305 : :
306 : 2 : TEST_F(RoutePredictionTestTown04, filter_duplicated_routes_multilane_not_algined)
307 : : {
308 : : auto routePredictions
309 : 1 : = route::planning::predictRoutesOnDistance(predictionStart,
310 : 2 : physics::Distance(30),
311 : : RouteCreationMode::SameDrivingDirection,
312 [ + - ]: 3 : planning::FilterDuplicatesMode::SubRoutesPreferShorterOnes);
313 : :
314 [ + - - + : 1 : EXPECT_EQ(1u, routePredictions.size());
- - - - -
- ]
315 : 1 : }
|
