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/intersection/Intersection.hpp>
11 : : #include <ad/map/landmark/LandmarkOperation.hpp>
12 : : #include <ad/map/lane/LaneOperation.hpp>
13 : : #include <ad/map/lane/Types.hpp>
14 : : #include <ad/map/match/AdMapMatching.hpp>
15 : : #include <ad/map/point/Types.hpp>
16 : : #include <ad/map/route/Planning.hpp>
17 : : #include <ad/map/route/RouteOperation.hpp>
18 : : #include <algorithm>
19 : :
20 : : #include "MapSetup.hpp"
21 : :
22 : : #include <gtest/gtest.h>
23 : :
24 : : namespace ad {
25 : : namespace map {
26 : :
27 : : using namespace point;
28 : : struct IntersectionTest : ::testing::Test
29 : : {
30 : 10 : virtual void SetUp()
31 : : {
32 : 10 : access::cleanup();
33 : 10 : }
34 : :
35 : 10 : virtual void TearDown()
36 : : {
37 : 10 : access::cleanup();
38 : 10 : }
39 : :
40 : 20 : void compareLists(lane::LaneIdSet ids, lane::LaneIdSet otherIds) const
41 : : {
42 [ + - ]: 20 : std::vector<lane::LaneId> idVector(ids.begin(), ids.end());
43 [ + - ]: 20 : std::vector<lane::LaneId> otherIdVector(otherIds.begin(), otherIds.end());
44 : :
45 [ + - ]: 20 : std::sort(idVector.begin(), idVector.end());
46 [ + - ]: 20 : std::sort(otherIdVector.begin(), otherIdVector.end());
47 [ + - - + : 20 : ASSERT_EQ(idVector.size(), otherIdVector.size());
- - - - -
- ]
48 : :
49 [ + + ]: 125 : for (uint32_t i = 0; i < idVector.size(); i++)
50 : : {
51 [ + - - + : 105 : ASSERT_EQ(idVector[i], otherIdVector[i]);
- - - - -
- ]
52 : : }
53 : : }
54 : :
55 : 15 : void compareParaLists(point::ParaPointList resultParaPoints, point::ParaPointList expectedParaPoints)
56 : : {
57 [ + - ]: 15 : std::vector<point::ParaPoint> idVector(resultParaPoints.begin(), resultParaPoints.end());
58 [ + - ]: 15 : std::vector<point::ParaPoint> otherIdVector(expectedParaPoints.begin(), expectedParaPoints.end());
59 : :
60 : : auto compareParaWithLaneId
61 : 169 : = [](point::ParaPoint &left, point::ParaPoint &right) { return left.laneId < right.laneId; };
62 [ + - ]: 15 : std::sort(idVector.begin(), idVector.end(), compareParaWithLaneId);
63 [ + - ]: 15 : std::sort(otherIdVector.begin(), otherIdVector.end(), compareParaWithLaneId);
64 [ + - - + : 15 : ASSERT_EQ(idVector.size(), otherIdVector.size());
- - - - -
- ]
65 : :
66 [ + + ]: 66 : for (uint32_t i = 0; i < idVector.size(); i++)
67 : : {
68 [ + - - + : 51 : ASSERT_EQ(idVector[i], otherIdVector[i]);
- - - - -
- ]
69 : : }
70 : : }
71 : : };
72 : :
73 : 2 : TEST_F(IntersectionTest, create_core_intersection_all_way_stop_all)
74 : : {
75 [ + - + - : 1 : ASSERT_NO_THROW(::map_setup::prepareMapAllWayStop());
+ - + - +
- - - - -
- - - - ]
76 [ + - ]: 1 : auto coreIntersections = ad::map::intersection::CoreIntersection::getCoreIntersectionsForMap();
77 [ + - - + : 1 : ASSERT_EQ(1u, coreIntersections.size());
- - - - -
- ]
78 : : }
79 : :
80 : 2 : TEST_F(IntersectionTest, create_core_intersection_town01_all)
81 : : {
82 [ + - + - : 1 : ASSERT_NO_THROW(::map_setup::prepareMapTown01PrioRight());
+ - + - +
- - - - -
- - - - ]
83 [ + - ]: 1 : auto coreIntersections = ad::map::intersection::CoreIntersection::getCoreIntersectionsForMap();
84 [ + - - + : 1 : ASSERT_EQ(12u, coreIntersections.size());
- - - - -
- ]
85 : : }
86 : :
87 : 2 : TEST_F(IntersectionTest, create_core_intersection_town01_from_point_inside_intersection)
88 : : {
89 [ + - + - : 1 : ASSERT_NO_THROW(::map_setup::prepareMapTown01PrioRight());
+ - + - +
- - - - -
- - - - ]
90 : 0 : auto coreIntersections = ad::map::intersection::CoreIntersection::getCoreIntersectionsForInLaneMatches(point::toENU(
91 [ + - + - ]: 1 : point::createGeoPoint(point::Longitude(0.000815115), point::Latitude(0.0000102114), point::Altitude(0))));
92 [ + - - + : 1 : ASSERT_EQ(1u, coreIntersections.size());
- - - - -
- ]
93 : : }
94 : :
95 : 2 : TEST_F(IntersectionTest, create_core_intersection_town01_from_point_outside_intersection)
96 : : {
97 [ + - + - : 1 : ASSERT_NO_THROW(::map_setup::prepareMapTown01PrioRight());
+ - + - +
- - - - -
- - - - ]
98 : 0 : auto coreIntersections = ad::map::intersection::CoreIntersection::getCoreIntersectionsForInLaneMatches(point::toENU(
99 [ + - + - ]: 1 : point::createGeoPoint(point::Longitude(0.0032321), point::Latitude(0.0000165612), point::Altitude(0))));
100 [ + - - + : 1 : ASSERT_EQ(0u, coreIntersections.size());
- - - - -
- ]
101 : : }
102 : :
103 : 2 : TEST_F(IntersectionTest, create_intersections_all_way_stop)
104 : : {
105 : : using point::createGeoPoint;
106 : : using namespace point;
107 [ + - + - : 1 : ASSERT_NO_THROW(::map_setup::prepareMapAllWayStop());
+ - + - +
- - - - -
- - - - ]
108 : 1 : auto fromWest = createGeoPoint(Longitude(8.437270), Latitude(49.018665), Altitude(0.));
109 [ + - ]: 1 : lane::LaneId fromWestId = lane::uniqueLaneId(fromWest);
110 : 1 : auto toEast = createGeoPoint(Longitude(8.437818), Latitude(49.018672), Altitude(0.));
111 [ + - ]: 1 : lane::LaneId toEastId = lane::uniqueLaneId(toEast);
112 [ + - - + : 1 : ASSERT_NE(fromWestId, toEastId);
- - - - -
- ]
113 : 1 : point::ParaPoint routeStart(point::createParaPoint(fromWestId, physics::ParametricValue(0.8)));
114 : 1 : point::ParaPoint routeEnd(point::createParaPoint(toEastId, physics::ParametricValue(0.5)));
115 [ + - ]: 1 : auto fullRoute = route::planning::planRoute(routeStart, routeEnd);
116 : :
117 [ + - ]: 1 : auto intersections = intersection::Intersection::getIntersectionsForRoute(fullRoute);
118 [ + - - + : 1 : ASSERT_EQ(1u, intersections.size());
- - - - -
- ]
119 : 1 : auto const &intersection = intersections[0];
120 [ + - ]: 1 : auto iType = intersection->intersectionType();
121 [ + - - + : 1 : ASSERT_EQ(intersection::IntersectionType::AllWayStop, iType);
- - - - -
- ]
122 [ + - + - : 1 : ASSERT_EQ(1u, intersection->outgoingParaPointsOnRoute().size());
- + - - -
- - - ]
123 : :
124 [ + - ]: 1 : lane::LaneId laneId = intersection->outgoingParaPointsOnRoute()[0].laneId;
125 [ + - - + : 1 : ASSERT_EQ(toEastId, laneId);
- - - - -
- ]
126 [ + - ]: 1 : ::ad::physics::ParametricValue parametricOffset = intersection->outgoingParaPointsOnRoute()[0].parametricOffset;
127 [ + - - + : 1 : ASSERT_EQ(physics::ParametricValue(0.), parametricOffset);
- - - - -
- ]
128 [ + - + - ]: 1 : auto incoming = intersection->incomingLanes();
129 [ + - - + : 1 : ASSERT_EQ(3u, incoming.size());
- - - - -
- ]
130 [ + - ]: 1 : lane::LaneIdSet expectedIncoming{lane::LaneId{248}, lane::LaneId{252}, lane::LaneId{264}};
131 [ + - + - : 1 : compareLists(incoming, expectedIncoming);
+ - ]
132 [ + - + - ]: 1 : auto internal = intersection->internalLanes();
133 [ + - - + : 1 : ASSERT_EQ(12u, internal.size());
- - - - -
- ]
134 : : lane::LaneIdSet expectedInternal{lane::LaneId{63},
135 : : lane::LaneId{76},
136 : : lane::LaneId{98},
137 : : lane::LaneId{110},
138 : : lane::LaneId{132},
139 : : lane::LaneId{144},
140 : : lane::LaneId{165},
141 : : lane::LaneId{176},
142 : : lane::LaneId{197},
143 : : lane::LaneId{208},
144 : : lane::LaneId{229},
145 [ + - ]: 1 : lane::LaneId{240}};
146 [ + - + - : 1 : compareLists(expectedInternal, internal);
+ - ]
147 [ + - + - ]: 1 : auto crossingLanes = intersection->crossingLanes();
148 [ + - - + : 1 : ASSERT_EQ(6u, crossingLanes.size());
- - - - -
- ]
149 : :
150 : 1 : lane::LaneIdSet resultLaneIdSets, expectedLaneIdSets;
151 [ + - + - ]: 1 : resultLaneIdSets = intersection->lanesOnRoute();
152 [ + - ]: 1 : expectedLaneIdSets = lane::LaneIdSet({lane::LaneId{110}});
153 [ + - + - : 1 : compareLists(resultLaneIdSets, expectedLaneIdSets);
+ - ]
154 : :
155 [ + - + - ]: 1 : resultLaneIdSets = intersection->incomingLanesOnRoute();
156 [ + - ]: 1 : expectedLaneIdSets = lane::LaneIdSet({lane::LaneId{268}});
157 [ + - + - : 1 : compareLists(resultLaneIdSets, expectedLaneIdSets);
+ - ]
158 : :
159 [ + - + - ]: 1 : resultLaneIdSets = intersection->outgoingLanesOnRoute();
160 [ + - ]: 1 : expectedLaneIdSets = lane::LaneIdSet({lane::LaneId{244}});
161 [ + - + - : 1 : compareLists(resultLaneIdSets, expectedLaneIdSets);
+ - ]
162 : :
163 [ + - + - ]: 1 : resultLaneIdSets = intersection->internalLanesWithLowerPriority();
164 : : expectedLaneIdSets
165 [ + - ]: 1 : = lane::LaneIdSet({lane::LaneId{63}, lane::LaneId{98}, lane::LaneId{165}, lane::LaneId{176}, lane::LaneId{229}});
166 [ + - + - : 1 : compareLists(resultLaneIdSets, expectedLaneIdSets);
+ - ]
167 : :
168 [ + - + - ]: 1 : resultLaneIdSets = intersection->outgoingLanes();
169 [ + - ]: 1 : expectedLaneIdSets = lane::LaneIdSet({lane::LaneId{244}, lane::LaneId{256}, lane::LaneId{260}, lane::LaneId{272}});
170 [ + - + - : 1 : compareLists(resultLaneIdSets, expectedLaneIdSets);
+ - ]
171 : :
172 [ + - + - ]: 1 : resultLaneIdSets = intersection->entryLanes();
173 [ + - ]: 1 : expectedLaneIdSets = lane::LaneIdSet({lane::LaneId{248}, lane::LaneId{252}, lane::LaneId{264}, lane::LaneId{268}});
174 [ + - + - : 1 : compareLists(resultLaneIdSets, expectedLaneIdSets);
+ - ]
175 : :
176 : 1 : physics::Speed speed;
177 [ + - ]: 1 : speed = intersection->getSpeedLimit();
178 [ + - - + : 1 : ASSERT_NEAR((double)speed, 1000, 0.001);
- - - - -
- ]
179 : :
180 [ + - + - : 1 : ASSERT_EQ(1u, intersection->outgoingParaPointsOnRoute().size());
- + - - -
- - - ]
181 [ + - ]: 1 : laneId = intersection->outgoingParaPointsOnRoute()[0].laneId;
182 [ + - + - : 1 : ASSERT_EQ(lane::uniqueLaneId(toEast), laneId);
- + - - -
- - - ]
183 [ + - ]: 1 : parametricOffset = intersection->outgoingParaPointsOnRoute()[0].parametricOffset;
184 [ + - - + : 1 : ASSERT_EQ(physics::ParametricValue(0.), parametricOffset);
- - - - -
- ]
185 : :
186 : 1 : point::ParaPoint paraPoint;
187 : 1 : point::ParaPointList resultParaPoints, expectedParaPoints;
188 : :
189 [ + - + - ]: 1 : resultParaPoints = intersection->paraPointsOnRoute();
190 : 1 : expectedParaPoints.clear();
191 : 1 : paraPoint.laneId = lane::LaneId(110);
192 : 1 : paraPoint.parametricOffset = physics::ParametricValue(0);
193 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
194 [ + - + - : 1 : compareParaLists(resultParaPoints, expectedParaPoints);
+ - ]
195 : :
196 [ + - + - ]: 1 : resultParaPoints = intersection->incomingParaPointsOnRoute();
197 : 1 : expectedParaPoints.clear();
198 : 1 : paraPoint.laneId = lane::LaneId(268);
199 : 1 : paraPoint.parametricOffset = physics::ParametricValue(1);
200 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
201 [ + - + - : 1 : compareParaLists(resultParaPoints, expectedParaPoints);
+ - ]
202 : :
203 [ + - + - ]: 1 : resultParaPoints = intersection->outgoingParaPoints();
204 : 1 : expectedParaPoints.clear();
205 : 1 : paraPoint.laneId = lane::LaneId(272);
206 : 1 : paraPoint.parametricOffset = physics::ParametricValue(1);
207 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
208 : 1 : paraPoint.laneId = lane::LaneId(256);
209 : 1 : paraPoint.parametricOffset = physics::ParametricValue(1);
210 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
211 : 1 : paraPoint.laneId = lane::LaneId(260);
212 : 1 : paraPoint.parametricOffset = physics::ParametricValue(0);
213 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
214 : 1 : paraPoint.laneId = lane::LaneId(244);
215 : 1 : paraPoint.parametricOffset = physics::ParametricValue(0);
216 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
217 [ + - + - : 1 : compareParaLists(resultParaPoints, expectedParaPoints);
+ - ]
218 : :
219 [ + - + - ]: 1 : resultParaPoints = intersection->entryParaPoints();
220 : 1 : expectedParaPoints.clear();
221 : 1 : paraPoint.laneId = lane::LaneId(264);
222 : 1 : paraPoint.parametricOffset = physics::ParametricValue(0);
223 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
224 : 1 : paraPoint.laneId = lane::LaneId(252);
225 : 1 : paraPoint.parametricOffset = physics::ParametricValue(1);
226 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
227 : 1 : paraPoint.laneId = lane::LaneId(268);
228 : 1 : paraPoint.parametricOffset = physics::ParametricValue(1);
229 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
230 : 1 : paraPoint.laneId = lane::LaneId(248);
231 : 1 : paraPoint.parametricOffset = physics::ParametricValue(0);
232 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
233 [ + - + - : 1 : compareParaLists(resultParaPoints, expectedParaPoints);
+ - ]
234 : :
235 [ + - + - ]: 1 : resultParaPoints = intersection->paraPointsOnRoute();
236 : 1 : expectedParaPoints.clear();
237 : 1 : paraPoint.laneId = lane::LaneId(110);
238 : 1 : paraPoint.parametricOffset = physics::ParametricValue(0);
239 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
240 [ + - + - : 1 : compareParaLists(resultParaPoints, expectedParaPoints);
+ - ]
241 : :
242 : 1 : route::RouteParaPoint routeParaPoint;
243 [ + - ]: 1 : routeParaPoint = intersection->getIntersectionStartOnRoute();
244 [ + - - + : 1 : ASSERT_NEAR((double)routeParaPoint.parametricOffset, 0., 0.001);
- - - - -
- ]
245 : :
246 : : route::RoutePlanningCounter routeCounter;
247 [ + - ]: 1 : routeCounter = intersection->getRoutePlanningCounter();
248 [ + - - + : 1 : ASSERT_EQ(routeCounter, 1u);
- - - - -
- ]
249 : : route::SegmentCounter segmentCounter;
250 [ + - ]: 1 : segmentCounter = intersection->getRouteSegmentCountFromDestination();
251 [ + - - + : 1 : ASSERT_EQ(segmentCounter, 2u);
- - - - -
- ]
252 [ + - ]: 1 : intersection->updateRouteCounters(4, 5);
253 [ + - ]: 1 : routeCounter = intersection->getRoutePlanningCounter();
254 [ + - - + : 1 : ASSERT_EQ(routeCounter, 4u);
- - - - -
- ]
255 [ + - ]: 1 : segmentCounter = intersection->getRouteSegmentCountFromDestination();
256 [ + - - + : 1 : ASSERT_EQ(segmentCounter, 5u);
- - - - -
- ]
257 [ + - ]: 1 : intersection->updateRouteCounters(1, 2);
258 : :
259 : 1 : match::MapMatchedObjectBoundingBox object;
260 : 1 : match::LaneOccupiedRegion region;
261 : :
262 : 1 : object.laneOccupiedRegions.clear();
263 : 1 : region.laneId = lane::LaneId(110);
264 [ + - ]: 1 : object.laneOccupiedRegions.push_back(region);
265 [ + - - + : 1 : ASSERT_TRUE(intersection->objectOnIntersectionRoute(object));
- - - - -
- - - ]
266 : :
267 : 1 : object.laneOccupiedRegions.clear();
268 : 1 : region.laneId = lane::LaneId(132);
269 [ + - ]: 1 : object.laneOccupiedRegions.push_back(region);
270 [ + - - + : 1 : ASSERT_TRUE(intersection->objectOnInternalLaneWithHigherPriority(object));
- - - - -
- - - ]
271 : :
272 : 1 : object.laneOccupiedRegions.clear();
273 : 1 : region.laneId = lane::LaneId(248);
274 [ + - ]: 1 : object.laneOccupiedRegions.push_back(region);
275 [ + - - + : 1 : ASSERT_TRUE(intersection->objectOnIncomingLaneWithHigherPriority(object));
- - - - -
- - - ]
276 : :
277 : 1 : object.laneOccupiedRegions.clear();
278 : 1 : region.laneId = lane::LaneId(132);
279 [ + - ]: 1 : object.laneOccupiedRegions.push_back(region);
280 [ + - - + : 1 : ASSERT_TRUE(intersection->objectOnCrossingLane(object));
- - - - -
- - - ]
281 : : }
282 : :
283 : 2 : TEST_F(IntersectionTest, create_intersection_from_route_ending_within_intersection)
284 : : {
285 [ + - + - : 1 : ASSERT_NO_THROW(::map_setup::prepareMapAllWayStop());
+ - + - +
- - - - -
- - - - ]
286 : 1 : point::ParaPoint incomingLane(point::createParaPoint(lane::LaneId{268}, physics::ParametricValue(0.8)));
287 : 1 : point::ParaPoint laneWithinIntersection(point::createParaPoint(lane::LaneId{110}, physics::ParametricValue(0.5)));
288 [ + - ]: 1 : auto fullRoute = route::planning::planRoute(incomingLane, laneWithinIntersection);
289 [ + - + - : 1 : ASSERT_GT(route::calcLength(fullRoute), physics::Distance(0.));
- + - - -
- - - ]
290 : 1 : std::vector<intersection::IntersectionPtr> intersections;
291 [ + - + - : 1 : ASSERT_NO_THROW(intersections = intersection::Intersection::getIntersectionsForRoute(fullRoute));
+ - + - +
- - - - -
- - - - ]
292 [ + - - + : 1 : ASSERT_EQ(1u, intersections.size());
- - - - -
- ]
293 : : }
294 : :
295 : 2 : TEST_F(IntersectionTest, traffic_lights_pfz_elf_to_rusch)
296 : : {
297 : : using point::createGeoPoint;
298 : : using namespace point;
299 [ + - + - : 1 : ASSERT_NO_THROW(::map_setup::prepareMapTrafficLightsPfz());
+ - + - +
- - - - -
- - - - ]
300 : : // inner lane (1990)
301 : 1 : auto fromSouth = createGeoPoint(Longitude(8.457617), Latitude(49.020345), Altitude(0.));
302 [ + - ]: 1 : lane::LaneId fromSouthId = lane::uniqueLaneId(fromSouth);
303 : : // 1964
304 : 1 : auto toEast = createGeoPoint(Longitude(8.457384), Latitude(49.020785), Altitude(0.));
305 [ + - ]: 1 : lane::LaneId toEastId = lane::uniqueLaneId(toEast);
306 [ + - - + : 1 : ASSERT_NE(fromSouthId, toEastId);
- - - - -
- ]
307 : 1 : point::ParaPoint routeStart(point::createParaPoint(fromSouthId, physics::ParametricValue(0.5)));
308 : 1 : point::ParaPoint routeEnd(point::createParaPoint(toEastId, physics::ParametricValue(0.5)));
309 [ + - ]: 1 : auto fullRoute = route::planning::planRoute(routeStart, routeEnd);
310 : : // contain left-straight arrows type traffic light
311 : : // no additional lanes with higher priorities
312 [ + - ]: 1 : auto intersections = intersection::Intersection::getIntersectionsForRoute(fullRoute);
313 [ + - - + : 1 : ASSERT_EQ(1u, intersections.size());
- - - - -
- ]
314 : 1 : auto const &intersection = intersections[0];
315 [ + - ]: 1 : auto iType = intersection->intersectionType();
316 [ + - - + : 1 : ASSERT_EQ(intersection::IntersectionType::TrafficLight, iType);
- - - - -
- ]
317 [ + - ]: 1 : auto const &priorityLanes = intersection->incomingParaPointsWithHigherPriority();
318 [ + - - + : 1 : ASSERT_EQ(priorityLanes.size(), 0u);
- - - - -
- ]
319 [ + - ]: 1 : auto const &lights = intersection->applicableTrafficLights();
320 [ + - - + : 1 : ASSERT_EQ(2u, lights.size());
- - - - -
- ]
321 : 1 : point::GeoPoint geoPoint;
322 : 1 : landmark::LandmarkId id1, id2;
323 : 1 : geoPoint = point::createGeoPoint(point::Longitude(8.457552564), point::Latitude(49.02053703), point::Altitude(3.));
324 [ + - ]: 1 : id1 = landmark::uniqueLandmarkId(geoPoint);
325 : 1 : geoPoint = point::createGeoPoint(point::Longitude(8.457664809), point::Latitude(49.02051672), point::Altitude(5.));
326 [ + - ]: 1 : id2 = landmark::uniqueLandmarkId(geoPoint);
327 [ + - + - : 1 : ASSERT_EQ(1u, lights.count(id1));
- + - - -
- - - ]
328 [ + - + - : 1 : ASSERT_EQ(1u, lights.count(id2));
- + - - -
- - - ]
329 : :
330 : 1 : lane::LaneIdSet resultLaneIdSets, expectedLaneIdSets;
331 [ + - + - ]: 1 : resultLaneIdSets = intersection->lanesOnRoute();
332 [ + - ]: 1 : expectedLaneIdSets = lane::LaneIdSet({lane::LaneId{360}, lane::LaneId{621}, lane::LaneId{624}});
333 [ + - + - : 1 : compareLists(resultLaneIdSets, expectedLaneIdSets);
+ - ]
334 : :
335 [ + - + - ]: 1 : resultLaneIdSets = intersection->incomingLanesOnRoute();
336 [ + - ]: 1 : expectedLaneIdSets = lane::LaneIdSet({lane::LaneId{588}, lane::LaneId{598}});
337 [ + - + - : 1 : compareLists(resultLaneIdSets, expectedLaneIdSets);
+ - ]
338 : :
339 [ + - + - ]: 1 : resultLaneIdSets = intersection->outgoingLanesOnRoute();
340 [ + - ]: 1 : expectedLaneIdSets = lane::LaneIdSet({lane::LaneId{471}});
341 [ + - + - : 1 : compareLists(resultLaneIdSets, expectedLaneIdSets);
+ - ]
342 : :
343 [ + - + - ]: 1 : resultLaneIdSets = intersection->internalLanesWithLowerPriority();
344 [ + - ]: 2 : expectedLaneIdSets = lane::LaneIdSet({lane::LaneId{187},
345 : : lane::LaneId{199},
346 : : lane::LaneId{220},
347 : : lane::LaneId{231},
348 : : lane::LaneId{253},
349 : : lane::LaneId{275},
350 : : lane::LaneId{297},
351 : : lane::LaneId{318},
352 : : lane::LaneId{339},
353 : : lane::LaneId{382},
354 : : lane::LaneId{404},
355 : : lane::LaneId{425},
356 : : lane::LaneId{446},
357 : : lane::LaneId{467},
358 : : lane::LaneId{475},
359 : : lane::LaneId{479},
360 : : lane::LaneId{511},
361 : : lane::LaneId{514},
362 : 1 : lane::LaneId{552}});
363 [ + - + - : 1 : compareLists(resultLaneIdSets, expectedLaneIdSets);
+ - ]
364 : :
365 [ + - + - ]: 1 : resultLaneIdSets = intersection->outgoingLanes();
366 : : expectedLaneIdSets
367 [ + - ]: 1 : = lane::LaneIdSet({lane::LaneId{471}, lane::LaneId{508}, lane::LaneId{548}, lane::LaneId{627}, lane::LaneId{630}});
368 [ + - + - : 1 : compareLists(resultLaneIdSets, expectedLaneIdSets);
+ - ]
369 : :
370 [ + - + - ]: 1 : resultLaneIdSets = intersection->entryLanes();
371 [ + - ]: 2 : expectedLaneIdSets = lane::LaneIdSet({lane::LaneId{487},
372 : : lane::LaneId{491},
373 : : lane::LaneId{522},
374 : : lane::LaneId{526},
375 : : lane::LaneId{578},
376 : : lane::LaneId{588},
377 : 1 : lane::LaneId{598}});
378 [ + - + - : 1 : compareLists(resultLaneIdSets, expectedLaneIdSets);
+ - ]
379 : :
380 : 1 : point::ParaPointList resultParaPoints, expectedParaPoints;
381 : 1 : point::ParaPoint paraPoint;
382 : :
383 [ + - + - ]: 1 : resultParaPoints = intersection->paraPointsOnRoute();
384 : 1 : expectedParaPoints.clear();
385 : 1 : paraPoint.laneId = lane::LaneId(360);
386 : 1 : paraPoint.parametricOffset = physics::ParametricValue(1);
387 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
388 : 1 : paraPoint.laneId = lane::LaneId(621);
389 : 1 : paraPoint.parametricOffset = physics::ParametricValue(1);
390 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
391 : 1 : paraPoint.laneId = lane::LaneId(624);
392 : 1 : paraPoint.parametricOffset = physics::ParametricValue(1);
393 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
394 [ + - + - : 1 : compareParaLists(resultParaPoints, expectedParaPoints);
+ - ]
395 : :
396 [ + - + - ]: 1 : resultParaPoints = intersection->incomingParaPointsOnRoute();
397 : 1 : expectedParaPoints.clear();
398 : 1 : paraPoint.laneId = lane::LaneId(588);
399 : 1 : paraPoint.parametricOffset = physics::ParametricValue(1);
400 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
401 : 1 : paraPoint.laneId = lane::LaneId(598);
402 : 1 : paraPoint.parametricOffset = physics::ParametricValue(1);
403 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
404 [ + - + - : 1 : compareParaLists(resultParaPoints, expectedParaPoints);
+ - ]
405 : :
406 [ + - + - ]: 1 : resultParaPoints = intersection->outgoingParaPoints();
407 : 1 : expectedParaPoints.clear();
408 : 1 : paraPoint.laneId = lane::LaneId(508);
409 : 1 : paraPoint.parametricOffset = physics::ParametricValue(0);
410 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
411 : 1 : paraPoint.laneId = lane::LaneId(627);
412 : 1 : paraPoint.parametricOffset = physics::ParametricValue(1);
413 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
414 : 1 : paraPoint.laneId = lane::LaneId(548);
415 : 1 : paraPoint.parametricOffset = physics::ParametricValue(0);
416 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
417 : 1 : paraPoint.laneId = lane::LaneId(471);
418 : 1 : paraPoint.parametricOffset = physics::ParametricValue(0);
419 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
420 : 1 : paraPoint.laneId = lane::LaneId(630);
421 : 1 : paraPoint.parametricOffset = physics::ParametricValue(1);
422 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
423 [ + - + - : 1 : compareParaLists(resultParaPoints, expectedParaPoints);
+ - ]
424 : :
425 [ + - + - ]: 1 : resultParaPoints = intersection->entryParaPoints();
426 : 1 : expectedParaPoints.clear();
427 : 1 : paraPoint.laneId = lane::LaneId(487);
428 : 1 : paraPoint.parametricOffset = physics::ParametricValue(0);
429 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
430 : 1 : paraPoint.laneId = lane::LaneId(578);
431 : 1 : paraPoint.parametricOffset = physics::ParametricValue(0);
432 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
433 : 1 : paraPoint.laneId = lane::LaneId(522);
434 : 1 : paraPoint.parametricOffset = physics::ParametricValue(0);
435 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
436 : 1 : paraPoint.laneId = lane::LaneId(588);
437 : 1 : paraPoint.parametricOffset = physics::ParametricValue(1);
438 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
439 : 1 : paraPoint.laneId = lane::LaneId(598);
440 : 1 : paraPoint.parametricOffset = physics::ParametricValue(1);
441 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
442 : 1 : paraPoint.laneId = lane::LaneId(491);
443 : 1 : paraPoint.parametricOffset = physics::ParametricValue(0);
444 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
445 : 1 : paraPoint.laneId = lane::LaneId(526);
446 : 1 : paraPoint.parametricOffset = physics::ParametricValue(0);
447 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
448 [ + - + - : 1 : compareParaLists(resultParaPoints, expectedParaPoints);
+ - ]
449 : :
450 [ + - + - ]: 1 : resultParaPoints = intersection->paraPointsOnRoute();
451 : 1 : expectedParaPoints.clear();
452 : 1 : paraPoint.laneId = lane::LaneId(360);
453 : 1 : paraPoint.parametricOffset = physics::ParametricValue(1);
454 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
455 : 1 : paraPoint.laneId = lane::LaneId(621);
456 : 1 : paraPoint.parametricOffset = physics::ParametricValue(1);
457 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
458 : 1 : paraPoint.laneId = lane::LaneId(624);
459 : 1 : paraPoint.parametricOffset = physics::ParametricValue(1);
460 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
461 [ + - + - : 1 : compareParaLists(resultParaPoints, expectedParaPoints);
+ - ]
462 : :
463 : 1 : physics::Speed speed;
464 [ + - ]: 1 : speed = intersection->getSpeedLimit();
465 [ + - - + : 1 : ASSERT_NEAR((double)speed, 13.888, 0.001);
- - - - -
- ]
466 : :
467 : : route::RoutePlanningCounter routeCounter;
468 [ + - ]: 1 : routeCounter = intersection->getRoutePlanningCounter();
469 [ + - - + : 1 : ASSERT_EQ(fullRoute.routePlanningCounter, routeCounter);
- - - - -
- ]
470 : : route::SegmentCounter segmentCounter;
471 [ + - ]: 1 : segmentCounter = intersection->getRouteSegmentCountFromDestination();
472 [ + - - + : 1 : ASSERT_EQ(segmentCounter, 4u);
- - - - -
- ]
473 [ + - ]: 1 : intersection->updateRouteCounters(4, 5);
474 [ + - ]: 1 : routeCounter = intersection->getRoutePlanningCounter();
475 [ + - - + : 1 : ASSERT_EQ(routeCounter, 4u);
- - - - -
- ]
476 [ + - ]: 1 : segmentCounter = intersection->getRouteSegmentCountFromDestination();
477 [ + - - + : 1 : ASSERT_EQ(segmentCounter, 5u);
- - - - -
- ]
478 [ + - ]: 1 : intersection->updateRouteCounters(2, 4);
479 : : }
480 : :
481 : 2 : TEST_F(IntersectionTest, traffic_lights_pfz_rusch_to_elf)
482 : : {
483 : : using point::createGeoPoint;
484 : : using namespace point;
485 [ + - + - : 1 : ASSERT_NO_THROW(::map_setup::prepareMapTrafficLightsPfz());
+ - + - +
- - - - -
- - - - ]
486 : : // inner lane (1990)
487 : 1 : auto fromSouth = createGeoPoint(Longitude(8.457617), Latitude(49.020345), Altitude(0.));
488 [ + - ]: 1 : lane::LaneId fromSouthId = lane::uniqueLaneId(fromSouth);
489 : : // 1912
490 : 1 : auto toWest = createGeoPoint(Longitude(8.458091), Latitude(49.020521), Altitude(0.));
491 [ + - ]: 1 : lane::LaneId toWestId = lane::uniqueLaneId(toWest);
492 : 1 : point::ParaPoint routeStart(point::createParaPoint(fromSouthId, physics::ParametricValue(0.5)));
493 : 1 : point::ParaPoint routeEnd(point::createParaPoint(toWestId, physics::ParametricValue(0.5)));
494 [ + - ]: 1 : auto fullRoute = route::planning::planRoute(routeStart, routeEnd);
495 : : // contain right arrow type traffic light
496 : : // no additional lanes with higher priorities
497 [ + - ]: 1 : auto intersections = intersection::Intersection::getIntersectionsForRoute(fullRoute);
498 [ + - - + : 1 : ASSERT_EQ(1u, intersections.size());
- - - - -
- ]
499 : 1 : auto const &intersection = intersections[0];
500 [ + - ]: 1 : auto iType = intersection->intersectionType();
501 [ + - - + : 1 : ASSERT_EQ(intersection::IntersectionType::TrafficLight, iType);
- - - - -
- ]
502 [ + - ]: 1 : auto const &priorityLanes = intersection->incomingParaPointsWithHigherPriority();
503 [ + - - + : 1 : ASSERT_EQ(priorityLanes.size(), 0u);
- - - - -
- ]
504 [ + - ]: 1 : auto const &lights = intersection->applicableTrafficLights();
505 [ + - - + : 1 : ASSERT_EQ(2u, lights.size());
- - - - -
- ]
506 : 1 : point::GeoPoint geoPoint;
507 : 1 : landmark::LandmarkId id1, id2;
508 : 1 : geoPoint = point::createGeoPoint(point::Longitude(8.457704425), point::Latitude(49.02050956), point::Altitude(5.));
509 [ + - ]: 1 : id1 = landmark::uniqueLandmarkId(geoPoint);
510 : 1 : geoPoint = point::createGeoPoint(point::Longitude(8.457737438), point::Latitude(49.02050359), point::Altitude(3.));
511 [ + - ]: 1 : id2 = landmark::uniqueLandmarkId(geoPoint);
512 [ + - + - : 1 : ASSERT_EQ(1u, lights.count(id1));
- + - - -
- - - ]
513 [ + - + - : 1 : ASSERT_EQ(1u, lights.count(id2));
- + - - -
- - - ]
514 : :
515 : 1 : lane::LaneIdSet resultLaneIdSets, expectedLaneIdSets;
516 [ + - + - ]: 1 : resultLaneIdSets = intersection->lanesOnRoute();
517 [ + - ]: 1 : expectedLaneIdSets = lane::LaneIdSet({lane::LaneId{382}, lane::LaneId{621}, lane::LaneId{624}});
518 [ + - + - : 1 : compareLists(resultLaneIdSets, expectedLaneIdSets);
+ - ]
519 : :
520 [ + - + - ]: 1 : resultLaneIdSets = intersection->incomingLanesOnRoute();
521 [ + - ]: 1 : expectedLaneIdSets = lane::LaneIdSet({lane::LaneId{588}, lane::LaneId{598}});
522 [ + - + - : 1 : compareLists(resultLaneIdSets, expectedLaneIdSets);
+ - ]
523 : :
524 [ + - + - ]: 1 : resultLaneIdSets = intersection->outgoingLanesOnRoute();
525 [ + - ]: 1 : expectedLaneIdSets = lane::LaneIdSet({lane::LaneId{508}});
526 [ + - + - : 1 : compareLists(resultLaneIdSets, expectedLaneIdSets);
+ - ]
527 : :
528 [ + - + - ]: 1 : resultLaneIdSets = intersection->internalLanesWithLowerPriority();
529 [ + - ]: 2 : expectedLaneIdSets = lane::LaneIdSet({lane::LaneId{187},
530 : : lane::LaneId{199},
531 : : lane::LaneId{220},
532 : : lane::LaneId{231},
533 : : lane::LaneId{253},
534 : : lane::LaneId{275},
535 : : lane::LaneId{297},
536 : : lane::LaneId{318},
537 : : lane::LaneId{339},
538 : : lane::LaneId{360},
539 : : lane::LaneId{404},
540 : : lane::LaneId{425},
541 : : lane::LaneId{446},
542 : : lane::LaneId{467},
543 : : lane::LaneId{475},
544 : : lane::LaneId{479},
545 : : lane::LaneId{511},
546 : : lane::LaneId{514},
547 : 1 : lane::LaneId{552}});
548 [ + - + - : 1 : compareLists(resultLaneIdSets, expectedLaneIdSets);
+ - ]
549 : :
550 [ + - + - ]: 1 : resultLaneIdSets = intersection->outgoingLanes();
551 : : expectedLaneIdSets
552 [ + - ]: 1 : = lane::LaneIdSet({lane::LaneId{471}, lane::LaneId{508}, lane::LaneId{548}, lane::LaneId{627}, lane::LaneId{630}});
553 [ + - + - : 1 : compareLists(resultLaneIdSets, expectedLaneIdSets);
+ - ]
554 : :
555 [ + - + - ]: 1 : resultLaneIdSets = intersection->entryLanes();
556 [ + - ]: 2 : expectedLaneIdSets = lane::LaneIdSet({lane::LaneId{487},
557 : : lane::LaneId{491},
558 : : lane::LaneId{522},
559 : : lane::LaneId{526},
560 : : lane::LaneId{578},
561 : : lane::LaneId{588},
562 : 1 : lane::LaneId{598}});
563 [ + - + - : 1 : compareLists(resultLaneIdSets, expectedLaneIdSets);
+ - ]
564 : :
565 : 1 : point::ParaPointList resultParaPoints, expectedParaPoints;
566 : 1 : point::ParaPoint paraPoint;
567 [ + - + - ]: 1 : resultParaPoints = intersection->paraPointsOnRoute();
568 : 1 : expectedParaPoints.clear();
569 : 1 : paraPoint.laneId = lane::LaneId(621);
570 : 1 : paraPoint.parametricOffset = physics::ParametricValue(1);
571 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
572 : 1 : paraPoint.laneId = lane::LaneId(382);
573 : 1 : paraPoint.parametricOffset = physics::ParametricValue(1);
574 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
575 : 1 : paraPoint.laneId = lane::LaneId(624);
576 : 1 : paraPoint.parametricOffset = physics::ParametricValue(1);
577 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
578 [ + - + - : 1 : compareParaLists(resultParaPoints, expectedParaPoints);
+ - ]
579 : :
580 [ + - + - ]: 1 : resultParaPoints = intersection->incomingParaPointsOnRoute();
581 : 1 : expectedParaPoints.clear();
582 : 1 : paraPoint.laneId = lane::LaneId(588);
583 : 1 : paraPoint.parametricOffset = physics::ParametricValue(1);
584 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
585 : 1 : paraPoint.laneId = lane::LaneId(598);
586 : 1 : paraPoint.parametricOffset = physics::ParametricValue(1);
587 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
588 [ + - + - : 1 : compareParaLists(resultParaPoints, expectedParaPoints);
+ - ]
589 : :
590 [ + - + - ]: 1 : resultParaPoints = intersection->outgoingParaPoints();
591 : 1 : expectedParaPoints.clear();
592 : 1 : paraPoint.laneId = lane::LaneId(627);
593 : 1 : paraPoint.parametricOffset = physics::ParametricValue(1);
594 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
595 : 1 : paraPoint.laneId = lane::LaneId(548);
596 : 1 : paraPoint.parametricOffset = physics::ParametricValue(0);
597 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
598 : 1 : paraPoint.laneId = lane::LaneId(471);
599 : 1 : paraPoint.parametricOffset = physics::ParametricValue(0);
600 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
601 : 1 : paraPoint.laneId = lane::LaneId(630);
602 : 1 : paraPoint.parametricOffset = physics::ParametricValue(1);
603 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
604 : 1 : paraPoint.laneId = lane::LaneId(508);
605 : 1 : paraPoint.parametricOffset = physics::ParametricValue(0);
606 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
607 [ + - + - : 1 : compareParaLists(resultParaPoints, expectedParaPoints);
+ - ]
608 : :
609 [ + - + - ]: 1 : resultParaPoints = intersection->entryParaPoints();
610 : 1 : expectedParaPoints.clear();
611 : 1 : paraPoint.laneId = lane::LaneId(487);
612 : 1 : paraPoint.parametricOffset = physics::ParametricValue(0);
613 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
614 : 1 : paraPoint.laneId = lane::LaneId(526);
615 : 1 : paraPoint.parametricOffset = physics::ParametricValue(0);
616 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
617 : 1 : paraPoint.laneId = lane::LaneId(598);
618 : 1 : paraPoint.parametricOffset = physics::ParametricValue(1);
619 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
620 : 1 : paraPoint.laneId = lane::LaneId(522);
621 : 1 : paraPoint.parametricOffset = physics::ParametricValue(0);
622 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
623 : 1 : paraPoint.laneId = lane::LaneId(588);
624 : 1 : paraPoint.parametricOffset = physics::ParametricValue(1);
625 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
626 : 1 : paraPoint.laneId = lane::LaneId(578);
627 : 1 : paraPoint.parametricOffset = physics::ParametricValue(0);
628 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
629 : 1 : paraPoint.laneId = lane::LaneId(491);
630 : 1 : paraPoint.parametricOffset = physics::ParametricValue(0);
631 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
632 [ + - + - : 1 : compareParaLists(resultParaPoints, expectedParaPoints);
+ - ]
633 : :
634 [ + - + - ]: 1 : resultParaPoints = intersection->paraPointsOnRoute();
635 : 1 : expectedParaPoints.clear();
636 : 1 : paraPoint.laneId = lane::LaneId(621);
637 : 1 : paraPoint.parametricOffset = physics::ParametricValue(1);
638 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
639 : 1 : paraPoint.laneId = lane::LaneId(382);
640 : 1 : paraPoint.parametricOffset = physics::ParametricValue(1);
641 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
642 : 1 : paraPoint.laneId = lane::LaneId(624);
643 : 1 : paraPoint.parametricOffset = physics::ParametricValue(1);
644 [ + - ]: 1 : expectedParaPoints.push_back(paraPoint);
645 [ + - + - : 1 : compareParaLists(resultParaPoints, expectedParaPoints);
+ - ]
646 : :
647 : 1 : physics::Speed speed;
648 [ + - ]: 1 : speed = intersection->getSpeedLimit();
649 [ + - - + : 1 : ASSERT_NEAR((double)speed, 13.888, 0.001);
- - - - -
- ]
650 : :
651 : : route::RoutePlanningCounter routeCounter;
652 [ + - ]: 1 : routeCounter = intersection->getRoutePlanningCounter();
653 [ + - - + : 1 : ASSERT_EQ(fullRoute.routePlanningCounter, routeCounter);
- - - - -
- ]
654 : : route::SegmentCounter segmentCounter;
655 [ + - ]: 1 : segmentCounter = intersection->getRouteSegmentCountFromDestination();
656 [ + - - + : 1 : ASSERT_EQ(segmentCounter, 4u);
- - - - -
- ]
657 [ + - ]: 1 : intersection->updateRouteCounters(4, 5);
658 [ + - ]: 1 : routeCounter = intersection->getRoutePlanningCounter();
659 [ + - - + : 1 : ASSERT_EQ(routeCounter, 4u);
- - - - -
- ]
660 [ + - ]: 1 : segmentCounter = intersection->getRouteSegmentCountFromDestination();
661 [ + - - + : 1 : ASSERT_EQ(segmentCounter, 5u);
- - - - -
- ]
662 [ + - ]: 1 : intersection->updateRouteCounters(3, 4);
663 : : }
664 : :
665 : 2 : TEST_F(IntersectionTest, extract_turn_direction)
666 : : {
667 : : using point::createGeoPoint;
668 : : using namespace point;
669 [ + - + - : 1 : ASSERT_NO_THROW(::map_setup::prepareMapTrafficLightsPfz());
+ - + - +
- - - - -
- - - - ]
670 : : // inner lane (1990)
671 : 1 : auto fromSouth = createGeoPoint(Longitude(8.457617), Latitude(49.020345), Altitude(0.));
672 [ + - ]: 1 : lane::LaneId fromSouthId = lane::uniqueLaneId(fromSouth);
673 : 1 : point::ParaPoint routeStart(point::createParaPoint(fromSouthId, physics::ParametricValue(0.5)));
674 : : // left turn
675 : : {
676 : : // 1964
677 : 1 : auto toEast = createGeoPoint(Longitude(8.457384), Latitude(49.020785), Altitude(0.));
678 [ + - ]: 1 : lane::LaneId toEastId = lane::uniqueLaneId(toEast);
679 : 1 : point::ParaPoint routeEnd(point::createParaPoint(toEastId, physics::ParametricValue(0.5)));
680 [ + - ]: 1 : auto fullRoute = route::planning::planRoute(routeStart, routeEnd);
681 [ + - ]: 1 : auto intersections = intersection::Intersection::getIntersectionsForRoute(fullRoute);
682 [ + - - + : 1 : ASSERT_EQ(1u, intersections.size());
- - - - -
- ]
683 : 1 : auto const &intersection = intersections[0];
684 [ + - ]: 1 : auto const &outgoing = intersection->outgoingParaPointsOnRoute();
685 [ + - - + : 1 : ASSERT_EQ(outgoing.size(), 1u);
- - - - -
- ]
686 [ + - + - : 1 : ASSERT_EQ(intersection::TurnDirection::Left, intersection->turnDirection());
- + - - -
- - - ]
687 : : }
688 : : // straight
689 : : {
690 : : // 1944
691 : 1 : auto toNorth = createGeoPoint(Longitude(8.457968), Latitude(49.020877), Altitude(0.));
692 [ + - ]: 1 : lane::LaneId toNorthId = lane::uniqueLaneId(toNorth);
693 : 1 : point::ParaPoint routeEnd(point::createParaPoint(toNorthId, physics::ParametricValue(0.5)));
694 [ + - ]: 1 : auto fullRoute = route::planning::planRoute(routeStart, routeEnd);
695 [ + - ]: 1 : auto intersections = intersection::Intersection::getIntersectionsForRoute(fullRoute);
696 [ + - - + : 1 : ASSERT_EQ(1u, intersections.size());
- - - - -
- ]
697 : 1 : auto const &intersection = intersections[0];
698 [ + - + - : 1 : ASSERT_EQ(intersection::TurnDirection::Straight, intersection->turnDirection());
- + - - -
- - - ]
699 : : }
700 : : // right turn
701 : : {
702 : : // 1912
703 : 1 : auto toWest = createGeoPoint(Longitude(8.458091), Latitude(49.020521), Altitude(0.));
704 [ + - ]: 1 : lane::LaneId toWestId = lane::uniqueLaneId(toWest);
705 : 1 : point::ParaPoint routeEnd(point::createParaPoint(toWestId, physics::ParametricValue(0.5)));
706 [ + - ]: 1 : auto fullRoute = route::planning::planRoute(routeStart, routeEnd);
707 [ + - ]: 1 : auto intersections = intersection::Intersection::getIntersectionsForRoute(fullRoute);
708 [ + - - + : 1 : ASSERT_EQ(1u, intersections.size());
- - - - -
- ]
709 : 1 : auto const &intersection = intersections[0];
710 [ + - + - : 1 : ASSERT_EQ(intersection::TurnDirection::Right, intersection->turnDirection());
- + - - -
- - - ]
711 : : }
712 : : }
713 : :
714 : 2 : TEST_F(IntersectionTest, validate_tpk_to_pfz_map)
715 : : {
716 [ + - + - : 1 : ASSERT_NO_THROW(::map_setup::prepareMapTpkPfzDrive());
+ - + - +
- - - - -
- - - - ]
717 : : // use map-matching to be independent of lane ids
718 : 1 : auto start = point::createGeoPoint(point::Longitude(8.4571128), point::Latitude(49.0169280), point::Altitude(0));
719 : 1 : auto end = point::createGeoPoint(point::Longitude(8.457508), point::Latitude(49.019861), point::Altitude(0));
720 [ + - ]: 1 : match::AdMapMatching mapMatching;
721 [ + - ]: 1 : auto from = mapMatching.getMapMatchedPositions(start, physics::Distance(1), physics::Probability(0.5));
722 [ + - ]: 1 : auto to = mapMatching.getMapMatchedPositions(end, physics::Distance(1), physics::Probability(0.5));
723 [ + - - + : 1 : ASSERT_EQ(from.size(), 1u);
- - - - -
- ]
724 [ + - - + : 1 : ASSERT_EQ(to.size(), 1u);
- - - - -
- ]
725 [ + - ]: 1 : auto fullRoute = route::planning::planRoute(from[0].lanePoint.paraPoint, to[0].lanePoint.paraPoint);
726 [ + - ]: 1 : auto intersections = intersection::Intersection::getIntersectionsForRoute(fullRoute);
727 [ + - - + : 1 : ASSERT_EQ(1u, intersections.size());
- - - - -
- ]
728 : 1 : auto const &intersection = intersections[0];
729 [ + - ]: 1 : auto iType = intersection->intersectionType();
730 [ + - - + : 1 : ASSERT_EQ(intersection::IntersectionType::TrafficLight, iType);
- - - - -
- ]
731 : :
732 : 0 : intersection::IntersectionPtr ptrInter;
733 [ - + - - : 1 : ASSERT_FALSE((bool)ptrInter);
- - - - -
- ]
734 [ + - ]: 1 : ptrInter = intersection->getNextIntersectionOnRoute(fullRoute);
735 [ - + - - : 1 : ASSERT_TRUE((bool)ptrInter);
- - - - -
- ]
736 [ + - + - : 1 : ASSERT_EQ(intersection::IntersectionType::TrafficLight, ptrInter->intersectionType());
- + - - -
- - - ]
737 : :
738 : 1 : match::MapMatchedObjectBoundingBox object;
739 : 1 : match::LaneOccupiedRegion region;
740 [ + - - + : 1 : ASSERT_FALSE(intersection->objectWithinIntersection(object));
- - - - -
- - - ]
741 [ + - - + : 1 : ASSERT_FALSE(intersection->objectOnIncomingLane(object));
- - - - -
- - - ]
742 [ + - - + : 1 : ASSERT_FALSE(intersection->objectOnInternalLaneWithLowerPriority(object));
- - - - -
- - - ]
743 [ + - - + : 1 : ASSERT_FALSE(intersection->objectOnIncomingLaneWithLowerPriority(object));
- - - - -
- - - ]
744 [ + - - + : 1 : ASSERT_FALSE(intersection->objectOnLaneWithLowerPriority(object));
- - - - -
- - - ]
745 [ + - - + : 1 : ASSERT_FALSE(intersection->objectOnInternalLaneWithHigherPriority(object));
- - - - -
- - - ]
746 [ + - - + : 1 : ASSERT_FALSE(intersection->objectOnIncomingLaneWithHigherPriority(object));
- - - - -
- - - ]
747 [ + - - + : 1 : ASSERT_FALSE(intersection->objectOnLaneWithHigherPriority(object));
- - - - -
- - - ]
748 [ + - - + : 1 : ASSERT_FALSE(intersection->objectOnCrossingLane(object));
- - - - -
- - - ]
749 [ + - - + : 1 : ASSERT_FALSE(intersection->objectOnIntersectionRoute(object));
- - - - -
- - - ]
750 : :
751 : 1 : object.laneOccupiedRegions.clear();
752 : 1 : region.laneId = lane::LaneId(7127);
753 [ + - ]: 1 : object.laneOccupiedRegions.push_back(region);
754 [ + - - + : 1 : ASSERT_TRUE(intersection->objectWithinIntersection(object));
- - - - -
- - - ]
755 : :
756 : 1 : object.laneOccupiedRegions.clear();
757 : 1 : region.laneId = lane::LaneId(11514);
758 [ + - ]: 1 : object.laneOccupiedRegions.push_back(region);
759 [ + - - + : 1 : ASSERT_TRUE(intersection->objectOnIncomingLane(object));
- - - - -
- - - ]
760 : :
761 : 1 : object.laneOccupiedRegions.clear();
762 : 1 : region.laneId = lane::LaneId(11334);
763 [ + - ]: 1 : object.laneOccupiedRegions.push_back(region);
764 [ + - - + : 1 : ASSERT_TRUE(intersection->objectOnInternalLaneWithLowerPriority(object));
- - - - -
- - - ]
765 : :
766 : 1 : object.laneOccupiedRegions.clear();
767 : 1 : region.laneId = lane::LaneId(11517);
768 [ + - ]: 1 : object.laneOccupiedRegions.push_back(region);
769 [ + - - + : 1 : ASSERT_TRUE(intersection->objectOnIncomingLaneWithLowerPriority(object));
- - - - -
- - - ]
770 : :
771 : 1 : object.laneOccupiedRegions.clear();
772 : 1 : region.laneId = lane::LaneId(11517);
773 [ + - ]: 1 : object.laneOccupiedRegions.push_back(region);
774 [ + - - + : 1 : ASSERT_TRUE(intersection->objectOnLaneWithLowerPriority(object));
- - - - -
- - - ]
775 : :
776 : 1 : physics::Distance dis;
777 : 1 : match::Object obj;
778 : :
779 : 1 : object.laneOccupiedRegions.clear();
780 : 1 : region.laneId = lane::LaneId(7127);
781 [ + - ]: 1 : object.laneOccupiedRegions.push_back(region);
782 [ + - ]: 1 : obj.mapMatchedBoundingBox = object;
783 [ + - ]: 1 : dis = intersection->objectDistanceToIntersection(obj);
784 [ + - - + : 1 : ASSERT_NEAR((double)dis, 0., 0.0001);
- - - - -
- ]
785 : :
786 : 1 : object.laneOccupiedRegions.clear();
787 : 1 : region.laneId = lane::LaneId(7127);
788 [ + - ]: 1 : object.laneOccupiedRegions.push_back(region);
789 [ + - ]: 1 : obj.mapMatchedBoundingBox = object;
790 : 1 : obj.enuPosition.dimension.length = physics::Distance(1.0);
791 [ + - ]: 1 : dis = intersection->objectInterpenetrationDistanceWithIntersection(obj);
792 [ + - - + : 1 : ASSERT_NEAR((double)dis, 1.0, 0.0001);
- - - - -
- ]
793 : :
794 [ + - ]: 1 : route::FindWaypointResult findWaypoint = route::intersectionOnRoute(*intersection, fullRoute);
795 [ - + - - : 1 : ASSERT_TRUE(findWaypoint.isValid());
- - - - -
- ]
796 : 1 : route::FullRoute routeConstruct;
797 : :
798 : 1 : ::ad::map::route::LaneSegment laneSegment;
799 : 1 : laneSegment.laneInterval.laneId = lane::LaneId(7127);
800 : 1 : laneSegment.laneInterval.start = ::ad::physics::ParametricValue(0.2);
801 : 1 : laneSegment.laneInterval.end = ::ad::physics::ParametricValue(0.8);
802 : :
803 : 1 : ::ad::map::route::LaneSegmentList drivableLaneSegments;
804 [ + - ]: 1 : drivableLaneSegments.push_back(laneSegment);
805 : 1 : ::ad::map::route::RoadSegment roadSegment;
806 [ + - ]: 1 : roadSegment.drivableLaneSegments = drivableLaneSegments;
807 : 1 : ::ad::map::route::RoadSegmentList roadSegments;
808 [ + - ]: 1 : roadSegments.push_back(roadSegment);
809 [ + - ]: 1 : routeConstruct.roadSegments = roadSegments;
810 : :
811 [ + - ]: 1 : route::FindWaypointResult findWaypoint1 = route::intersectionOnRoute(*intersection, routeConstruct);
812 [ - + - - : 1 : ASSERT_TRUE(findWaypoint1.isValid());
- - - - -
- ]
813 : :
814 [ + - - + : 1 : ASSERT_FALSE(intersection->objectRouteCrossesIntersectionRoute(fullRoute));
- - - - -
- - - ]
815 [ + - - + : 1 : ASSERT_TRUE(intersection->objectRouteFromSameArmAsIntersectionRoute(fullRoute));
- - - - -
- - - ]
816 [ + - - + : 1 : ASSERT_FALSE(intersection->objectRouteOppositeToIntersectionRoute(fullRoute));
- - - - -
- - - ]
817 [ + - - + : 1 : ASSERT_FALSE(intersection->objectRouteCrossesLanesWithHigherPriority(fullRoute));
- - - - -
- - - ]
818 : : }
819 : :
820 : : } // namespace map
821 : : } // namespace ad
|
