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/physics/Angle.hpp>
15 : : #include <ad/physics/Dimension2D.hpp>
16 : : #include <ad/physics/Distance.hpp>
17 : : #include "ad/rss/situation/VehicleState.hpp"
18 : : #include "ad/rss/unstructured/Geometry.hpp"
19 : :
20 : : /*!
21 : : * @brief namespace ad
22 : : */
23 : : namespace ad {
24 : : /*!
25 : : * @brief namespace rss
26 : : */
27 : : namespace rss {
28 : : /*!
29 : : * @brief namespace unstructured
30 : : */
31 : : namespace unstructured {
32 : :
33 : : /**
34 : : * @brief corner of a vehicle
35 : : */
36 : : enum class VehicleCorner
37 : : {
38 : : frontLeft,
39 : : frontRight,
40 : : backLeft,
41 : : backRight
42 : : };
43 : :
44 : : /**
45 : : * @brief a point of a trajectory
46 : : */
47 : : struct TrajectoryPoint
48 : : {
49 : 500 : TrajectoryPoint()
50 : 500 : {
51 : 500 : }
52 : :
53 : 240 : TrajectoryPoint(situation::VehicleState const &vehicleState)
54 : 240 : {
55 : 240 : position = toPoint(vehicleState.objectState.centerPoint);
56 : 240 : angle = vehicleState.objectState.yaw;
57 : 240 : speed = vehicleState.objectState.speed;
58 : 240 : yawRate = vehicleState.objectState.yawRate;
59 : 240 : }
60 : :
61 : : TrajectoryPoint(Point const &inPoint,
62 : : ad::physics::Angle const &inAngle,
63 : : ad::physics::Speed const &inSpeed,
64 : : physics::AngularVelocity const &inYawRate)
65 : : : position(inPoint)
66 : : , speed(inSpeed)
67 : : , angle(inAngle)
68 : : , yawRate(inYawRate)
69 : : {
70 : : }
71 : :
72 : : /*!
73 : : * The current position
74 : : */
75 : : Point position;
76 : :
77 : : /*!
78 : : * The current position
79 : : */
80 : : ad::physics::Speed speed;
81 : :
82 : : /*!
83 : : * The current heading angle
84 : : */
85 : : ad::physics::Angle angle;
86 : :
87 : : /*!
88 : : * The current yawRate
89 : : */
90 : : physics::AngularVelocity yawRate;
91 : : };
92 : :
93 : : struct TrajectorySetStep
94 : : {
95 : 100 : TrajectorySetStep()
96 : 100 : {
97 : 100 : }
98 : :
99 : : TrajectorySetStep(TrajectoryPoint const &inLeft, TrajectoryPoint const &inRight, TrajectoryPoint const &inCenter)
100 : : : center(inCenter)
101 : : {
102 : : left.push_back(inLeft);
103 : : right.push_back(inRight);
104 : : }
105 : :
106 : : std::vector<TrajectoryPoint> left; // with positive yaw rate ratio
107 : : std::vector<TrajectoryPoint> right; // with negative yaw rate ratio
108 : : TrajectoryPoint center;
109 : : };
110 : :
111 : : /**
112 : : * @brief get the point describing the corner of a vehicle
113 : : *
114 : : * @param[in] point trajectory point
115 : : * @param[in] vehicleDimension vehicle dimension
116 : : * @param[in] corner which corner to calculate
117 : : *
118 : : * @returns corner point of the vehicle
119 : : */
120 : : Point getVehicleCorner(TrajectoryPoint const &point,
121 : : ad::physics::Dimension2D const &vehicleDimension,
122 : : VehicleCorner const corner);
123 : :
124 : : struct TrafficParticipantLocation
125 : : {
126 : 360 : TrafficParticipantLocation()
127 : 360 : {
128 : 360 : }
129 : :
130 : 760 : TrafficParticipantLocation(TrajectoryPoint const &pt, situation::VehicleState const &vehicleState)
131 : 760 : {
132 : 760 : frontLeft = getVehicleCorner(pt, vehicleState.objectState.dimension, VehicleCorner::frontLeft);
133 : 760 : frontRight = getVehicleCorner(pt, vehicleState.objectState.dimension, VehicleCorner::frontRight);
134 : 760 : backLeft = getVehicleCorner(pt, vehicleState.objectState.dimension, VehicleCorner::backLeft);
135 : 760 : backRight = getVehicleCorner(pt, vehicleState.objectState.dimension, VehicleCorner::backRight);
136 : 760 : }
137 : :
138 : 760 : Polygon toPolygon() const
139 : : {
140 : 760 : Polygon vehiclePolygon;
141 [ + - ]: 760 : boost::geometry::append(vehiclePolygon, frontRight);
142 [ + - ]: 760 : boost::geometry::append(vehiclePolygon, frontLeft);
143 [ + - ]: 760 : boost::geometry::append(vehiclePolygon, backLeft);
144 [ + - ]: 760 : boost::geometry::append(vehiclePolygon, backRight);
145 [ + - ]: 760 : boost::geometry::append(vehiclePolygon, frontRight);
146 : 760 : return vehiclePolygon;
147 : : }
148 : 1360 : MultiPoint toMultiPoint() const
149 : : {
150 : 1360 : MultiPoint geometry;
151 [ + - ]: 1360 : boost::geometry::append(geometry, frontRight);
152 [ + - ]: 1360 : boost::geometry::append(geometry, frontLeft);
153 [ + - ]: 1360 : boost::geometry::append(geometry, backLeft);
154 [ + - ]: 1360 : boost::geometry::append(geometry, backRight);
155 [ + - ]: 1360 : boost::geometry::append(geometry, frontRight);
156 : 1360 : return geometry;
157 : : }
158 : :
159 : 180 : bool operator==(TrafficParticipantLocation const &other) const
160 : : {
161 [ + - + - ]: 300 : return (frontLeft == other.frontLeft) && (frontRight == other.frontRight) && (backLeft == other.backLeft)
162 [ + + + - ]: 300 : && (backRight == other.backRight);
163 : : }
164 : : bool operator!=(TrafficParticipantLocation const &other) const
165 : : {
166 : : return !(*this == other);
167 : : }
168 : : Point frontLeft;
169 : : Point frontRight;
170 : : Point backLeft;
171 : : Point backRight;
172 : : };
173 : :
174 : : /*!
175 : : * a trajectory
176 : : */
177 : : using Trajectory = std::vector<TrajectoryPoint>;
178 : :
179 : : struct TrajectorySetStepVehicleLocation
180 : : {
181 : : TrafficParticipantLocation left;
182 : : TrafficParticipantLocation right;
183 : : TrafficParticipantLocation center;
184 : :
185 : 100 : bool operator==(TrajectorySetStepVehicleLocation const &other) const
186 : : {
187 [ + + + - : 100 : return (left == other.left) && (right == other.right) && (center == other.center);
+ - ]
188 : : }
189 : :
190 : : bool operator!=(TrajectorySetStepVehicleLocation const &other) const
191 : : {
192 : : return !(*this == other);
193 : : }
194 : : };
195 : :
196 : : /**
197 : : * @brief Calculate a trajectory set estimation between two steps
198 : : *
199 : : * @param[inout] polygon polygon to work on
200 : : * @param[in] previousVehicleLocation the previous possible vehicle locations
201 : : * @param[in] currentVehicleLocation the current possible vehicle locations
202 : : * @param[in] debugNamespace namespace for debugging purposes
203 : : *
204 : : * @returns false if a failure occurred during calculations, true otherwise
205 : : */
206 : : bool calculateEstimationBetweenSteps(Polygon &polygon,
207 : : TrajectorySetStepVehicleLocation const &previousVehicleLocation,
208 : : TrajectorySetStepVehicleLocation const ¤tVehicleLocation,
209 : : std::string const &debugNamespace);
210 : : /**
211 : : * @brief Calculate a polygon for one step
212 : : *
213 : : * @param[in] vehicleState current state of the vehicle
214 : : * @param[in] step step to use for calculations
215 : : * @param[in] debugNamespace namespace for debugging purposes
216 : : * @param[out] polygon the resulting polygon
217 : : * @param[out] stepVehicleLocation vehicle locations after calculation
218 : : *
219 : : * @returns false if a failure occurred during calculations, true otherwise
220 : : */
221 : : bool calculateStepPolygon(situation::VehicleState const &vehicleState,
222 : : TrajectorySetStep const &step,
223 : : std::string const &debugNamespace,
224 : : Polygon &polygon,
225 : : TrajectorySetStepVehicleLocation &stepVehicleLocation);
226 : :
227 : : /**
228 : : * @brief Calculate the front and side polygon
229 : : *
230 : : * @param[in] vehicleState current state of the vehicle
231 : : * @param[in] initialStepVehicleLocation the vehicle locations for the initial calculation step
232 : : * @param[in] sideSteps trajectory set steps that define the sides of the polygon
233 : : * @param[in] front trajectory set step that define the front of the polygon
234 : : * @param[in] debugNamespace namespace for debugging purposes
235 : : * @param[out] resultPolygon the resulting polygon
236 : : * @param[out] frontSideStepVehicleLocation vehicle locations of the front side
237 : : *
238 : : * @returns false if a failure occurred during calculations, true otherwise
239 : : */
240 : : bool calculateFrontAndSidePolygon(situation::VehicleState const &vehicleState,
241 : : TrajectorySetStepVehicleLocation const &initialStepVehicleLocation,
242 : : std::vector<TrajectorySetStep> const &sideSteps,
243 : : TrajectorySetStep const &front,
244 : : std::string const &debugNamespace,
245 : : Polygon &resultPolygon,
246 : : TrajectorySetStepVehicleLocation &frontSideStepVehicleLocation);
247 : :
248 : : } // namespace unstructured
249 : : } // namespace rss
250 : : } // namespace ad
|
