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/landmark/LandmarkOperation.hpp>
10 : : #include <ad/map/lane/LaneOperation.hpp>
11 : : #include <ad/map/test_support/NoLogTestMacros.hpp>
12 : : #include "LandmarkTestBase.hpp"
13 : :
14 : : struct LandmarkOperationTests : public LandmarkTestBase
15 : : {
16 : : };
17 : :
18 : 2 : TEST_F(LandmarkOperationTests, TestLandmark)
19 : : {
20 : 1 : GeoPoint point = createGeoPoint(Longitude(8.44038283714), Latitude(49.0199362465), Altitude(115.));
21 [ + - ]: 1 : ECEFPoint position = toECEF(point);
22 : :
23 : : // setup a landmark
24 : 1 : landmark::LandmarkId landmarkId(1234);
25 : 1 : GeoPoint orientation = createGeoPoint(Longitude(8.44038283714), Latitude(49.0), Altitude(115.));
26 [ + - ]: 1 : ECEFPoint orientationECEF = toECEF(orientation);
27 [ + - + - ]: 2 : Geometry boundingBox = createGeometry({position, orientationECEF}, true);
28 : :
29 [ + - + - : 2 : EXPECT_THROW(landmark::uniqueLandmarkId(point), std::invalid_argument);
+ - - + -
+ - - - -
- - - - ]
30 : :
31 [ + - - + : 1 : EXPECT_TRUE(mFactory->addLandmark(
- - - - -
- - - ]
32 : : mPartitionId, landmarkId, landmark::LandmarkType::UNKNOWN, position, orientationECEF, boundingBox));
33 [ + - + - : 1 : EXPECT_EQ(landmark::getLandmarks().size(), 1u);
- + - - -
- - - ]
34 : :
35 : 1 : landmark::LandmarkId result;
36 [ + - ]: 1 : result = landmark::uniqueLandmarkId(point);
37 [ + - - + : 1 : ASSERT_EQ(result, landmarkId);
- - - - -
- ]
38 : 1 : point = createGeoPoint(Longitude(8.44038283714), Latitude(51.0199362465), Altitude(115.));
39 [ + - + - : 2 : EXPECT_THROW(landmark::uniqueLandmarkId(point), std::invalid_argument);
+ - - + -
+ - - - -
- - - - ]
40 : :
41 : : // add the landmark as visible landmark to a lane
42 [ + - - + : 1 : EXPECT_TRUE(mFactory->add(mLaneId, landmarkId));
- - - - -
- - - ]
43 : :
44 [ + - + - ]: 2 : const auto landmark = landmark::getLandmark(landmarkId);
45 [ + - + - : 1 : EXPECT_NEAR(double(landmark::getENUHeading(landmark)), -M_PI / 2., 1e-3);
- + - - -
- - - ]
46 : :
47 [ + - + - ]: 1 : reinitFromWrittenMap("test_files/test_landmark.adm");
48 : :
49 : : // check if the landmark is in the store
50 [ + - + - : 1 : EXPECT_EQ(landmark::getLandmarks().size(), 1u);
- + - - -
- - - ]
51 [ + - + - : 1 : EXPECT_TRUE(landmark::getLandmarks().front() == landmarkId);
- + - - -
- - - -
- ]
52 : :
53 : : // get the lane from the store and check the visible landmarks
54 [ + - + - : 1 : EXPECT_TRUE(lane::getLanes().front() == mLaneId);
- + - - -
- - - -
- ]
55 [ + - + - ]: 2 : auto lane = lane::getLane(mLaneId);
56 [ + - - + : 1 : EXPECT_EQ(lane.visibleLandmarks.size(), 1u);
- - - - -
- ]
57 [ + - - + : 1 : EXPECT_TRUE(lane.visibleLandmarks.front() == landmarkId);
- - - - -
- - - ]
58 : : }
59 : :
60 : 2 : TEST_F(LandmarkOperationTests, TestLandmark_Orientation)
61 : : {
62 : 1 : GeoPoint point = createGeoPoint(Longitude(8.4), Latitude(49.1), Altitude(115.));
63 [ + - ]: 1 : ECEFPoint position = toECEF(point);
64 : :
65 [ + - ]: 1 : ECEFPoint orientation1 = toECEF(createGeoPoint(Longitude(8.4), Latitude(49.2), Altitude(115.)));
66 [ + - ]: 1 : ECEFPoint orientation2 = toECEF(createGeoPoint(Longitude(8.4), Latitude(49.), Altitude(115.)));
67 [ + - ]: 1 : ECEFPoint orientation3 = toECEF(createGeoPoint(Longitude(8.5), Latitude(49.1), Altitude(115.)));
68 [ + - ]: 1 : ECEFPoint orientation4 = toECEF(createGeoPoint(Longitude(8.3), Latitude(49.1), Altitude(115.)));
69 : :
70 : : // setup a landmark
71 [ + - + - ]: 3 : Geometry boundingBox = createGeometry({orientation1, orientation4}, true);
72 [ + - - + : 1 : EXPECT_TRUE(mFactory->addLandmark(
- - - - -
- - - ]
73 : : mPartitionId, landmark::LandmarkId(1), landmark::LandmarkType::UNKNOWN, position, orientation1, boundingBox));
74 : :
75 [ + - - + : 1 : EXPECT_TRUE(mFactory->addLandmark(
- - - - -
- - - ]
76 : : mPartitionId, landmark::LandmarkId(2), landmark::LandmarkType::UNKNOWN, position, orientation2, boundingBox));
77 : :
78 [ + - - + : 1 : EXPECT_TRUE(mFactory->addLandmark(
- - - - -
- - - ]
79 : : mPartitionId, landmark::LandmarkId(3), landmark::LandmarkType::UNKNOWN, position, orientation3, boundingBox));
80 : :
81 [ + - - + : 1 : EXPECT_TRUE(mFactory->addLandmark(
- - - - -
- - - ]
82 : : mPartitionId, landmark::LandmarkId(4), landmark::LandmarkType::UNKNOWN, position, orientation4, boundingBox));
83 : :
84 [ + - + - : 1 : EXPECT_NEAR(double(landmark::getENUHeading(landmark::getLandmark(landmark::LandmarkId(1)))), M_PI / 2., 1e-3);
+ - - + -
- - - -
- ]
85 [ + - + - : 1 : EXPECT_NEAR(double(landmark::getENUHeading(landmark::getLandmark(landmark::LandmarkId(2)))), -M_PI / 2., 1e-3);
+ - - + -
- - - -
- ]
86 [ + - + - : 1 : EXPECT_NEAR(double(landmark::getENUHeading(landmark::getLandmark(landmark::LandmarkId(3)))), 0., 1e-3);
+ - - + -
- - - -
- ]
87 [ + - + - : 1 : EXPECT_NEAR(double(landmark::getENUHeading(landmark::getLandmark(landmark::LandmarkId(4)))), M_PI, 1e-3);
+ - - + -
- - - -
- ]
88 : 1 : }
89 : :
90 : 2 : TEST_F(LandmarkOperationTests, TestTrafficLight)
91 : : {
92 : : // setup a traffic light
93 : 1 : landmark::LandmarkId landmarkId(1234);
94 : 1 : ECEFPoint orientation = createECEFPoint(0., 0., 0.);
95 : 1 : ECEFPoint position = createECEFPoint(1., 0., 0.);
96 [ + - + - ]: 2 : Geometry bounding_box = createGeometry({position, orientation}, false);
97 [ + - - + : 1 : EXPECT_TRUE(mFactory->addTrafficLight(
- - - - -
- - - ]
98 : : mPartitionId, landmarkId, landmark::TrafficLightType::SOLID_RED_YELLOW_GREEN, position, orientation, bounding_box));
99 [ + - + - : 1 : EXPECT_EQ(landmark::getLandmarks().size(), 1u);
- + - - -
- - - ]
100 : :
101 : : // add the traffic light as contact for two lanes
102 [ + - ]: 1 : lane::LaneId x12 = addLane();
103 [ + - ]: 1 : lane::LaneId x13 = addLane();
104 [ + - - + : 1 : ASSERT_TRUE(isValid(x12));
- - - - -
- - - ]
105 [ + - - + : 1 : ASSERT_TRUE(isValid(x13));
- - - - -
- - - ]
106 : :
107 : 1 : lane::ContactLocation succ(lane::ContactLocation::SUCCESSOR);
108 [ + - ]: 2 : lane::ContactTypeList traffic_light_contact({lane::ContactType::TRAFFIC_LIGHT});
109 [ + - ]: 2 : lane::ContactTypeList free_contact({lane::ContactType::FREE});
110 : 2 : restriction::Restrictions no_restrictions;
111 : :
112 : : // valid contact, type: traffic light and valid traffic light id
113 [ + - - + : 1 : EXPECT_TRUE(mFactory->add(mLaneId, x12, succ, traffic_light_contact, no_restrictions, landmarkId));
- - - - -
- - - ]
114 : : // invalid contact, type: traffic light and no traffic light id
115 [ + - + - : 1 : EXPECT_FALSE_NO_LOG(mFactory->add(x12, x13, succ, traffic_light_contact, no_restrictions));
+ - + - -
+ - - - -
- - - - +
- ]
116 : : // invalid contact, type: free and valid traffic light id
117 [ + - + - : 1 : EXPECT_FALSE_NO_LOG(mFactory->add(x12, x13, succ, free_contact, no_restrictions, landmarkId));
+ - + - -
+ - - - -
- - - - +
- ]
118 : :
119 : : // add the traffic light also as visible landmark to the lane mLaneId
120 [ + - - + : 1 : EXPECT_TRUE(mFactory->add(mLaneId, landmarkId));
- - - - -
- - - ]
121 : :
122 [ + - + - ]: 1 : reinitFromWrittenMap("test_files/test_traffic_light.adm");
123 : :
124 : : // check that the traffic light is in the store
125 [ + - + - : 1 : EXPECT_EQ(landmark::getLandmarks().size(), 1u);
- + - - -
- - - ]
126 [ + - + - : 1 : EXPECT_TRUE(landmark::getLandmarks().front() == landmarkId);
- + - - -
- - - -
- ]
127 [ + - + - ]: 2 : auto traffic_light = landmark::getLandmark(landmarkId);
128 [ - + - - : 1 : EXPECT_TRUE(traffic_light.type == landmark::LandmarkType::TRAFFIC_LIGHT);
- - - - -
- ]
129 [ - + - - : 1 : EXPECT_TRUE(traffic_light.trafficLightType == landmark::TrafficLightType::SOLID_RED_YELLOW_GREEN);
- - - - -
- ]
130 : :
131 : : // check the lane contact in the store
132 [ + - + - : 1 : EXPECT_TRUE(lane::getLanes().front() == mLaneId);
- + - - -
- - - -
- ]
133 [ + - + - ]: 2 : auto lane = lane::getLane(mLaneId);
134 [ + - - + : 1 : EXPECT_EQ(lane.visibleLandmarks.size(), 1u);
- - - - -
- ]
135 [ + - - + : 1 : EXPECT_TRUE(lane.visibleLandmarks.front() == landmarkId);
- - - - -
- - - ]
136 [ + - ]: 2 : auto contact = lane.contactLanes.front();
137 [ + - - + : 1 : EXPECT_TRUE(contact.trafficLightId == landmarkId);
- - - - -
- - - ]
138 [ - + - - : 1 : EXPECT_TRUE(contact.types.front() == lane::ContactType::TRAFFIC_LIGHT);
- - - - -
- ]
139 : : }
140 : :
141 : 2 : TEST_F(LandmarkOperationTests, TestTrafficSign)
142 : : {
143 : : // setup a landmark
144 : 1 : landmark::LandmarkId landmarkId(1234);
145 : 1 : ECEFPoint orientation = createECEFPoint(0., 0., 0.);
146 : 1 : ECEFPoint position = createECEFPoint(1., 0., 0.);
147 [ + - + - ]: 3 : Geometry bounding_box = createGeometry({position, orientation}, false);
148 [ + - ]: 2 : std::string text("60");
149 [ + - - + : 1 : EXPECT_TRUE(mFactory->addTrafficSign(
- - - - -
- - - ]
150 : : mPartitionId, landmarkId, landmark::TrafficSignType::MAX_SPEED, position, orientation, bounding_box, text));
151 [ + - + - : 1 : EXPECT_EQ(landmark::getLandmarks().size(), 1u);
- + - - -
- - - ]
152 : :
153 : : // add the landmark as visible landmark to a lane
154 [ + - - + : 1 : EXPECT_TRUE(mFactory->add(mLaneId, landmarkId));
- - - - -
- - - ]
155 : :
156 [ + - + - ]: 1 : reinitFromWrittenMap("test_files/test_traffic_sign.adm");
157 : :
158 : : // check if the landmark is in the store
159 [ + - + - : 1 : EXPECT_EQ(landmark::getLandmarks().size(), 1u);
- + - - -
- - - ]
160 [ + - + - : 1 : EXPECT_TRUE(landmark::getLandmarks().front() == landmarkId);
- + - - -
- - - -
- ]
161 : :
162 [ + - + - : 2 : EXPECT_THROW_NO_LOG(landmark::getLandmarkPtr(landmark::LandmarkId(1000)), std::invalid_argument);
+ - + - +
- + - - +
- + - - -
- - - + -
- - ]
163 : :
164 [ + - + - : 2 : EXPECT_EQ(landmark::getLandmarkPtr(landmarkId)->supplementaryText, "60");
- + - - -
- - - ]
165 : :
166 : : // get the lane from the store and check the visible landmarks
167 [ + - + - : 1 : EXPECT_TRUE(lane::getLanes().front() == mLaneId);
- + - - -
- - - -
- ]
168 [ + - + - ]: 2 : auto lane = lane::getLane(mLaneId);
169 [ + - - + : 1 : EXPECT_EQ(lane.visibleLandmarks.size(), 1u);
- - - - -
- ]
170 [ + - - + : 1 : EXPECT_TRUE(lane.visibleLandmarks.front() == landmarkId);
- - - - -
- - - ]
171 : 1 : }
|
