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/Factory.hpp>
10 : : #include <ad/map/access/Operation.hpp>
11 : : #include <ad/map/lane/LaneOperation.hpp>
12 : : #include <ad/map/point/Operation.hpp>
13 : : #include <ad/map/route/LaneIntervalOperation.hpp>
14 : : #include <ad/map/route/RouteOperation.hpp>
15 : : #include <ad/map/route/Types.hpp>
16 : : #include <algorithm>
17 : : #include <gtest/gtest.h>
18 : : #include "../point/RandomGeometry.hpp"
19 : :
20 : : using namespace ::ad;
21 : : using namespace ::ad::map;
22 : : using namespace ::ad::map::route;
23 : : using namespace ::ad::map::point;
24 : : using namespace ::ad::map::lane;
25 : :
26 : : struct LaneIntervalOperationTest : ::testing::Test
27 : : {
28 : 5 : LaneIntervalOperationTest()
29 : 5 : {
30 : 5 : }
31 : :
32 : 5 : virtual void SetUp()
33 : : {
34 : 5 : }
35 : 5 : virtual void TearDown()
36 : : {
37 : 5 : access::cleanup();
38 : 5 : }
39 : 1 : void setupSingleLongLaneV1()
40 : : {
41 [ + - + - : 1 : mStorePtr.reset(new access::Store());
+ - ]
42 [ + - + - ]: 1 : pFactory.reset(new access::Factory(*mStorePtr));
43 : : // setup one single long lane
44 : : point::GeoPoint bottomRight
45 : 1 : = point::createGeoPoint(point::Longitude(8.04937788), point::Latitude(49.00736837), point::Altitude(0.));
46 : : point::GeoPoint upRight
47 : 1 : = point::createGeoPoint(point::Longitude(8.05010064), point::Latitude(49.00884320), point::Altitude(0.));
48 : : point::GeoPoint bottomLeft
49 : 1 : = point::createGeoPoint(point::Longitude(8.04933898), point::Latitude(49.00737633), point::Altitude(0.));
50 : : point::GeoPoint upLeft
51 : 1 : = point::createGeoPoint(point::Longitude(8.05006151), point::Latitude(49.00885163), point::Altitude(0.));
52 : :
53 : 1 : point::ECEFEdge rightPoints;
54 [ + - + - ]: 1 : rightPoints.push_back(point::toECEF(bottomRight));
55 [ + - + - ]: 1 : rightPoints.push_back(point::toECEF(upRight));
56 [ + - ]: 1 : rightEdge = point::createGeometry(rightPoints, false);
57 : 1 : point::ECEFEdge leftPoints;
58 [ + - + - ]: 1 : leftPoints.push_back(point::toECEF(bottomLeft));
59 [ + - + - ]: 1 : leftPoints.push_back(point::toECEF(upLeft));
60 [ + - ]: 1 : leftEdge = point::createGeometry(leftPoints, false);
61 [ + - ]: 1 : pFactory->set(access::TrafficType::RIGHT_HAND_TRAFFIC);
62 : :
63 [ + - ]: 1 : pFactory->add(p, laneId, lane::LaneType::NORMAL, lane::LaneDirection::POSITIVE);
64 : :
65 [ + - ]: 1 : pFactory->set(laneId, leftEdge, rightEdge);
66 : :
67 [ + - ]: 1 : access::cleanup();
68 [ + - - + : 1 : ASSERT_TRUE(access::init(mStorePtr));
- - - - -
- - - ]
69 : : }
70 : 3 : void setupSingleLongLaneV2()
71 : : {
72 [ + - ]: 3 : mStorePtr.reset(new access::Store());
73 : 3 : pFactory.reset(new access::Factory(*mStorePtr));
74 : :
75 : 3 : edge_ecef1.clear();
76 : 3 : edge_ecef2.clear();
77 : 3 : basePoint = randECEFPoint();
78 : 3 : edge_ecef1.push_back(basePoint);
79 [ + - + - ]: 3 : edge_ecef1.push_back(basePoint + createECEFPoint(10., 0., 0.));
80 [ + - + - ]: 3 : edge_ecef2.push_back(basePoint + createECEFPoint(0., 10., 0.));
81 [ + - + - ]: 3 : edge_ecef2.push_back(basePoint + createECEFPoint(10., 10., 0.));
82 : 3 : geo1 = createGeometry(edge_ecef1, false);
83 : 3 : geo2 = createGeometry(edge_ecef2, false);
84 : 3 : x12 = lane::LaneId(i++);
85 : 3 : pFactory->add(p, x12, lane::LaneType::NORMAL, lane::LaneDirection::POSITIVE);
86 [ + - ]: 3 : pFactory->set(access::TrafficType::RIGHT_HAND_TRAFFIC);
87 : 3 : pFactory->set(x12, geo1, geo2);
88 : 3 : laneInt1.start = ::ad::physics::ParametricValue(0.2);
89 : 3 : laneInt1.end = ::ad::physics::ParametricValue(0.8);
90 : 3 : laneInt2.start = ::ad::physics::ParametricValue(0.8);
91 : 3 : laneInt2.end = ::ad::physics::ParametricValue(0.2);
92 : :
93 : 3 : access::cleanup();
94 [ + - - + : 3 : ASSERT_TRUE(access::init(mStorePtr));
- - - - -
- - - ]
95 : : }
96 : : lane::LaneId laneId{4711}, x11, x12, x13;
97 : : uint32_t i = 100;
98 : : route::LaneInterval laneInt1, laneInt2;
99 : : ECEFPoint basePoint;
100 : : ECEFEdge edge_ecef1, edge_ecef2, edge_ecef3, edge_ecef4;
101 : : access::PartitionId p{0};
102 : : point::Geometry leftEdge, rightEdge, geo1, geo2;
103 : : access::Store::Ptr mStorePtr;
104 : : std::shared_ptr<access::Factory> pFactory;
105 : : };
106 : :
107 : 2 : TEST_F(LaneIntervalOperationTest, LaneIntervalOperation)
108 : : {
109 [ + - ]: 1 : setupSingleLongLaneV1();
110 : 1 : laneInt1.laneId = laneId;
111 : 1 : laneInt1.start = ::ad::physics::ParametricValue(0.2);
112 : 1 : laneInt1.end = ::ad::physics::ParametricValue(0.8);
113 : :
114 : 1 : point::ParaPoint first, second;
115 : 1 : first.laneId = lane::LaneId(100);
116 : 1 : first.parametricOffset = ::ad::physics::ParametricValue(0.2);
117 : 1 : second.laneId = lane::LaneId(100);
118 : 1 : second.parametricOffset = ::ad::physics::ParametricValue(0.8);
119 [ + - + - : 2 : EXPECT_THROW(getSignedDistance(laneInt1, first, second), std::invalid_argument);
+ - - + -
+ - - - -
- - - - ]
120 [ + - + - : 2 : EXPECT_THROW(getUnsignedDistance(laneInt1, first, second), std::invalid_argument);
+ - - + -
+ - - - -
- - - - ]
121 : 1 : first.laneId = lane::LaneId(4711);
122 [ + - + - : 2 : EXPECT_THROW(getSignedDistance(laneInt1, first, second), std::invalid_argument);
+ - - + -
+ - - - -
- - - - ]
123 [ + - + - : 2 : EXPECT_THROW(getUnsignedDistance(laneInt1, first, second), std::invalid_argument);
+ - - + -
+ - - - -
- - - - ]
124 : 1 : second.laneId = lane::LaneId(4711);
125 [ + - + - : 1 : ASSERT_EQ(getSignedDistance(laneInt1, first, second), ::ad::physics::ParametricValue(0.6));
- + - - -
- - - ]
126 : 1 : laneInt1.end = ::ad::physics::ParametricValue(0.1);
127 [ + - + - : 1 : ASSERT_EQ(getSignedDistance(laneInt1, first, second), ::ad::physics::ParametricValue(-0.6));
- + - - -
- - - ]
128 [ + - + - : 1 : ASSERT_EQ(getUnsignedDistance(laneInt1, first, second), ::ad::physics::ParametricValue(0.6));
- + - - -
- - - ]
129 : :
130 : 1 : laneInt1.start = laneInt1.end = ::ad::physics::ParametricValue(0.5);
131 [ + - + - : 1 : ASSERT_EQ(extendIntervalFromStart(laneInt1, physics::Distance(0.5)), laneInt1);
- + - - -
- - - ]
132 [ + - + - : 1 : ASSERT_EQ(extendIntervalFromEnd(laneInt1, physics::Distance(0.5)), laneInt1);
- + - - -
- - - ]
133 : :
134 : 1 : LaneInterval resultInterval;
135 [ + - + - : 1 : ASSERT_EQ(extendIntervalUntilStart(laneInt1), laneInt1);
- + - - -
- - - ]
136 [ + - + - : 1 : ASSERT_EQ(extendIntervalUntilEnd(laneInt1), laneInt1);
- + - - -
- - - ]
137 : :
138 : 1 : laneInt1.end = ::ad::physics::ParametricValue(0.8);
139 : 1 : resultInterval = laneInt1;
140 : 1 : resultInterval.start = ::ad::physics::ParametricValue(0.0);
141 [ + - + - : 1 : ASSERT_EQ(extendIntervalUntilStart(laneInt1), resultInterval);
- + - - -
- - - ]
142 : 1 : resultInterval = laneInt1;
143 : 1 : resultInterval.end = ::ad::physics::ParametricValue(1.0);
144 [ + - + - : 1 : ASSERT_EQ(extendIntervalUntilEnd(laneInt1), resultInterval);
- + - - -
- - - ]
145 : :
146 : 1 : laneInt1.end = ::ad::physics::ParametricValue(0.2);
147 : 1 : resultInterval = laneInt1;
148 : 1 : resultInterval.start = ::ad::physics::ParametricValue(1.0);
149 [ + - + - : 1 : ASSERT_EQ(extendIntervalUntilStart(laneInt1), resultInterval);
- + - - -
- - - ]
150 : 1 : resultInterval = laneInt1;
151 : 1 : resultInterval.end = ::ad::physics::ParametricValue(0.0);
152 [ + - + - : 1 : ASSERT_EQ(extendIntervalUntilEnd(laneInt1), resultInterval);
- + - - -
- - - ]
153 : :
154 : 1 : laneInt1.end = ::ad::physics::ParametricValue(0.8);
155 : 1 : resultInterval = laneInt1;
156 [ + - + - : 1 : ASSERT_EQ(cutIntervalAtStart(laneInt1, ::ad::physics::ParametricValue(0.1)), resultInterval);
- + - - -
- - - ]
157 [ + - + - : 1 : ASSERT_EQ(cutIntervalAtEnd(laneInt1, ::ad::physics::ParametricValue(0.1)), resultInterval);
- + - - -
- - - ]
158 : :
159 : 1 : resultInterval.start = ::ad::physics::ParametricValue(0.6);
160 [ + - + - : 1 : ASSERT_EQ(cutIntervalAtStart(laneInt1, ::ad::physics::ParametricValue(0.6)), resultInterval);
- + - - -
- - - ]
161 : 1 : resultInterval = laneInt1;
162 : 1 : resultInterval.end = ::ad::physics::ParametricValue(0.6);
163 [ + - + - : 1 : ASSERT_EQ(cutIntervalAtEnd(laneInt1, ::ad::physics::ParametricValue(0.6)), resultInterval);
- + - - -
- - - ]
164 : :
165 : 1 : laneInt1.start = ::ad::physics::ParametricValue(0.5);
166 : 1 : laneInt1.end = ::ad::physics::ParametricValue(0.3);
167 [ + - ]: 1 : resultInterval = shortenIntervalFromBegin(laneInt1, physics::Distance(17.2351));
168 [ + - - + : 1 : ASSERT_EQ(resultInterval.laneId, laneInt1.laneId);
- - - - -
- ]
169 [ + - - + : 1 : ASSERT_EQ(resultInterval.end, laneInt1.end);
- - - - -
- ]
170 [ + - - + : 1 : ASSERT_NEAR((double)resultInterval.start, 0.4, 0.0001);
- - - - -
- ]
171 : :
172 [ + - ]: 1 : resultInterval = restrictIntervalFromBegin(laneInt1, physics::Distance(17.2351));
173 [ + - - + : 1 : ASSERT_EQ(resultInterval.laneId, laneInt1.laneId);
- - - - -
- ]
174 [ + - - + : 1 : ASSERT_EQ(resultInterval.start, laneInt1.start);
- - - - -
- ]
175 [ + - - + : 1 : ASSERT_NEAR((double)resultInterval.end, 0.4, 0.0001);
- - - - -
- ]
176 : 1 : laneInt1.start = ::ad::physics::ParametricValue(0.3);
177 : 1 : laneInt1.end = ::ad::physics::ParametricValue(0.5);
178 [ + - ]: 1 : resultInterval = restrictIntervalFromBegin(laneInt1, physics::Distance(17.2351));
179 [ + - - + : 1 : ASSERT_EQ(resultInterval.laneId, laneInt1.laneId);
- - - - -
- ]
180 [ + - - + : 1 : ASSERT_EQ(resultInterval.start, laneInt1.start);
- - - - -
- ]
181 [ + - - + : 1 : ASSERT_NEAR((double)resultInterval.end, 0.4, 0.0001);
- - - - -
- ]
182 : : }
183 : :
184 : 2 : TEST_F(LaneIntervalOperationTest, GetEdgeAndBorder)
185 : : {
186 [ + - ]: 1 : setupSingleLongLaneV2();
187 : :
188 : 1 : GeoEdge edge_geo1, edge_geo2;
189 : 1 : ECEFEdge edge_ecef5;
190 : 1 : Geometry geo3, geo4;
191 : :
192 : 1 : laneInt1.laneId = x12;
193 : :
194 : 1 : lane::ECEFBorder border_ecef1;
195 [ + - ]: 1 : border_ecef1 = getECEFBorder(laneInt1);
196 : 1 : edge_ecef3.clear();
197 : 1 : edge_ecef4.clear();
198 [ + - + - ]: 1 : edge_ecef3.push_back(basePoint + createECEFPoint(2., 0., 0.));
199 [ + - + - ]: 1 : edge_ecef3.push_back(basePoint + createECEFPoint(8., 0., 0.));
200 [ + - + - ]: 1 : edge_ecef4.push_back(basePoint + createECEFPoint(2., 10., 0.));
201 [ + - + - ]: 1 : edge_ecef4.push_back(basePoint + createECEFPoint(8., 10., 0.));
202 [ + - - + : 1 : ASSERT_EQ(border_ecef1.left, edge_ecef3);
- - - - -
- ]
203 [ + - - + : 1 : ASSERT_EQ(border_ecef1.right, edge_ecef4);
- - - - -
- ]
204 : :
205 : 1 : RoadSegment roadSeg;
206 : 1 : struct LaneSegment laneSeg;
207 : 1 : laneSeg.laneInterval = laneInt1;
208 [ + - ]: 1 : roadSeg.drivableLaneSegments.push_back(laneSeg);
209 : 1 : lane::ECEFBorder border_ecef_road;
210 [ + - ]: 1 : border_ecef_road = getECEFBorderOfRoadSegment(roadSeg, physics::ParametricValue(0.1));
211 [ + - - + : 1 : ASSERT_EQ(border_ecef1.left, border_ecef_road.left);
- - - - -
- ]
212 [ + - - + : 1 : ASSERT_EQ(border_ecef1.right, border_ecef_road.right);
- - - - -
- ]
213 : :
214 [ + - ]: 1 : edge_ecef5 = getLeftECEFEdge(laneInt1);
215 [ + - - + : 1 : ASSERT_EQ(edge_ecef5, edge_ecef3);
- - - - -
- ]
216 [ + - ]: 1 : edge_ecef5 = getRightECEFEdge(laneInt1);
217 [ + - - + : 1 : ASSERT_EQ(edge_ecef5, edge_ecef4);
- - - - -
- ]
218 : :
219 [ + - ]: 1 : lane::GeoBorder geo_border1 = getGeoBorder(laneInt1);
220 [ + - + - : 2 : ASSERT_EQ(geo_border1.left, toGeo(edge_ecef3));
- + - - -
- - - ]
221 [ + - + - : 2 : ASSERT_EQ(geo_border1.right, toGeo(edge_ecef4));
- + - - -
- - - ]
222 : :
223 : 1 : lane::GeoBorder border_geo_road;
224 [ + - ]: 1 : border_geo_road = getGeoBorderOfRoadSegment(roadSeg, physics::ParametricValue(0.1));
225 [ + - - + : 1 : ASSERT_EQ(geo_border1.left, border_geo_road.left);
- - - - -
- ]
226 [ + - - + : 1 : ASSERT_EQ(geo_border1.right, border_geo_road.right);
- - - - -
- ]
227 : :
228 [ + - ]: 1 : edge_geo1 = getLeftGeoEdge(laneInt1);
229 [ + - + - : 2 : ASSERT_EQ(edge_geo1, toGeo(edge_ecef3));
- + - - -
- - - ]
230 [ + - ]: 1 : edge_geo1 = getRightGeoEdge(laneInt1);
231 [ + - + - : 2 : ASSERT_EQ(edge_geo1, toGeo(edge_ecef4));
- + - - -
- - - ]
232 : :
233 [ + - ]: 1 : GeoPoint geo_pt1 = toGeo(basePoint);
234 [ + - ]: 1 : access::setENUReferencePoint(geo_pt1);
235 [ + - ]: 1 : CoordinateTransform mCoordinateTransform;
236 [ + - + - ]: 1 : mCoordinateTransform.setENUReferencePoint(access::getENUReferencePoint());
237 [ + - ]: 1 : lane::ENUBorder border_enu1 = getENUBorder(laneInt1);
238 : 1 : ENUEdge edge_enu1, edge_enu2, edge_enu3;
239 [ + - + - : 1 : edge_enu1.push_back(mCoordinateTransform.ECEF2ENU(basePoint + createECEFPoint(2., 0., 0.)));
+ - ]
240 [ + - + - : 1 : edge_enu1.push_back(mCoordinateTransform.ECEF2ENU(basePoint + createECEFPoint(8., 0., 0.)));
+ - ]
241 [ + - + - : 1 : edge_enu2.push_back(mCoordinateTransform.ECEF2ENU(basePoint + createECEFPoint(2., 10., 0.)));
+ - ]
242 [ + - + - : 1 : edge_enu2.push_back(mCoordinateTransform.ECEF2ENU(basePoint + createECEFPoint(8., 10., 0.)));
+ - ]
243 [ + - - + : 1 : ASSERT_EQ(border_enu1.left, edge_enu1);
- - - - -
- ]
244 [ + - - + : 1 : ASSERT_EQ(border_enu1.right, edge_enu2);
- - - - -
- ]
245 : :
246 : 1 : lane::ENUBorder border_enu_road;
247 [ + - ]: 1 : border_enu_road = getENUBorderOfRoadSegment(roadSeg, physics::ParametricValue(0.1));
248 [ + - - + : 1 : ASSERT_EQ(border_enu1.left, border_enu_road.left);
- - - - -
- ]
249 [ + - - + : 1 : ASSERT_EQ(border_enu1.right, border_enu_road.right);
- - - - -
- ]
250 : :
251 [ + - ]: 1 : edge_enu3 = getLeftENUEdge(laneInt1);
252 [ + - - + : 1 : ASSERT_EQ(edge_enu3, edge_enu1);
- - - - -
- ]
253 [ + - ]: 1 : edge_enu3 = getRightENUEdge(laneInt1);
254 [ + - - + : 1 : ASSERT_EQ(edge_enu3, edge_enu2);
- - - - -
- ]
255 : :
256 [ + - ]: 1 : border_enu1 = getENUProjectedBorder(laneInt1);
257 [ + - - + : 1 : ASSERT_EQ(border_enu1.left, edge_enu1);
- - - - -
- ]
258 [ + - - + : 1 : ASSERT_EQ(border_enu1.right, edge_enu2);
- - - - -
- ]
259 [ + - ]: 1 : edge_enu3 = getLeftProjectedENUEdge(laneInt1);
260 [ + - - + : 1 : ASSERT_EQ(edge_enu3, edge_enu1);
- - - - -
- ]
261 [ + - ]: 1 : edge_enu3 = getRightProjectedENUEdge(laneInt1);
262 [ + - - + : 1 : ASSERT_EQ(edge_enu3, edge_enu2);
- - - - -
- ]
263 : :
264 : 1 : laneInt2.laneId = x12;
265 : :
266 [ + - ]: 1 : edge_ecef5 = getLeftECEFEdge(laneInt2);
267 [ + - ]: 1 : reverse(edge_ecef4.begin(), edge_ecef4.end());
268 [ + - - + : 1 : ASSERT_EQ(edge_ecef5, edge_ecef4);
- - - - -
- ]
269 : :
270 [ + - ]: 1 : edge_ecef5 = getRightECEFEdge(laneInt2);
271 [ + - ]: 1 : reverse(edge_ecef3.begin(), edge_ecef3.end());
272 [ + - - + : 1 : ASSERT_EQ(edge_ecef5, edge_ecef3);
- - - - -
- ]
273 : :
274 [ + - ]: 1 : edge_geo1 = getLeftGeoEdge(laneInt2);
275 [ + - + - : 2 : ASSERT_EQ(edge_geo1, toGeo(edge_ecef4));
- + - - -
- - - ]
276 [ + - ]: 1 : edge_geo1 = getRightGeoEdge(laneInt2);
277 [ + - + - : 2 : ASSERT_EQ(edge_geo1, toGeo(edge_ecef3));
- + - - -
- - - ]
278 : :
279 [ + - ]: 1 : edge_enu3 = getLeftProjectedENUEdge(laneInt2);
280 [ + - ]: 1 : reverse(edge_enu2.begin(), edge_enu2.end());
281 [ + - - + : 1 : ASSERT_EQ(edge_enu3, edge_enu2);
- - - - -
- ]
282 [ + - ]: 1 : edge_enu3 = getRightProjectedENUEdge(laneInt2);
283 [ + - ]: 1 : reverse(edge_enu1.begin(), edge_enu1.end());
284 [ + - - + : 1 : ASSERT_EQ(edge_enu3, edge_enu1);
- - - - -
- ]
285 : :
286 [ + - ]: 1 : border_enu1 = getENUProjectedBorder(laneInt2);
287 [ + - - + : 1 : ASSERT_EQ(border_enu1.left, edge_enu2);
- - - - -
- ]
288 [ + - - + : 1 : ASSERT_EQ(border_enu1.right, edge_enu1);
- - - - -
- ]
289 : : }
290 : :
291 : 2 : TEST_F(LaneIntervalOperationTest, GetProjectedENUEdgeOnTown01)
292 : : {
293 [ + - + - : 1 : ASSERT_TRUE(access::init("test_files/Town01.txt"));
- + - - -
- - - -
- ]
294 : :
295 : 1 : ENUPoint point_edu1 = createENUPoint(4.453, -4.560, 0);
296 [ + - + - : 1 : ASSERT_NEAR((double)lane::calcWidth(point_edu1), 3.99, 0.0001);
- + - - -
- - - ]
297 [ + - ]: 1 : GeoPoint point_geo1 = toGeo(point_edu1);
298 [ + - ]: 1 : auto startLaneId = lane::uniqueLaneId(point_geo1);
299 : 1 : laneInt1.laneId = startLaneId;
300 : 1 : laneInt1.start = ::ad::physics::ParametricValue(0.2);
301 : 1 : laneInt1.end = ::ad::physics::ParametricValue(0.8);
302 : 1 : laneInt2.laneId = startLaneId;
303 : 1 : laneInt2.start = ::ad::physics::ParametricValue(0.8);
304 : 1 : laneInt2.end = ::ad::physics::ParametricValue(0.2);
305 : :
306 : 1 : point::ENUEdge edge_enu1, edge_enu2, edge_enu3, edge_enu4;
307 [ + - ]: 1 : edge_enu1 = getLeftProjectedENUEdge(laneInt1);
308 [ + - - + : 1 : ASSERT_GT(edge_enu1.size(), 0u);
- - - - -
- ]
309 : :
310 [ + - ]: 1 : edge_enu2 = getRightProjectedENUEdge(laneInt1);
311 [ + - - + : 1 : ASSERT_GT(edge_enu2.size(), 0u);
- - - - -
- ]
312 : :
313 : 1 : lane::ENUBorder border_enu1;
314 [ + - ]: 1 : border_enu1 = getENUProjectedBorder(laneInt1);
315 [ + - - + : 1 : ASSERT_EQ(border_enu1.left, edge_enu1);
- - - - -
- ]
316 [ + - - + : 1 : ASSERT_EQ(border_enu1.right, edge_enu2);
- - - - -
- ]
317 : :
318 [ + - ]: 1 : edge_enu3 = getLeftProjectedENUEdge(laneInt2);
319 [ + - ]: 1 : reverse(edge_enu2.begin(), edge_enu2.end());
320 [ + - - + : 1 : ASSERT_EQ(edge_enu3, edge_enu2);
- - - - -
- ]
321 : :
322 [ + - ]: 1 : edge_enu4 = getRightProjectedENUEdge(laneInt2);
323 [ + - ]: 1 : reverse(edge_enu1.begin(), edge_enu1.end());
324 [ + - - + : 1 : ASSERT_EQ(edge_enu4, edge_enu1);
- - - - -
- ]
325 : :
326 [ + - ]: 1 : border_enu1 = getENUProjectedBorder(laneInt2);
327 [ + - - + : 1 : ASSERT_EQ(border_enu1.left, edge_enu2);
- - - - -
- ]
328 [ + - - + : 1 : ASSERT_EQ(border_enu1.right, edge_enu1);
- - - - -
- ]
329 : : }
330 : :
331 : 2 : TEST_F(LaneIntervalOperationTest, GetProjectedOffsetOnNeighborLane)
332 : : {
333 [ + - ]: 1 : setupSingleLongLaneV2();
334 : 1 : physics::ParametricValue paraVal;
335 : :
336 : 1 : laneInt2.laneId = lane::LaneId(1003);
337 [ + - + - : 2 : EXPECT_THROW(getProjectedParametricOffsetOnNeighborLane(laneInt1, laneInt2, ::ad::physics::ParametricValue(0.8)),
+ - - + -
+ - - - -
- - - - ]
338 : : std::invalid_argument);
339 : :
340 : 1 : laneInt1.laneId = x12;
341 : 1 : laneInt2.laneId = x12;
342 [ + - ]: 1 : paraVal = getProjectedParametricOffsetOnNeighborLane(laneInt1, laneInt2, ::ad::physics::ParametricValue(0.8));
343 [ + - - + : 1 : ASSERT_EQ(paraVal, ::ad::physics::ParametricValue(0.8));
- - - - -
- ]
344 : :
345 : 1 : edge_ecef1.clear();
346 : 1 : edge_ecef2.clear();
347 [ + - ]: 1 : edge_ecef1.push_back(createECEFPoint(20000, 200000, 300000));
348 [ + - ]: 1 : edge_ecef1.push_back(createECEFPoint(30000, 200000, 300000));
349 [ + - ]: 1 : edge_ecef2.push_back(createECEFPoint(20000, 300000, 300000));
350 [ + - ]: 1 : edge_ecef2.push_back(createECEFPoint(30000, 300000, 300000));
351 [ + - ]: 1 : geo1 = createGeometry(edge_ecef1, false);
352 [ + - ]: 1 : geo2 = createGeometry(edge_ecef2, false);
353 : 1 : x11 = lane::LaneId(i++);
354 [ + - ]: 1 : pFactory->add(p, x11, lane::LaneType::NORMAL, lane::LaneDirection::POSITIVE);
355 [ + - ]: 1 : pFactory->set(x11, geo1, geo2);
356 : :
357 : 1 : edge_ecef1.clear();
358 : 1 : edge_ecef2.clear();
359 [ + - ]: 1 : edge_ecef1.push_back(createECEFPoint(30000, 200000, 300000));
360 [ + - ]: 1 : edge_ecef1.push_back(createECEFPoint(40000, 200000, 300000));
361 [ + - ]: 1 : edge_ecef2.push_back(createECEFPoint(30000, 300000, 300000));
362 [ + - ]: 1 : edge_ecef2.push_back(createECEFPoint(40000, 300000, 300000));
363 [ + - ]: 1 : geo1 = createGeometry(edge_ecef1, false);
364 [ + - ]: 1 : geo2 = createGeometry(edge_ecef2, false);
365 : 1 : x12 = lane::LaneId(i++);
366 [ + - ]: 1 : pFactory->add(p, x12, lane::LaneType::NORMAL, lane::LaneDirection::POSITIVE);
367 [ + - ]: 1 : pFactory->set(x12, geo1, geo2);
368 : :
369 : 1 : edge_ecef1.clear();
370 : 1 : edge_ecef2.clear();
371 [ + - ]: 1 : edge_ecef1.push_back(createECEFPoint(40000, 200000, 300000));
372 [ + - ]: 1 : edge_ecef1.push_back(createECEFPoint(50000, 200000, 300000));
373 [ + - ]: 1 : edge_ecef2.push_back(createECEFPoint(40000, 300000, 300000));
374 [ + - ]: 1 : edge_ecef2.push_back(createECEFPoint(50000, 300000, 300000));
375 [ + - ]: 1 : geo1 = createGeometry(edge_ecef1, false);
376 [ + - ]: 1 : geo2 = createGeometry(edge_ecef2, false);
377 : 1 : x13 = lane::LaneId(i++);
378 [ + - ]: 1 : pFactory->add(p, x13, lane::LaneType::NORMAL, lane::LaneDirection::POSITIVE);
379 [ + - ]: 1 : pFactory->set(x13, geo1, geo2);
380 : :
381 : 1 : lane::ContactLocation left(lane::ContactLocation::LEFT);
382 : 1 : lane::ContactLocation right(lane::ContactLocation::RIGHT);
383 [ + - ]: 1 : lane::ContactTypeList free({lane::ContactType::FREE});
384 : 1 : restriction::Restriction all_vehicles;
385 : : all_vehicles.roadUserTypes
386 [ + - ]: 1 : = {restriction::RoadUserType::CAR, restriction::RoadUserType::BUS, restriction::RoadUserType::TRUCK};
387 : 1 : restriction::RestrictionList all_vehicles_list;
388 [ + - ]: 1 : all_vehicles_list.push_back(all_vehicles);
389 : 1 : restriction::Restrictions all_vehicle_restrs;
390 [ + - ]: 1 : all_vehicle_restrs.conjunctions = all_vehicles_list;
391 [ + - - + : 1 : ASSERT_TRUE(pFactory->add(x12, x11, left, free, all_vehicle_restrs));
- - - - -
- - - ]
392 [ + - - + : 1 : ASSERT_TRUE(pFactory->add(x12, x13, right, free, all_vehicle_restrs));
- - - - -
- - - ]
393 : :
394 : 1 : laneInt1.laneId = x12;
395 : 1 : laneInt1.start = ::ad::physics::ParametricValue(0.2);
396 : 1 : laneInt1.end = ::ad::physics::ParametricValue(0.8);
397 : 1 : laneInt2.laneId = x11;
398 : 1 : laneInt2.start = ::ad::physics::ParametricValue(0.2);
399 : 1 : laneInt2.end = ::ad::physics::ParametricValue(0.8);
400 [ + - ]: 1 : paraVal = getProjectedParametricOffsetOnNeighborLane(laneInt1, laneInt2, ::ad::physics::ParametricValue(0.8));
401 [ + - - + : 1 : ASSERT_DOUBLE_EQ((double)paraVal, 1.0);
- - - - -
- ]
402 : :
403 : 1 : laneInt2.laneId = x13;
404 [ + - ]: 1 : paraVal = getProjectedParametricOffsetOnNeighborLane(laneInt1, laneInt2, ::ad::physics::ParametricValue(0.8));
405 [ + - - + : 1 : ASSERT_DOUBLE_EQ((double)paraVal, 0.0);
- - - - -
- ]
406 : : }
407 : :
408 : 2 : TEST_F(LaneIntervalOperationTest, RouteInterval)
409 : : {
410 [ + - ]: 1 : setupSingleLongLaneV2();
411 : 1 : point::ParaPoint para;
412 : 1 : FullRoute fullRoute;
413 [ + - + - : 2 : EXPECT_THROW(getIntervalStart(fullRoute, x12), std::invalid_argument);
+ - - + -
+ - - - -
- - - - ]
414 : :
415 : 1 : ::ad::map::route::LaneSegment laneSegment;
416 : 1 : laneSegment.laneInterval.laneId = x12;
417 : 1 : laneSegment.laneInterval.start = ::ad::physics::ParametricValue(0.2);
418 : 1 : laneSegment.laneInterval.end = ::ad::physics::ParametricValue(0.8);
419 : :
420 : 1 : ::ad::map::route::LaneSegmentList drivableLaneSegments;
421 [ + - ]: 1 : drivableLaneSegments.push_back(laneSegment);
422 : 1 : ::ad::map::route::RoadSegment roadSegment;
423 [ + - ]: 1 : roadSegment.drivableLaneSegments = drivableLaneSegments;
424 : 1 : ::ad::map::route::RoadSegmentList roadSegments;
425 [ + - ]: 1 : roadSegments.push_back(roadSegment);
426 [ + - ]: 1 : fullRoute.roadSegments = roadSegments;
427 [ + - ]: 1 : para = getIntervalStart(fullRoute, x12);
428 [ + - - + : 1 : ASSERT_EQ(para.laneId, x12);
- - - - -
- ]
429 [ + - - + : 1 : ASSERT_EQ(para.parametricOffset, ::ad::physics::ParametricValue(0.2));
- - - - -
- ]
430 : :
431 [ + - ]: 1 : para = getLaneParaPoint(physics::ParametricValue(0.5), laneSegment.laneInterval);
432 [ + - - + : 1 : ASSERT_EQ(para.laneId, x12);
- - - - -
- ]
433 [ + - - + : 1 : ASSERT_NEAR((double)para.parametricOffset, 0.5, 0.0001);
- - - - -
- ]
434 : :
435 : 1 : laneSegment.laneInterval.start = ::ad::physics::ParametricValue(0.5);
436 : 1 : laneSegment.laneInterval.end = ::ad::physics::ParametricValue(0.5);
437 [ + - ]: 1 : para = getLaneParaPoint(physics::ParametricValue(0.5), laneSegment.laneInterval);
438 [ + - - + : 1 : ASSERT_EQ(para.laneId, x12);
- - - - -
- ]
439 [ + - - + : 1 : ASSERT_NEAR((double)para.parametricOffset, 0.5, 0.0001);
- - - - -
- ]
440 : :
441 : 1 : FullRoute emptyRoute;
442 : 1 : match::MapMatchedPositionConfidenceList centerMapMatched;
443 [ + - + - : 2 : EXPECT_THROW(signedDistanceToLane(lane::LaneId(100), emptyRoute, centerMapMatched), std::runtime_error);
+ - - + -
+ - - - -
- - - - ]
444 : :
445 : 1 : laneSegment.laneInterval.start = ::ad::physics::ParametricValue(0.5);
446 : 1 : laneSegment.laneInterval.end = ::ad::physics::ParametricValue(0.4);
447 : 1 : para.laneId = x12;
448 : 1 : para.parametricOffset = ::ad::physics::ParametricValue(0.3);
449 [ + - - + : 1 : ASSERT_TRUE(isAfterInterval(laneSegment.laneInterval, para));
- - - - -
- - - ]
450 : : }
|
