Branch data Line data Source code
1 : :
2 : : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
3 : : //
4 : : // Copyright (C) 2020-2021 Intel Corporation
5 : : //
6 : : // SPDX-License-Identifier: LGPL-2.1-only
7 : : //
8 : : // ----------------- END LICENSE BLOCK -----------------------------------
9 : : /**
10 : : * @file
11 : : */
12 : :
13 : : #include "TrajectoryCommon.hpp"
14 : : #include <ad/physics/Operation.hpp>
15 : : #include "ad/rss/unstructured/DebugDrawing.hpp"
16 : :
17 : : /*!
18 : : * @brief namespace ad
19 : : */
20 : : namespace ad {
21 : : /*!
22 : : * @brief namespace rss
23 : : */
24 : : namespace rss {
25 : : /*!
26 : : * @brief namespace unstructured
27 : : */
28 : : namespace unstructured {
29 : :
30 : 3040 : Point getVehicleCorner(TrajectoryPoint const &point,
31 : : ad::physics::Dimension2D const &vehicleDimension,
32 : : VehicleCorner const corner)
33 : : {
34 : : Point resultPoint;
35 [ + - ]: 3040 : auto const vehicleAngle = point.angle - ad::physics::cPI_2;
36 [ + + + + : 3040 : switch (corner)
- ]
37 : : {
38 : 760 : case VehicleCorner::frontLeft:
39 : : resultPoint = rotateAroundPoint(
40 [ + - + - : 760 : point.position, toPoint(-vehicleDimension.width / 2.0, vehicleDimension.length / 2.0), vehicleAngle);
+ - + - +
- ]
41 : 760 : break;
42 : 760 : case VehicleCorner::frontRight:
43 : : resultPoint = rotateAroundPoint(
44 [ + - + - : 760 : point.position, toPoint(vehicleDimension.width / 2.0, vehicleDimension.length / 2.0), vehicleAngle);
+ - + - ]
45 : 760 : break;
46 : 760 : case VehicleCorner::backLeft:
47 : : resultPoint = rotateAroundPoint(
48 [ + - + - : 760 : point.position, toPoint(-vehicleDimension.width / 2.0, -vehicleDimension.length / 2.0), vehicleAngle);
+ - + - +
- + - ]
49 : 760 : break;
50 : 760 : case VehicleCorner::backRight:
51 : : resultPoint = rotateAroundPoint(
52 [ + - + - : 760 : point.position, toPoint(vehicleDimension.width / 2.0, -vehicleDimension.length / 2.0), vehicleAngle);
+ - + - +
- ]
53 : 760 : break;
54 : 0 : default:
55 [ # # ]: 0 : throw std::runtime_error("unstructured::getVehicleCorner>> invalid corner requested");
56 : : break;
57 : : }
58 : 3040 : return resultPoint;
59 : : }
60 : :
61 : 120 : bool calculateStepPolygon(situation::VehicleState const &vehicleState,
62 : : TrajectorySetStep const &step,
63 : : std::string const &debugNamespace,
64 : : Polygon &polygon,
65 : : TrajectorySetStepVehicleLocation &stepVehicleLocation)
66 : : {
67 : 240 : MultiPoint frontPtsLeft;
68 : 240 : MultiPoint frontPtsRight;
69 : :
70 : 120 : int idx = 0;
71 [ + + ]: 440 : for (auto it = step.left.cbegin(); it != step.left.cend(); ++it)
72 : : {
73 [ + - ]: 320 : auto vehicleLocationLeft = TrafficParticipantLocation(*it, vehicleState);
74 [ + - + - : 320 : DEBUG_DRAWING_POLYGON(vehicleLocationLeft.toPolygon(), "blue", debugNamespace + "_left_" + std::to_string(idx));
+ - + - +
- + - +
- ]
75 [ + - + - ]: 320 : boost::geometry::append(frontPtsLeft, vehicleLocationLeft.toMultiPoint());
76 [ + + ]: 320 : if (it == step.left.end() - 1)
77 : : {
78 : 120 : stepVehicleLocation.left = vehicleLocationLeft;
79 : : }
80 : 320 : ++idx;
81 : : }
82 : :
83 : : // center
84 [ + - ]: 120 : auto vehicleLocationCenter = TrafficParticipantLocation(step.center, vehicleState);
85 [ + - + - : 120 : DEBUG_DRAWING_POLYGON(vehicleLocationCenter.toPolygon(), "blue", debugNamespace + "_center");
+ - + - +
- ]
86 [ + - + - ]: 120 : boost::geometry::append(frontPtsLeft, vehicleLocationCenter.toMultiPoint());
87 : 120 : stepVehicleLocation.center = vehicleLocationCenter;
88 [ + - + - ]: 120 : boost::geometry::append(frontPtsRight, vehicleLocationCenter.toMultiPoint());
89 : :
90 : 120 : idx = 0;
91 [ + + ]: 440 : for (auto it = step.right.cbegin(); it != step.right.cend(); ++it)
92 : : {
93 [ + - ]: 320 : auto vehicleLocationRight = TrafficParticipantLocation(*it, vehicleState);
94 [ + - + - : 320 : DEBUG_DRAWING_POLYGON(vehicleLocationRight.toPolygon(), "blue", debugNamespace + "_right_" + std::to_string(idx));
+ - + - +
- + - +
- ]
95 [ + - + - ]: 320 : boost::geometry::append(frontPtsRight, vehicleLocationRight.toMultiPoint());
96 [ + + ]: 320 : if (it == step.right.begin())
97 : : {
98 : 120 : stepVehicleLocation.right = vehicleLocationRight;
99 : : }
100 : 320 : ++idx;
101 : : }
102 : :
103 [ + - ]: 240 : Polygon hullLeft;
104 [ + - ]: 120 : Polygon hullRight;
105 [ + - ]: 120 : boost::geometry::convex_hull(frontPtsLeft, hullLeft);
106 [ + - ]: 120 : boost::geometry::convex_hull(frontPtsRight, hullRight);
107 [ + - ]: 120 : auto result = combinePolygon(hullLeft, hullRight, polygon);
108 : 240 : return result;
109 : : }
110 : :
111 : 100 : bool calculateEstimationBetweenSteps(Polygon &polygon,
112 : : TrajectorySetStepVehicleLocation const &previousStepVehicleLocation,
113 : : TrajectorySetStepVehicleLocation const ¤tStepVehicleLocation,
114 : : std::string const &debugNamespace)
115 : : {
116 : : // Fill potential gap between two calculation steps by using the previous and current step
117 [ + - + + ]: 100 : if (previousStepVehicleLocation == currentStepVehicleLocation)
118 : : {
119 : 40 : return true;
120 : : }
121 : :
122 [ + - ]: 120 : Polygon hull;
123 : 120 : MultiPoint interimPtsLeft;
124 [ + - + - ]: 60 : boost::geometry::append(interimPtsLeft, previousStepVehicleLocation.left.toMultiPoint());
125 [ + - + - ]: 60 : boost::geometry::append(interimPtsLeft, previousStepVehicleLocation.center.toMultiPoint());
126 [ + - + - ]: 60 : boost::geometry::append(interimPtsLeft, currentStepVehicleLocation.left.toMultiPoint());
127 [ + - + - ]: 60 : boost::geometry::append(interimPtsLeft, currentStepVehicleLocation.center.toMultiPoint());
128 [ + - ]: 120 : Polygon hullLeft;
129 [ + - ]: 60 : boost::geometry::convex_hull(interimPtsLeft, hullLeft);
130 : :
131 : 120 : MultiPoint interimPtsRight;
132 [ + - + - ]: 60 : boost::geometry::append(interimPtsRight, previousStepVehicleLocation.right.toMultiPoint());
133 [ + - + - ]: 60 : boost::geometry::append(interimPtsRight, previousStepVehicleLocation.center.toMultiPoint());
134 [ + - + - ]: 60 : boost::geometry::append(interimPtsRight, currentStepVehicleLocation.right.toMultiPoint());
135 [ + - + - ]: 60 : boost::geometry::append(interimPtsRight, currentStepVehicleLocation.center.toMultiPoint());
136 [ + - ]: 60 : Polygon hullRight;
137 [ + - ]: 60 : boost::geometry::convex_hull(interimPtsRight, hullRight);
138 [ + - ]: 60 : auto result = combinePolygon(hullRight, hullLeft, hull);
139 [ - + ]: 60 : if (!result)
140 : : {
141 [ # # ]: 0 : spdlog::debug("unstructured::calculateEstimationBetweenSteps>> Could not create estimation polygon ({})."
142 : : "left {}, right {}",
143 : : debugNamespace,
144 [ # # ]: 0 : std::to_string(hullLeft),
145 [ # # ]: 0 : std::to_string(hullRight));
146 : : }
147 : :
148 [ + - ]: 60 : if (result)
149 : : {
150 [ + - + - : 60 : DEBUG_DRAWING_POLYGON(hull, "yellow", debugNamespace + "_hull_front");
+ - + - ]
151 [ + - ]: 60 : result = combinePolygon(polygon, hull, polygon);
152 [ - + ]: 60 : if (!result)
153 : : {
154 [ # # ]: 0 : spdlog::warn("unstructured::calculateEstimationBetweenSteps>> Could not combine front estimation polygon ({}) "
155 : : "with final polygon."
156 : : "polygon {}, hull {}",
157 : : debugNamespace,
158 [ # # ]: 0 : std::to_string(polygon),
159 [ # # ]: 0 : std::to_string(hull));
160 : : }
161 : : }
162 : 60 : return result;
163 : : }
164 : :
165 : 40 : bool calculateFrontAndSidePolygon(situation::VehicleState const &vehicleState,
166 : : TrajectorySetStepVehicleLocation const &initialStepVehicleLocation,
167 : : std::vector<TrajectorySetStep> const &sideSteps,
168 : : TrajectorySetStep const &front,
169 : : std::string const &debugNamespace,
170 : : Polygon &resultPolygon,
171 : : TrajectorySetStepVehicleLocation &frontSideStepVehicleLocation)
172 : : {
173 : 40 : auto result = true;
174 : 40 : auto previousStepVehicleLocation = initialStepVehicleLocation;
175 : : //-------------
176 : : // sides
177 : : //-------------
178 : 40 : auto idx = 0;
179 [ + + + - : 100 : for (auto sideStepIt = sideSteps.cbegin(); (sideStepIt != sideSteps.cend()) && result; ++sideStepIt)
+ + ]
180 : : {
181 [ + - ]: 60 : Polygon stepPolygon;
182 : 60 : TrajectorySetStepVehicleLocation currentStepVehicleLocation;
183 [ + - ]: 60 : result = calculateStepPolygon(
184 [ + - + - : 120 : vehicleState, *sideStepIt, debugNamespace + "_" + std::to_string(idx), stepPolygon, currentStepVehicleLocation);
+ - ]
185 [ - + ]: 60 : if (!result)
186 : : {
187 [ # # ]: 0 : spdlog::debug("unstructured::calculateFrontAndSidePolygon>> Could not calculate step polygon");
188 : : }
189 [ + - ]: 60 : if (result)
190 : : {
191 [ + - ]: 60 : result = calculateEstimationBetweenSteps(resultPolygon,
192 : : previousStepVehicleLocation,
193 : : currentStepVehicleLocation,
194 [ + - + - : 120 : debugNamespace + "_step_estimation_" + std::to_string(idx));
+ - ]
195 [ - + ]: 60 : if (!result)
196 : : {
197 [ # # ]: 0 : spdlog::debug("unstructured::calculateFrontAndSidePolygon>> Could not calculate between steps");
198 : : }
199 : 60 : previousStepVehicleLocation = currentStepVehicleLocation;
200 : : }
201 : :
202 [ + - ]: 60 : if (result)
203 : : {
204 [ + - ]: 60 : result = combinePolygon(resultPolygon, stepPolygon, resultPolygon);
205 : : }
206 : 60 : idx++;
207 : : }
208 : :
209 : : //-------------
210 : : // front
211 : : //-------------
212 [ + - ]: 40 : Polygon frontPolygon;
213 [ + - ]: 40 : if (result)
214 : : {
215 [ + - ]: 40 : result = calculateStepPolygon(
216 [ + - ]: 80 : vehicleState, front, debugNamespace + "_front", frontPolygon, frontSideStepVehicleLocation);
217 : : }
218 [ + - ]: 40 : if (result)
219 : : {
220 [ + - ]: 40 : result = calculateEstimationBetweenSteps(
221 [ + - ]: 80 : resultPolygon, previousStepVehicleLocation, frontSideStepVehicleLocation, debugNamespace + "_front_estimation");
222 [ - + ]: 40 : if (!result)
223 : : {
224 [ # # ]: 0 : spdlog::debug("unstructured::calculateFrontAndSidePolygon>> Could not calculate between last step and "
225 : : "front polygon.");
226 : : }
227 : : }
228 [ + - ]: 40 : if (result)
229 : : {
230 [ + - ]: 40 : result = combinePolygon(resultPolygon, frontPolygon, resultPolygon);
231 : : }
232 : 80 : return result;
233 : : }
234 : :
235 : : } // namespace unstructured
236 : : } // namespace rss
237 : : } // namespace ad
|
