Branch data Line data Source code
1 : : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
2 : : //
3 : : // Copyright (C) 2020-2021 Intel Corporation
4 : : //
5 : : // SPDX-License-Identifier: LGPL-2.1-only
6 : : //
7 : : // ----------------- END LICENSE BLOCK -----------------------------------
8 : : /**
9 : : * @file
10 : : */
11 : :
12 : : #pragma once
13 : :
14 : : #include <ad/geometry/GeometryOperation.hpp>
15 : : #include <ad/geometry/Types.hpp>
16 : : #include <ad/physics/Angle.hpp>
17 : : #include <ad/physics/Dimension2D.hpp>
18 : : #include <ad/physics/Distance.hpp>
19 : : #include "ad/rss/core/RelativeObjectState.hpp"
20 : :
21 : : /*!
22 : : * @brief namespace ad
23 : : */
24 : : namespace ad {
25 : : /*!
26 : : * @brief namespace rss
27 : : */
28 : : namespace rss {
29 : : /*!
30 : : * @brief namespace unstructured
31 : : */
32 : : namespace unstructured {
33 : :
34 : : /**
35 : : * @brief corner of a vehicle
36 : : */
37 : : enum class VehicleCorner
38 : : {
39 : : frontLeft,
40 : : frontRight,
41 : : backLeft,
42 : : backRight
43 : : };
44 : :
45 : : /**
46 : : * @brief a point of a trajectory
47 : : */
48 : : struct TrajectoryPoint
49 : : {
50 : 616 : TrajectoryPoint()
51 : 616 : {
52 : 616 : }
53 : :
54 : : enum class SpeedMode
55 : : {
56 : : Min,
57 : : Max
58 : : };
59 : :
60 : 342 : explicit TrajectoryPoint(core::RelativeObjectState const &vehicleState, SpeedMode const speedMode)
61 : 342 : {
62 : 342 : position = ::ad::geometry::toPoint(vehicleState.unstructured_object_state.center_point);
63 : 342 : angle = vehicleState.unstructured_object_state.yaw;
64 [ + + ]: 342 : if (speedMode == SpeedMode::Min)
65 : : {
66 : 118 : speed = vehicleState.unstructured_object_state.speed_range.minimum;
67 : : }
68 : : else
69 : : {
70 : 224 : speed = vehicleState.unstructured_object_state.speed_range.maximum;
71 : : }
72 : 342 : yaw_rate = vehicleState.unstructured_object_state.yaw_rate;
73 : 342 : }
74 : :
75 : : TrajectoryPoint(::ad::geometry::Point const &inPoint,
76 : : physics::Angle const &inAngle,
77 : : physics::Speed const &inSpeed,
78 : : physics::AngularVelocity const &inYawRate)
79 : : : position(inPoint)
80 : : , speed(inSpeed)
81 : : , angle(inAngle)
82 : : , yaw_rate(inYawRate)
83 : : {
84 : : }
85 : :
86 : : /*!
87 : : * standard comparison operator
88 : : */
89 : : bool operator==(const TrajectoryPoint &other) const
90 : : {
91 : : return (position == other.position) && (speed == other.speed) && (angle == other.angle)
92 : : && (yaw_rate == other.yaw_rate);
93 : : }
94 : :
95 : : /*!
96 : : * standard comparison operator
97 : : */
98 : : bool operator!=(const TrajectoryPoint &other) const
99 : : {
100 : : return !operator==(other);
101 : : }
102 : :
103 : : /*!
104 : : * The current position
105 : : */
106 : : ::ad::geometry::Point position;
107 : :
108 : : /*!
109 : : * The current position
110 : : */
111 : : physics::Speed speed;
112 : :
113 : : /*!
114 : : * The current heading angle
115 : : */
116 : : physics::Angle angle;
117 : :
118 : : /*!
119 : : * The current yaw_rate
120 : : */
121 : : physics::AngularVelocity yaw_rate;
122 : : };
123 : :
124 : : struct TrajectorySetStep
125 : : {
126 : 122 : TrajectorySetStep()
127 : 122 : {
128 : 122 : }
129 : :
130 : : TrajectorySetStep(TrajectoryPoint const &inLeft, TrajectoryPoint const &inRight, TrajectoryPoint const &inCenter)
131 : : : center(inCenter)
132 : : {
133 : : left.push_back(inLeft);
134 : : right.push_back(inRight);
135 : : }
136 : :
137 : : /*!
138 : : * standard comparison operator
139 : : */
140 : : bool operator==(const TrajectorySetStep &other) const
141 : : {
142 : : return (left == other.left) && (right == other.right) && (center == other.center);
143 : : }
144 : :
145 : : /*!
146 : : * standard comparison operator
147 : : */
148 : : bool operator!=(const TrajectorySetStep &other) const
149 : : {
150 : : return !operator==(other);
151 : : }
152 : :
153 : : std::vector<TrajectoryPoint> left; // with positive yaw rate ratio
154 : : std::vector<TrajectoryPoint> right; // with negative yaw rate ratio
155 : : TrajectoryPoint center;
156 : : };
157 : :
158 : : /**
159 : : * @brief get the point describing the corner of a vehicle
160 : : *
161 : : * @param[in] point trajectory point
162 : : * @param[in] vehicleState vehicle state to be considered
163 : : * @param[in] corner which corner to calculate
164 : : *
165 : : * @returns corner point of the vehicle
166 : : */
167 : : ::ad::geometry::Point getVehicleCorner(TrajectoryPoint const &point,
168 : : core::RelativeObjectState const &vehicleState,
169 : : VehicleCorner const corner);
170 : :
171 : : struct TrafficParticipantLocation
172 : : {
173 : 426 : TrafficParticipantLocation()
174 : 426 : {
175 : 426 : }
176 : :
177 : 976 : TrafficParticipantLocation(TrajectoryPoint const &pt, core::RelativeObjectState const &vehicleState)
178 : 976 : {
179 : 976 : frontLeft = getVehicleCorner(pt, vehicleState, VehicleCorner::frontLeft);
180 : 976 : frontRight = getVehicleCorner(pt, vehicleState, VehicleCorner::frontRight);
181 : 976 : backLeft = getVehicleCorner(pt, vehicleState, VehicleCorner::backLeft);
182 : 976 : backRight = getVehicleCorner(pt, vehicleState, VehicleCorner::backRight);
183 : 976 : }
184 : :
185 : 924 : ::ad::geometry::Polygon toPolygon() const
186 : : {
187 : 924 : ::ad::geometry::Polygon vehiclePolygon;
188 [ + - ]: 924 : boost::geometry::append(vehiclePolygon, frontRight);
189 [ + - ]: 924 : boost::geometry::append(vehiclePolygon, frontLeft);
190 [ + - ]: 924 : boost::geometry::append(vehiclePolygon, backLeft);
191 [ + - ]: 924 : boost::geometry::append(vehiclePolygon, backRight);
192 [ + - ]: 924 : boost::geometry::append(vehiclePolygon, frontRight);
193 : 924 : return vehiclePolygon;
194 : 0 : }
195 : 1856 : ::ad::geometry::MultiPoint toMultiPoint() const
196 : : {
197 : 1856 : ::ad::geometry::MultiPoint geometry;
198 [ + - ]: 1856 : boost::geometry::append(geometry, frontRight);
199 [ + - ]: 1856 : boost::geometry::append(geometry, frontLeft);
200 [ + - ]: 1856 : boost::geometry::append(geometry, backLeft);
201 [ + - ]: 1856 : boost::geometry::append(geometry, backRight);
202 [ + - ]: 1856 : boost::geometry::append(geometry, frontRight);
203 : 1856 : return geometry;
204 : 0 : }
205 : :
206 : 204 : bool operator==(TrafficParticipantLocation const &other) const
207 : : {
208 [ + - + - ]: 332 : return (frontLeft == other.frontLeft) && (frontRight == other.frontRight) && (backLeft == other.backLeft)
209 [ + + + - ]: 332 : && (backRight == other.backRight);
210 : : }
211 : : bool operator!=(TrafficParticipantLocation const &other) const
212 : : {
213 : : return !(*this == other);
214 : : }
215 : : ::ad::geometry::Point frontLeft;
216 : : ::ad::geometry::Point frontRight;
217 : : ::ad::geometry::Point backLeft;
218 : : ::ad::geometry::Point backRight;
219 : : };
220 : :
221 : : /*!
222 : : * a trajectory
223 : : */
224 : : using Trajectory = std::vector<TrajectoryPoint>;
225 : :
226 : : struct TrajectorySetStepVehicleLocation
227 : : {
228 : : TrafficParticipantLocation left;
229 : : TrafficParticipantLocation right;
230 : : TrafficParticipantLocation center;
231 : :
232 : 118 : bool operator==(TrajectorySetStepVehicleLocation const &other) const
233 : : {
234 [ + + + + : 118 : return (left == other.left) && (right == other.right) && (center == other.center);
+ - ]
235 : : }
236 : :
237 : : bool operator!=(TrajectorySetStepVehicleLocation const &other) const
238 : : {
239 : : return !(*this == other);
240 : : }
241 : : };
242 : :
243 : : /**
244 : : * @brief Calculate a trajectory set estimation between two steps
245 : : *
246 : : * @param[inout] polygon polygon to work on
247 : : * @param[in] previousVehicleLocation the previous possible vehicle locations
248 : : * @param[in] currentVehicleLocation the current possible vehicle locations
249 : : * @param[in] debugNamespace namespace for debugging purposes
250 : : *
251 : : * @returns false if a failure occurred during calculations, true otherwise
252 : : */
253 : : bool calculateEstimationBetweenSteps(::ad::geometry::Polygon &polygon,
254 : : TrajectorySetStepVehicleLocation const &previousVehicleLocation,
255 : : TrajectorySetStepVehicleLocation const ¤tVehicleLocation,
256 : : std::string const &debugNamespace);
257 : : /**
258 : : * @brief Calculate a polygon for one step
259 : : *
260 : : * @param[in] vehicleState current state of the vehicle
261 : : * @param[in] step step to use for calculations
262 : : * @param[in] debugNamespace namespace for debugging purposes
263 : : * @param[out] polygon the resulting polygon
264 : : * @param[out] stepVehicleLocation vehicle locations after calculation
265 : : *
266 : : * @returns false if a failure occurred during calculations, true otherwise
267 : : */
268 : : bool calculateStepPolygon(core::RelativeObjectState const &vehicleState,
269 : : TrajectorySetStep const &step,
270 : : std::string const &debugNamespace,
271 : : ::ad::geometry::Polygon &polygon,
272 : : TrajectorySetStepVehicleLocation &stepVehicleLocation);
273 : :
274 : : /**
275 : : * @brief Calculate the front and side polygon
276 : : *
277 : : * @param[in] vehicleState current state of the vehicle
278 : : * @param[in] initialStepVehicleLocation the vehicle locations for the initial calculation step
279 : : * @param[in] sideSteps trajectory set steps that define the sides of the polygon
280 : : * @param[in] front trajectory set step that define the front of the polygon
281 : : * @param[in] debugNamespace namespace for debugging purposes
282 : : * @param[out] resultPolygon the resulting polygon
283 : : * @param[out] frontSideStepVehicleLocation vehicle locations of the front side
284 : : *
285 : : * @returns false if a failure occurred during calculations, true otherwise
286 : : */
287 : : bool calculateFrontAndSidePolygon(core::RelativeObjectState const &vehicleState,
288 : : TrajectorySetStepVehicleLocation const &initialStepVehicleLocation,
289 : : std::vector<TrajectorySetStep> const &sideSteps,
290 : : TrajectorySetStep const &front,
291 : : std::string const &debugNamespace,
292 : : ::ad::geometry::Polygon &resultPolygon,
293 : : TrajectorySetStepVehicleLocation &frontSideStepVehicleLocation);
294 : :
295 : : /**
296 : : * @brief Calculate a simple polygon spanning the current vehicle pose polygon and the provided
297 : : * initialStepVehicleLocation and combine it with the given resultPolygon
298 : : *
299 : : * @param[in] vehicleState current state of the vehicle
300 : : * @param[in] initialStepVehicleLocation the vehicle locations for the initial calculation step
301 : : * @param[in] debugNamespace namespace for debugging purposes
302 : : * @param[inout] resultPolygon the resulting polygon
303 : : *
304 : : * @returns false if a failure occurred during calculations, true otherwise
305 : : */
306 : : bool calculateResponseTimePolygon(core::RelativeObjectState const &vehicleState,
307 : : TrajectorySetStepVehicleLocation const &initialStepVehicleLocation,
308 : : std::string const &debugNamespace,
309 : : ::ad::geometry::Polygon &resultPolygon);
310 : : } // namespace unstructured
311 : : } // namespace rss
312 : : } // namespace ad
|
