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 "SyntheticIntersectionTestBase.hpp"
10 : :
11 : : #include <ad/map/access/Operation.hpp>
12 : : #include <algorithm>
13 : :
14 : : // comparison operator for sorting
15 : 1310 : bool smaller(const ad::map::point::ParaPoint &left, const ad::map::point::ParaPoint &right)
16 : : {
17 : 1310 : return (left.laneId < right.laneId)
18 [ + + - + : 1310 : || ((left.laneId == right.laneId) && (left.parametricOffset < right.parametricOffset));
- - ]
19 : : }
20 : :
21 : : namespace ad {
22 : : namespace map {
23 : :
24 : 196 : lane::LaneIdSet SyntheticIntersectionTestBase::getIncomingLanesWithHigherPriority() const
25 : : {
26 : 196 : auto resultSet = getIncomingLanes();
27 [ + - + + ]: 592 : for (auto const &lowerPrioLane : expectedIncomingLanesWithLowerPriority())
28 : : {
29 [ + - ]: 396 : resultSet.erase(lowerPrioLane);
30 : : }
31 : 196 : return resultSet;
32 : : }
33 : :
34 : 98 : point::ParaPointList SyntheticIntersectionTestBase::getIncomingParaPointsWithHigherPriority() const
35 : : {
36 [ + - ]: 98 : auto resultVector = getIncomingParaPoints();
37 [ + - ]: 196 : auto const incomingLanesWithLowerPriority = getIncomingLanesWithLowerPriority();
38 : 0 : resultVector.erase(std::remove_if(resultVector.begin(),
39 : : resultVector.end(),
40 : 295 : [&incomingLanesWithLowerPriority](point::ParaPoint const ¶Point) {
41 : 295 : return incomingLanesWithLowerPriority.count(paraPoint.laneId) > 0;
42 [ + - ]: 98 : }),
43 [ + - ]: 196 : resultVector.end());
44 : 196 : return resultVector;
45 : : }
46 : :
47 : 196 : lane::LaneIdSet SyntheticIntersectionTestBase::getIncomingLanesWithLowerPriority() const
48 : : {
49 : 196 : auto resultSet = getIncomingLanes();
50 [ + - + + ]: 390 : for (auto const &higherPrioLane : expectedIncomingLanesWithHigherPriority())
51 : : {
52 [ + - ]: 194 : resultSet.erase(higherPrioLane);
53 : : }
54 : 196 : return resultSet;
55 : : }
56 : :
57 : 98 : point::ParaPointList SyntheticIntersectionTestBase::getIncomingParaPointsWithLowerPriority() const
58 : : {
59 [ + - ]: 98 : auto resultVector = getIncomingParaPoints();
60 [ + - ]: 196 : auto const incomingLanesWithHigherPriority = getIncomingLanesWithHigherPriority();
61 : 0 : resultVector.erase(std::remove_if(resultVector.begin(),
62 : : resultVector.end(),
63 : 295 : [&incomingLanesWithHigherPriority](point::ParaPoint const ¶Point) {
64 : 295 : return incomingLanesWithHigherPriority.count(paraPoint.laneId) > 0;
65 [ + - ]: 98 : }),
66 [ + - ]: 196 : resultVector.end());
67 : 196 : return resultVector;
68 : : }
69 : :
70 : 76 : lane::LaneIdSet SyntheticIntersectionTestBase::getCrossingLanes() const
71 : : {
72 : 76 : return expectedCrossingLanes();
73 : : }
74 : :
75 : 294 : void SyntheticIntersectionTestBase::compareVectors(point::ParaPointList left, point::ParaPointList right) const
76 : : {
77 : 294 : std::sort(left.begin(), left.end(), &smaller);
78 : 294 : std::sort(right.begin(), right.end(), &smaller);
79 [ + - - + : 294 : ASSERT_EQ(left, right);
- - - - -
- ]
80 : : }
81 : :
82 : 544 : void SyntheticIntersectionTestBase::compareLists(lane::LaneIdSet ids, lane::LaneIdSet otherIds) const
83 : : {
84 [ + - - + : 544 : ASSERT_EQ(ids, otherIds);
- - - - -
- ]
85 : : }
86 : :
87 : 98 : void SyntheticIntersectionTestBase::performBasicChecks()
88 : : {
89 [ + - + - : 98 : ASSERT_EQ(expectedIntersectionType(), mIntersection->intersectionType());
+ - - + -
- - - -
- ]
90 : :
91 [ + - ]: 196 : SCOPED_TRACE("internal lanes");
92 [ + - + - : 98 : compareLists(getInternalLanes(), mIntersection->internalLanes());
+ - + - ]
93 [ + - ]: 196 : SCOPED_TRACE("incoming lanes");
94 [ + - + - : 98 : compareLists(getIncomingLanes(), mIntersection->incomingLanes());
+ - + - ]
95 [ + - ]: 196 : SCOPED_TRACE("incoming lanes with higher prio");
96 [ + - + - : 98 : compareLists(getIncomingLanesWithHigherPriority(), mIntersection->incomingLanesWithHigherPriority());
+ - + - ]
97 [ + - ]: 196 : SCOPED_TRACE("incoming lanes with lower prio");
98 [ + - + - : 98 : compareLists(getIncomingLanesWithLowerPriority(), mIntersection->incomingLanesWithLowerPriority());
+ - + - ]
99 : :
100 [ + - + - : 98 : compareVectors(getIncomingParaPoints(), mIntersection->incomingParaPoints());
+ - + - ]
101 [ + - + - : 98 : compareVectors(getIncomingParaPointsWithHigherPriority(), mIntersection->incomingParaPointsWithHigherPriority());
+ - + - ]
102 [ + - + - : 98 : compareVectors(getIncomingParaPointsWithLowerPriority(), mIntersection->incomingParaPointsWithLowerPriority());
+ - + - ]
103 : : //@todo verify the geometric problems with the crossing lanes
104 [ + - + + ]: 98 : if (mIntersection->intersectionType() != intersection::IntersectionType::TrafficLight)
105 : : {
106 [ + - ]: 152 : SCOPED_TRACE("internal lanes with higher prio");
107 [ + - + - : 76 : compareLists(expectedInternalLanesWithHigherPriority(), mIntersection->internalLanesWithHigherPriority());
+ - + - ]
108 [ + - ]: 76 : SCOPED_TRACE("crossing lanes");
109 [ + - + - : 76 : compareLists(getCrossingLanes(), mIntersection->crossingLanes());
+ - + - ]
110 : : }
111 : : }
112 : :
113 : 4 : TrafficLightForTest SyntheticIntersectionTestBase::expectedTrafficLight(uint64_t landmarkId,
114 : : landmark::TrafficLightType type) const
115 : : {
116 : 4 : TrafficLightForTest expected;
117 : 4 : expected.id = landmark::LandmarkId(landmarkId);
118 : 4 : expected.type = type;
119 : 4 : return expected;
120 : : }
121 : :
122 : 33 : TrafficLightForTest SyntheticIntersectionTestBase::expectedTrafficLight(uint64_t landmarkId) const
123 : : {
124 : 33 : TrafficLightForTest expected;
125 : 33 : expected.id = landmark::LandmarkId(landmarkId);
126 : 33 : expected.type = landmark::TrafficLightType::UNKNOWN;
127 : 33 : return expected;
128 : : }
129 : :
130 : 21 : void SyntheticIntersectionTestBase::performBasicTrafficLightsChecks(
131 : : std::vector<TrafficLightForTest> expectedTrafficLights /* = {}*/)
132 : : {
133 : 21 : auto const &trafficLights = mIntersection->applicableTrafficLights();
134 [ + - - + : 21 : ASSERT_EQ(expectedTrafficLights.size(), trafficLights.size()) << "Number of traffic lights don't match";
- - - - -
- - - ]
135 : :
136 [ + + ]: 58 : for (auto expectedTrafficLight : expectedTrafficLights)
137 : : {
138 [ + - + - : 37 : ASSERT_EQ(1u, trafficLights.count(expectedTrafficLight.id))
- + - - -
- - - ]
139 [ # # # # : 0 : << "Traffic Light ID=" << static_cast<uint64_t>(expectedTrafficLight.id) << " NOT FOUND";
# # ]
140 [ + + ]: 37 : if (expectedTrafficLight.type != landmark::TrafficLightType::UNKNOWN)
141 : : {
142 [ + - + - : 4 : ASSERT_EQ(mIntersection->extractTrafficLightType(expectedTrafficLight.id), expectedTrafficLight.type)
- + - - -
- - - ]
143 [ # # ]: 0 : << "Traffic Light type don't match for"
144 [ # # ]: 0 : << " Traffic Light ID=";
145 : : }
146 : : }
147 : :
148 : 21 : performBasicChecks();
149 : : }
150 : :
151 : : } // namespace map
152 : : } // namespace ad
|
