Branch data Line data Source code
1 : : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
2 : : //
3 : : // Copyright (C) 2018-2021 Intel Corporation
4 : : //
5 : : // SPDX-License-Identifier: MIT
6 : : //
7 : : // ----------------- END LICENSE BLOCK -----------------------------------
8 : :
9 : : #include <ad/map/access/Operation.hpp>
10 : : #include <ad/map/match/AdMapMatching.hpp>
11 : : #include <ad/map/route/Planning.hpp>
12 : : #include <ad/map/route/RouteOperation.hpp>
13 : : #include <algorithm>
14 : :
15 : : #include <gtest/gtest.h>
16 : :
17 : : using namespace ::ad;
18 : : using namespace ::ad::map;
19 : :
20 : : struct LaneChangeTest : ::testing::Test
21 : : {
22 : 5 : LaneChangeTest()
23 [ + - ]: 5 : {
24 : 5 : }
25 : :
26 : 5 : virtual ~LaneChangeTest() = default;
27 : :
28 : 5 : virtual void SetUp()
29 : : {
30 : 5 : loadTestFile();
31 : 5 : }
32 : :
33 : 5 : virtual void TearDown()
34 : : {
35 : 5 : ad::map::access::cleanup();
36 : 5 : }
37 : :
38 : : match::MapMatchedPosition getPOIMapMatchedPosition(std::string const poiName) const;
39 : :
40 : : /* mTestFile looks like the following:
41 : : *
42 : : * f
43 : : * _f
44 : : * e
45 : : * e
46 : : * aaaa|bbbb|cccc|dddd|ee
47 : : * |bbbb| |dddd|gggg|hhhh
48 : : *
49 : : * transitions: (a->b), (b->c) (c->d) (d->e) and (d->f)
50 : : * route points:
51 : : * - A: a point on road segment a
52 : : * - BL: a point on the left lane of road segment b
53 : : * - BR: a point on the right lane of road segment b
54 : : * - F: a point on road segment f
55 : : * - H: a point on road segment h
56 : : * the lane change always happens on road section d
57 : : *
58 : : *
59 : : */
60 : : std::string mTestFile{"test_files/LaneChange.adm.txt"};
61 : :
62 : : void loadTestFile();
63 : :
64 : : lane::ContactLaneList getSuccessorsAndPredecessorsForLaneId(lane::LaneId const &laneId) const;
65 : :
66 : : void testsLaneChangeEnd(route::FindLaneChangeResult const &findLaneChangeResult, int expectedEndIndex) const;
67 : :
68 : : void testsLaneChangeStart(route::FindLaneChangeResult const &findLaneChangeResult, int expectedStartIndex) const;
69 : : };
70 : :
71 : 5 : void LaneChangeTest::loadTestFile()
72 : : {
73 [ + - - + : 5 : ASSERT_TRUE(ad::map::access::init(mTestFile));
- - - - -
- - - ]
74 : :
75 : 5 : auto mapCenterPoint = point::GeoPoint();
76 : 5 : mapCenterPoint.latitude = point::Latitude(49.0183631);
77 : 5 : mapCenterPoint.longitude = point::Longitude(8.4411676);
78 : 5 : mapCenterPoint.altitude = point::Altitude(0.0);
79 : :
80 [ + - ]: 5 : ad::map::access::setENUReferencePoint(mapCenterPoint);
81 [ + - + - : 5 : ASSERT_EQ(5u, ad::map::access::getPointsOfInterest().size());
- + - - -
- - - ]
82 : : }
83 : :
84 : 13 : match::MapMatchedPosition LaneChangeTest::getPOIMapMatchedPosition(std::string const poiName) const
85 : : {
86 : 13 : match::MapMatchedPosition invalidPoint;
87 : :
88 : : auto foundPoi
89 : : = std::find_if(std::begin(ad::map::access::getPointsOfInterest()),
90 : : std::end(ad::map::access::getPointsOfInterest()),
91 [ + - + - : 50 : [&poiName](config::PointOfInterest const &poi) { return poi.name.compare(poiName) == 0; });
+ - ]
92 [ + - + - : 13 : EXPECT_NE(foundPoi, std::end(ad::map::access::getPointsOfInterest()));
- + - - -
- - - ]
93 [ + - - + ]: 13 : if (foundPoi == std::end(ad::map::access::getPointsOfInterest()))
94 : : {
95 : 0 : return invalidPoint;
96 : : }
97 : :
98 [ + - ]: 26 : ad::map::match::AdMapMatching mapMatching;
99 : : match::MapMatchedPositionConfidenceList mapMatchingResult
100 [ + - ]: 26 : = mapMatching.getMapMatchedPositions(foundPoi->geoPoint, ::ad::physics::Distance(0.2), physics::Probability(.8));
101 [ + - - + : 13 : EXPECT_EQ(1u, mapMatchingResult.size()) << " requested POI: " << poiName;
- - - - -
- - - -
- ]
102 [ - + ]: 13 : if (1u != mapMatchingResult.size())
103 : : {
104 : 0 : return invalidPoint;
105 : : }
106 : :
107 : 13 : return mapMatchingResult.front();
108 : : }
109 : :
110 : 2 : void LaneChangeTest::testsLaneChangeEnd(route::FindLaneChangeResult const &findLaneChangeResult,
111 : : int expectedEndIndex) const
112 : : {
113 [ + - - + : 2 : ASSERT_TRUE(findLaneChangeResult.isValid());
- - - - -
- - - ]
114 : :
115 [ + - + - : 2 : ASSERT_EQ(expectedEndIndex,
- + - - -
- - - ]
116 : : std::distance(std::begin(findLaneChangeResult.queryRoute.roadSegments),
117 : : findLaneChangeResult.laneChangeEndRouteIterator));
118 : :
119 : : // The end of the lane change cannot be the end of the route, otherwise we would not have a "next" lane to change to
120 [ + - + - : 2 : ASSERT_NE(findLaneChangeResult.laneChangeEndRouteIterator,
- + - - -
- - - ]
121 : : std::prev(findLaneChangeResult.queryRoute.roadSegments.end()));
122 : : }
123 : :
124 : 2 : void LaneChangeTest::testsLaneChangeStart(route::FindLaneChangeResult const &findLaneChangeResult,
125 : : int expectedStartIndex) const
126 : : {
127 : : // The lane change start is never at the end
128 [ + - - + : 2 : ASSERT_NE(findLaneChangeResult.laneChangeStartRouteIterator, std::end(findLaneChangeResult.queryRoute.roadSegments));
- - - - -
- ]
129 : :
130 [ + - + - : 2 : ASSERT_EQ(expectedStartIndex,
- + - - -
- - - ]
131 : : std::distance(std::begin(findLaneChangeResult.queryRoute.roadSegments),
132 : : findLaneChangeResult.laneChangeStartRouteIterator));
133 : : }
134 : :
135 : 2 : TEST_F(LaneChangeTest, current_position_not_on_route)
136 : : {
137 [ + - + - ]: 1 : auto mapMatchedPositionStart = getPOIMapMatchedPosition("A");
138 : : // ASSERT_NE(match::MapMatchedPosition(), mapMatchedPositionStart);
139 : :
140 [ + - + - ]: 1 : auto mapMatchedPositionEnd = getPOIMapMatchedPosition("H");
141 : : // ASSERT_NE(match::MapMatchedPosition(), mapMatchedPositionEnd);
142 : :
143 [ + - + - ]: 1 : auto currentPosition = getPOIMapMatchedPosition("F");
144 : : // ASSERT_NE(match::MapMatchedPosition(), currentPosition);
145 : :
146 : : auto route = route::planning::planRoute(mapMatchedPositionStart.lanePoint.paraPoint,
147 [ + - ]: 1 : mapMatchedPositionEnd.lanePoint.paraPoint);
148 : :
149 : : // This function needs to be tested:
150 [ + - ]: 1 : auto findLaneChangeResult = route::findFirstLaneChange(currentPosition, route);
151 : :
152 : : // F is not on the route - the result is invalid
153 [ + - - + : 1 : ASSERT_FALSE(findLaneChangeResult.isValid());
- - - - -
- - - ]
154 [ + - - + : 1 : ASSERT_EQ(&findLaneChangeResult.queryRoute, &route);
- - - - -
- ]
155 [ + - - + : 1 : ASSERT_EQ(findLaneChangeResult.laneChangeStartRouteIterator, std::end(route.roadSegments));
- - - - -
- ]
156 [ + - - + : 1 : ASSERT_EQ(findLaneChangeResult.laneChangeEndRouteIterator, std::end(route.roadSegments));
- - - - -
- ]
157 [ + - - + : 1 : ASSERT_EQ(findLaneChangeResult.laneChangeDirection, route::LaneChangeDirection::Invalid);
- - - - -
- ]
158 : : }
159 : :
160 : 2 : TEST_F(LaneChangeTest, find_lane_change_A_to_H_start_at_A)
161 : : {
162 [ + - + - ]: 1 : auto mapMatchedPositionStart = getPOIMapMatchedPosition("A");
163 : : // ASSERT_NE(match::MapMatchedPosition(), mapMatchedPositionStart);
164 : :
165 [ + - + - ]: 1 : auto mapMatchedPositionEnd = getPOIMapMatchedPosition("H");
166 : : // ASSERT_NE(match::MapMatchedPosition(), mapMatchedPositionEnd);
167 : :
168 : : auto route = route::planning::planRoute(mapMatchedPositionStart.lanePoint.paraPoint,
169 [ + - ]: 1 : mapMatchedPositionEnd.lanePoint.paraPoint);
170 : :
171 : : // We assume our car is on the start point A
172 : 1 : auto currentPosition = mapMatchedPositionStart;
173 : :
174 : : // This function needs to be tested:
175 [ + - ]: 1 : auto findLaneChangeResult = route::findFirstLaneChange(currentPosition, route);
176 : :
177 : : // The query route should be the route given
178 [ + - - + : 1 : ASSERT_EQ(&findLaneChangeResult.queryRoute, &route);
- - - - -
- ]
179 : :
180 : : // The end of the lane change is at index 3 (road segment d)
181 [ + - ]: 1 : testsLaneChangeEnd(findLaneChangeResult, 3);
182 : :
183 : : // The start of the lane change is at index 3
184 [ + - ]: 1 : testsLaneChangeStart(findLaneChangeResult, 3);
185 : :
186 : : // Check if the lane change was to the left
187 [ + - - + : 1 : ASSERT_EQ(findLaneChangeResult.laneChangeDirection, route::LaneChangeDirection::LeftToRight);
- - - - -
- ]
188 : :
189 : : // There is a lane change on the route, so the returned result is valid
190 [ + - - + : 1 : ASSERT_TRUE(findLaneChangeResult.isValid());
- - - - -
- - - ]
191 : : }
192 : :
193 : 2 : TEST_F(LaneChangeTest, find_lane_change_A_to_F_start_at_A)
194 : : {
195 [ + - + - ]: 1 : auto mapMatchedPositionStart = getPOIMapMatchedPosition("A");
196 : : // ASSERT_NE(match::MapMatchedPosition(), mapMatchedPositionStart);
197 : :
198 [ + - + - ]: 1 : auto mapMatchedPositionEnd = getPOIMapMatchedPosition("F");
199 : : // ASSERT_NE(match::MapMatchedPosition(), mapMatchedPositionEnd);
200 : :
201 : : auto route = route::planning::planRoute(mapMatchedPositionStart.lanePoint.paraPoint,
202 [ + - ]: 1 : mapMatchedPositionEnd.lanePoint.paraPoint);
203 : :
204 : : // Set our car on the start of the route
205 : 1 : auto currentPosition = mapMatchedPositionStart;
206 : :
207 : : // This function needs to be tested:
208 [ + - ]: 1 : auto findLaneChangeResult = route::findFirstLaneChange(currentPosition, route);
209 : :
210 : : // The query route should be the route given
211 [ + - - + : 1 : ASSERT_EQ(&findLaneChangeResult.queryRoute, &route);
- - - - -
- ]
212 : :
213 : : // There is no lane change on the route, so the boost optional is not set
214 [ + - - + : 1 : ASSERT_FALSE(findLaneChangeResult.isValid());
- - - - -
- - - ]
215 : :
216 : : // The start of the lane change set to invalid (std::end(route.roadSegments))
217 [ + - - + : 1 : ASSERT_EQ(findLaneChangeResult.laneChangeStartRouteIterator, std::end(route.roadSegments));
- - - - -
- ]
218 : :
219 : : // The end of the lane change is set to invalid (std::end(route.roadSegments))
220 [ + - - + : 1 : ASSERT_EQ(findLaneChangeResult.laneChangeEndRouteIterator, std::end(route.roadSegments));
- - - - -
- ]
221 : :
222 [ + - - + : 1 : ASSERT_EQ(findLaneChangeResult.laneChangeDirection, route::LaneChangeDirection::Invalid);
- - - - -
- ]
223 : : }
224 : :
225 : 2 : TEST_F(LaneChangeTest, find_lane_change_A_to_F_start_at_BL)
226 : : {
227 [ + - + - ]: 1 : auto mapMatchedPositionStart = getPOIMapMatchedPosition("A");
228 : : // ASSERT_NE(match::MapMatchedPosition(), mapMatchedPositionStart);
229 : :
230 [ + - + - ]: 1 : auto mapMatchedPositionEnd = getPOIMapMatchedPosition("F");
231 : : // ASSERT_NE(match::MapMatchedPosition(), mapMatchedPositionEnd);
232 : :
233 [ + - + - ]: 1 : auto currentPosition = getPOIMapMatchedPosition("BL");
234 : :
235 : : auto route = route::planning::planRoute(mapMatchedPositionStart.lanePoint.paraPoint,
236 [ + - ]: 1 : mapMatchedPositionEnd.lanePoint.paraPoint);
237 : :
238 : : // This function needs to be tested:
239 [ + - ]: 1 : auto findLaneChangeResult = route::findFirstLaneChange(currentPosition, route);
240 : :
241 : : // The query route should be the route given
242 [ + - - + : 1 : ASSERT_EQ(&findLaneChangeResult.queryRoute, &route);
- - - - -
- ]
243 : :
244 : : // There is no lane change on the route
245 [ + - - + : 1 : ASSERT_FALSE(findLaneChangeResult.isValid());
- - - - -
- - - ]
246 [ + - - + : 1 : ASSERT_EQ(findLaneChangeResult.laneChangeStartRouteIterator, std::end(route.roadSegments));
- - - - -
- ]
247 [ + - - + : 1 : ASSERT_EQ(findLaneChangeResult.laneChangeEndRouteIterator, std::end(route.roadSegments));
- - - - -
- ]
248 [ + - - + : 1 : ASSERT_EQ(findLaneChangeResult.laneChangeDirection, route::LaneChangeDirection::Invalid);
- - - - -
- ]
249 : : }
250 : :
251 : 2 : TEST_F(LaneChangeTest, find_lane_change_BL_to_F_start_at_BR)
252 : : {
253 [ + - + - ]: 1 : auto mapMatchedPositionStart = getPOIMapMatchedPosition("BL");
254 : : // ASSERT_NE(match::MapMatchedPosition(), mapMatchedPositionStart);
255 [ + - + - ]: 1 : auto mapMatchedPositionEnd = getPOIMapMatchedPosition("F");
256 : : // ASSERT_NE(match::MapMatchedPosition(), mapMatchedPositionEnd);
257 [ + - + - ]: 1 : auto currentPosition = getPOIMapMatchedPosition("BR");
258 : : // ASSERT_NE(match::MapMatchedPosition(), currentPosition);
259 : :
260 : : auto route = route::planning::planRoute(mapMatchedPositionStart.lanePoint.paraPoint,
261 [ + - ]: 1 : mapMatchedPositionEnd.lanePoint.paraPoint);
262 : :
263 : : // This function needs to be tested:
264 [ + - ]: 1 : auto findLaneChangeResult = route::findFirstLaneChange(currentPosition, route);
265 : :
266 : : // The query route should be the route given
267 [ + - - + : 1 : ASSERT_EQ(&findLaneChangeResult.queryRoute, &route);
- - - - -
- ]
268 : :
269 : : // The lane change ends at the very first segment (happens on b only)
270 [ + - ]: 1 : testsLaneChangeEnd(findLaneChangeResult, 0);
271 : :
272 [ + - ]: 1 : testsLaneChangeStart(findLaneChangeResult, 0);
273 : :
274 : : // There is a lane change on the route from (b left to b right)
275 [ + - - + : 1 : ASSERT_TRUE(findLaneChangeResult.isValid());
- - - - -
- - - ]
276 : :
277 [ + - - + : 1 : ASSERT_EQ(findLaneChangeResult.laneChangeDirection, route::LaneChangeDirection::RightToLeft);
- - - - -
- ]
278 : :
279 : 1 : physics::Distance dis;
280 [ + - ]: 1 : dis = findLaneChangeResult.calcZoneLength();
281 [ + - - + : 1 : ASSERT_NEAR((double)dis, 8.2828, 0.0001);
- - - - -
- ]
282 : : }
|
