Branch data Line data Source code
1 : : // ----------------- BEGIN LICENSE BLOCK ---------------------------------
2 : : //
3 : : // Copyright (C) 2018-2021 Intel Corporation
4 : : //
5 : : // SPDX-License-Identifier: LGPL-2.1-only
6 : : //
7 : : // ----------------- END LICENSE BLOCK -----------------------------------
8 : :
9 : : #include "ad/rss/structured/RssFormulas.hpp"
10 : : #include <algorithm>
11 : : #include "ad/rss/core/Physics.hpp"
12 : : #include "ad/rss/core/RelativeObjectStateValidInputRange.hpp"
13 : :
14 : : namespace ad {
15 : : namespace rss {
16 : : namespace structured {
17 : :
18 : : // make the code more readable
19 : : using physics::Acceleration;
20 : : using physics::Distance;
21 : : using physics::Duration;
22 : : using physics::Speed;
23 : :
24 : 133240 : inline bool objectStateWithinVaildInputRange(core::RelativeObjectState const &objectState)
25 : : {
26 [ + + ]: 133240 : if (!withinValidInputRange(objectState))
27 : : {
28 : 12010 : return false;
29 : : }
30 [ + - - + ]: 121230 : if (objectState.structured_object_state.velocity.speed_lon_min < Speed(0.))
31 : : {
32 : 0 : return false;
33 : : }
34 : 121230 : return true;
35 : : }
36 : :
37 : 57137 : bool calculateLongitudinalDistanceOffsetAfterStatedBrakingPattern(Speed const ¤tSpeed,
38 : : Speed const &max_speed_on_acceleration,
39 : : Duration const &response_time,
40 : : Acceleration const &acceleration,
41 : : Acceleration const &deceleration,
42 : : Distance &distanceOffset)
43 : : {
44 : 57137 : Speed resultingSpeed = Speed(0.);
45 : 57137 : Distance distanceOffsetAfterResponseTime = Distance(0.);
46 [ + - ]: 57137 : bool result = core::calculateAcceleratedLimitedMovement(currentSpeed,
47 : : max_speed_on_acceleration,
48 : : acceleration,
49 : : response_time,
50 : : resultingSpeed,
51 : : distanceOffsetAfterResponseTime);
52 : :
53 : 57137 : Distance distanceToStop = Distance(0.);
54 [ + + ]: 57137 : if (std::signbit(resultingSpeed.mSpeed) != std::signbit(deceleration.mAcceleration))
55 : : {
56 : : // if speed after stated braking pattern has the same direction as the acceleration
57 : : // further braking to full stop in that moving direction has to be added
58 [ + + + - : 57136 : result = result && core::calculateStoppingDistance(resultingSpeed, deceleration, distanceToStop);
+ - ]
59 : : }
60 : :
61 [ + + ]: 57137 : if (result)
62 : : {
63 [ + - ]: 57135 : distanceOffset = distanceOffsetAfterResponseTime + distanceToStop;
64 : : }
65 : :
66 : 57137 : return result;
67 : : }
68 : :
69 : 4067 : bool calculateLateralDistanceOffsetAfterStatedBrakingPattern(Speed const ¤tSpeed,
70 : : Duration const &response_time,
71 : : Acceleration const &acceleration,
72 : : Acceleration const &deceleration,
73 : : Distance &distanceOffset)
74 : : {
75 : 4067 : Speed resultingSpeed = Speed(0.);
76 : 4067 : Distance distanceOffsetAfterResponseTime = Distance(0.);
77 : :
78 [ + - ]: 4067 : bool result = core::calculateSpeedInAcceleratedMovement(currentSpeed, acceleration, response_time, resultingSpeed);
79 : 4067 : result = result
80 [ + + + - : 4067 : && core::calculateDistanceOffsetInAcceleratedMovement(
+ - ]
81 : : currentSpeed, acceleration, response_time, distanceOffsetAfterResponseTime);
82 : :
83 : 4067 : Distance distanceToStop = Distance(0.);
84 [ + + ]: 4067 : if (std::signbit(resultingSpeed.mSpeed) != std::signbit(deceleration.mAcceleration))
85 : : {
86 : : // if speed after stated braking pattern has the same direction as the acceleration
87 : : // further braking to full stop in that moving direction has to be added
88 [ + - + - : 3688 : result = result && core::calculateStoppingDistance(resultingSpeed, deceleration, distanceToStop);
+ - ]
89 : : }
90 : :
91 [ + + ]: 4067 : if (result)
92 : : {
93 [ + - ]: 4066 : distanceOffset = distanceOffsetAfterResponseTime + distanceToStop;
94 : : }
95 : :
96 : 4067 : return result;
97 : : }
98 : :
99 : 65598 : bool calculateSafeLongitudinalDistanceSameDirection(core::RelativeObjectState const &leadingObject,
100 : : core::RelativeObjectState const &followingObject,
101 : : Distance &safe_distance)
102 : : {
103 [ + - + + : 65598 : if (!objectStateWithinVaildInputRange(leadingObject) || !objectStateWithinVaildInputRange(followingObject))
+ - + + +
+ ]
104 : : {
105 : 12002 : return false;
106 : : }
107 : :
108 : 53596 : Distance distanceStatedBraking = Distance(0.);
109 : :
110 : : bool result = calculateLongitudinalDistanceOffsetAfterStatedBrakingPattern( // LCOV_EXCL_LINE: wrong detection
111 : 53596 : followingObject.structured_object_state.velocity.speed_lon_max,
112 : 53596 : followingObject.dynamics.max_speed_on_acceleration,
113 : 53596 : followingObject.dynamics.response_time,
114 : 53596 : followingObject.dynamics.alpha_lon.accel_max,
115 [ + - ]: 53596 : followingObject.dynamics.alpha_lon.brake_min,
116 : : distanceStatedBraking);
117 : 53596 : Distance distanceMaxBrake = Distance(0.);
118 : 53596 : result = result
119 [ + - + - : 53596 : && core::calculateStoppingDistance(leadingObject.structured_object_state.velocity.speed_lon_min,
+ - ]
120 : : leadingObject.dynamics.alpha_lon.brake_max,
121 : : distanceMaxBrake);
122 : :
123 [ + - ]: 53596 : if (result)
124 : : {
125 : : // we need to be able to brake before min_longitudinal_safety_distance is reached
126 [ + - + - ]: 53596 : safe_distance = std::max(distanceStatedBraking - distanceMaxBrake, Distance(0.));
127 [ + - ]: 53596 : safe_distance += followingObject.dynamics.min_longitudinal_safety_distance;
128 : : }
129 : :
130 : 53596 : return result;
131 : : }
132 : :
133 : 65596 : bool checkSafeLongitudinalDistanceSameDirection(core::RelativeObjectState const &leadingObject,
134 : : core::RelativeObjectState const &followingObject,
135 : : Distance const &objectDistance,
136 : : Distance &safe_distance,
137 : : bool &isDistanceSafe)
138 : : {
139 [ + - + + ]: 65596 : if (objectDistance < Distance(0.))
140 : : {
141 : 1 : return false;
142 : : }
143 : :
144 : 65595 : isDistanceSafe = false;
145 : 65595 : safe_distance = std::numeric_limits<physics::Distance>::max();
146 : :
147 : 65595 : bool const result = calculateSafeLongitudinalDistanceSameDirection(leadingObject, followingObject, safe_distance);
148 : :
149 [ + + ]: 65595 : if (objectDistance > safe_distance)
150 : : {
151 : 51701 : isDistanceSafe = true;
152 : : }
153 : 65595 : return result;
154 : : }
155 : :
156 : 1070 : bool calculateSafeLongitudinalDistanceOppositeDirection(core::RelativeObjectState const &correctObject,
157 : : core::RelativeObjectState const &oppositeObject,
158 : : Distance &safe_distance)
159 : : {
160 [ + - + + : 1070 : if (!objectStateWithinVaildInputRange(correctObject) || !objectStateWithinVaildInputRange(oppositeObject))
+ - + + +
+ ]
161 : : {
162 : 4 : return false;
163 : : }
164 : :
165 : 1066 : Distance distanceStatedBrakingCorrect = Distance(0.);
166 : :
167 : : bool result = calculateLongitudinalDistanceOffsetAfterStatedBrakingPattern( // LCOV_EXCL_LINE: wrong detection
168 : 1066 : correctObject.structured_object_state.velocity.speed_lon_max,
169 : 1066 : correctObject.dynamics.max_speed_on_acceleration,
170 : 1066 : correctObject.dynamics.response_time,
171 : 1066 : correctObject.dynamics.alpha_lon.accel_max,
172 [ + - ]: 1066 : correctObject.dynamics.alpha_lon.brake_min_correct,
173 : : distanceStatedBrakingCorrect);
174 : :
175 : 1066 : Distance distanceStatedBrakingOpposite = Distance(0.);
176 : :
177 [ + - ]: 1066 : if (result)
178 : : {
179 : : result = calculateLongitudinalDistanceOffsetAfterStatedBrakingPattern( // LCOV_EXCL_LINE: wrong detection
180 : 1066 : oppositeObject.structured_object_state.velocity.speed_lon_max,
181 : 1066 : oppositeObject.dynamics.max_speed_on_acceleration,
182 : 1066 : oppositeObject.dynamics.response_time,
183 : 1066 : oppositeObject.dynamics.alpha_lon.accel_max,
184 [ + - ]: 1066 : oppositeObject.dynamics.alpha_lon.brake_min,
185 : : distanceStatedBrakingOpposite);
186 : : }
187 : :
188 [ + - ]: 1066 : if (result)
189 : : {
190 : : // we need to be able to brake before min_longitudinal_safety_distance is reached
191 : 1066 : safe_distance = distanceStatedBrakingCorrect + distanceStatedBrakingOpposite
192 : 1066 : + std::max(correctObject.dynamics.min_longitudinal_safety_distance,
193 [ + - + - : 1066 : oppositeObject.dynamics.min_longitudinal_safety_distance);
+ - ]
194 : : }
195 : :
196 : 1066 : return result;
197 : : }
198 : :
199 : 1069 : bool checkSafeLongitudinalDistanceOppositeDirection(core::RelativeObjectState const &correctObject,
200 : : core::RelativeObjectState const &oppositeObject,
201 : : Distance const &objectDistance,
202 : : Distance &safe_distance,
203 : : bool &isDistanceSafe)
204 : : {
205 [ + - + + ]: 1069 : if (objectDistance < Distance(0.))
206 : : {
207 : 1 : return false;
208 : : }
209 : :
210 : 1068 : isDistanceSafe = false;
211 : 1068 : safe_distance = std::numeric_limits<physics::Distance>::max();
212 : 1068 : bool const result = calculateSafeLongitudinalDistanceOppositeDirection(correctObject, oppositeObject, safe_distance);
213 : :
214 [ + + ]: 1068 : if (objectDistance > safe_distance)
215 : : {
216 : 389 : isDistanceSafe = true;
217 : : }
218 : 1068 : return result;
219 : : }
220 : :
221 : 857 : bool checkStopInFrontIntersection(core::RelativeObjectState const &object,
222 : : Distance &safe_distance,
223 : : bool &isDistanceSafe)
224 : : {
225 [ + + ]: 857 : if (!objectStateWithinVaildInputRange(object))
226 : : {
227 : 1 : return false;
228 : : }
229 : :
230 : 856 : isDistanceSafe = false;
231 : :
232 : 856 : safe_distance = Distance(0.);
233 : : bool result = calculateLongitudinalDistanceOffsetAfterStatedBrakingPattern( // LCOV_EXCL_LINE: wrong detection
234 : 856 : object.structured_object_state.velocity.speed_lon_max,
235 : 856 : object.dynamics.max_speed_on_acceleration,
236 : 856 : object.dynamics.response_time,
237 : 856 : object.dynamics.alpha_lon.accel_max,
238 : 856 : object.dynamics.alpha_lon.brake_min,
239 : : safe_distance);
240 : :
241 : : // we need to be able to brake before min_longitudinal_safety_distance is reached
242 : 856 : safe_distance += object.dynamics.min_longitudinal_safety_distance;
243 : :
244 [ + + ]: 856 : if (safe_distance < object.structured_object_state.distance_to_enter_intersection)
245 : : {
246 : 339 : isDistanceSafe = true;
247 : : }
248 : :
249 : 856 : return result;
250 : : }
251 : :
252 : 2028 : bool calculateSafeLateralDistance(core::RelativeObjectState const &leftObject,
253 : : core::RelativeObjectState const &rightObject,
254 : : Distance &safe_distance)
255 : : {
256 [ + - + + : 2028 : if (!objectStateWithinVaildInputRange(leftObject) || !objectStateWithinVaildInputRange(rightObject))
+ - + + +
+ ]
257 : : {
258 : 3 : return false;
259 : : }
260 : :
261 : 2025 : bool result = false;
262 : 2025 : Distance distanceOffsetStatedBrakingLeft = Distance(0.);
263 : 2025 : Distance distanceOffsetStatedBrakingRight = Distance(0.);
264 : :
265 : : result = calculateLateralDistanceOffsetAfterStatedBrakingPattern( // LCOV_EXCL_LINE: wrong detection
266 : 2025 : leftObject.structured_object_state.velocity.speed_lat_max,
267 : 2025 : leftObject.dynamics.response_time,
268 : 2025 : leftObject.dynamics.alpha_lat.accel_max,
269 [ + - ]: 2025 : leftObject.dynamics.alpha_lat.brake_min,
270 : : distanceOffsetStatedBrakingLeft);
271 : :
272 : 2025 : result = result
273 : : && calculateLateralDistanceOffsetAfterStatedBrakingPattern( // LCOV_EXCL_LINE: wrong detection
274 : 2025 : rightObject.structured_object_state.velocity.speed_lat_min,
275 [ + - ]: 2025 : rightObject.dynamics.response_time,
276 [ + - ]: 2025 : -rightObject.dynamics.alpha_lat.accel_max,
277 [ + - ]: 2025 : -rightObject.dynamics.alpha_lat.brake_min,
278 : : distanceOffsetStatedBrakingRight);
279 : :
280 [ + - ]: 2025 : if (result)
281 : : {
282 : : // safe distance is the difference of both distances
283 [ + - ]: 2025 : safe_distance = distanceOffsetStatedBrakingLeft - distanceOffsetStatedBrakingRight;
284 : : // plus the lateral fluctuation margin: here we use the 0.5*my of both
285 : : safe_distance
286 [ + - + - : 2025 : += 0.5 * (leftObject.dynamics.lateral_fluctuation_margin + rightObject.dynamics.lateral_fluctuation_margin);
+ - ]
287 [ + - ]: 2025 : safe_distance = std::max(safe_distance, Distance(0.));
288 : : }
289 : 2025 : return result;
290 : : }
291 : :
292 : 1993 : bool checkSafeLateralDistance(core::RelativeObjectState const &leftObject,
293 : : core::RelativeObjectState const &rightObject,
294 : : Distance const &objectDistance,
295 : : Distance &safe_distance,
296 : : bool &isDistanceSafe)
297 : : {
298 [ + - + + ]: 1993 : if (objectDistance < Distance(0.))
299 : : {
300 : 1 : return false;
301 : : }
302 : :
303 : 1992 : isDistanceSafe = false;
304 : 1992 : safe_distance = std::numeric_limits<physics::Distance>::max();
305 : 1992 : bool const result = calculateSafeLateralDistance(leftObject, rightObject, safe_distance);
306 : :
307 [ + + ]: 1992 : if (objectDistance > safe_distance)
308 : : {
309 : 1339 : isDistanceSafe = true;
310 : : }
311 : :
312 : 1992 : return result;
313 : : }
314 : :
315 : : } // namespace structured
316 : : } // namespace rss
317 : : } // namespace ad
|
