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/situation/RssFormulas.hpp"
10 : : #include <algorithm>
11 : : #include "ad/rss/situation/Physics.hpp"
12 : : #include "ad/rss/situation/VehicleStateValidInputRange.hpp"
13 : :
14 : : namespace ad {
15 : :
16 : : namespace rss {
17 : : namespace situation {
18 : :
19 : : // make the code more readable
20 : : using physics::Acceleration;
21 : : using physics::Distance;
22 : : using physics::Duration;
23 : : using physics::Speed;
24 : : using situation::calculateStoppingDistance;
25 : :
26 : 133247 : inline bool vehicleStateWithinVaildInputRange(VehicleState const &vehicleState)
27 : : {
28 [ + + ]: 133247 : if (!withinValidInputRange(vehicleState))
29 : : {
30 : 12007 : return false;
31 : : }
32 [ + - + + ]: 121240 : if (vehicleState.velocity.speedLon.minimum < Speed(0.))
33 : : {
34 : 3 : return false;
35 : : }
36 : 121237 : return true;
37 : : }
38 : :
39 : 56786 : bool calculateLongitudinalDistanceOffsetAfterStatedBrakingPattern(Speed const ¤tSpeed,
40 : : Speed const &maxSpeedOnAcceleration,
41 : : Duration const &responseTime,
42 : : Acceleration const &acceleration,
43 : : Acceleration const &deceleration,
44 : : Distance &distanceOffset)
45 : : {
46 : 56786 : Speed resultingSpeed = Speed(0.);
47 : 56786 : Distance distanceOffsetAfterResponseTime = Distance(0.);
48 [ + - ]: 56786 : bool result = calculateAcceleratedLimitedMovement(
49 : : currentSpeed, maxSpeedOnAcceleration, acceleration, responseTime, resultingSpeed, distanceOffsetAfterResponseTime);
50 : :
51 : 56786 : Distance distanceToStop = Distance(0.);
52 [ + + ]: 56786 : if (std::signbit(static_cast<double>(resultingSpeed)) != std::signbit(static_cast<double>(deceleration)))
53 : : {
54 : : // if speed after stated braking pattern has the same direction as the acceleration
55 : : // further braking to full stop in that moving direction has to be added
56 [ + + + - : 56785 : result = result && calculateStoppingDistance(resultingSpeed, deceleration, distanceToStop);
+ - ]
57 : : }
58 : :
59 [ + + ]: 56786 : if (result)
60 : : {
61 [ + - ]: 56784 : distanceOffset = distanceOffsetAfterResponseTime + distanceToStop;
62 : : }
63 : :
64 : 56786 : return result;
65 : : }
66 : :
67 : 4097 : bool calculateLateralDistanceOffsetAfterStatedBrakingPattern(Speed const ¤tSpeed,
68 : : Duration const &responseTime,
69 : : Acceleration const &acceleration,
70 : : Acceleration const &deceleration,
71 : : Distance &distanceOffset)
72 : : {
73 : 4097 : Speed resultingSpeed = Speed(0.);
74 : 4097 : Distance distanceOffsetAfterResponseTime = Distance(0.);
75 : :
76 [ + - ]: 4097 : bool result = calculateSpeedInAcceleratedMovement(currentSpeed, acceleration, responseTime, resultingSpeed);
77 : 4097 : result = result
78 [ + + + - : 4097 : && calculateDistanceOffsetInAcceleratedMovement(
+ - ]
79 : : currentSpeed, acceleration, responseTime, distanceOffsetAfterResponseTime);
80 : :
81 : 4097 : Distance distanceToStop = Distance(0.);
82 [ + + ]: 4097 : if (std::signbit(static_cast<double>(resultingSpeed)) != std::signbit(static_cast<double>(deceleration)))
83 : : {
84 : : // if speed after stated braking pattern has the same direction as the acceleration
85 : : // further braking to full stop in that moving direction has to be added
86 [ + - + - : 3718 : result = result && calculateStoppingDistance(resultingSpeed, deceleration, distanceToStop);
+ - ]
87 : : }
88 : :
89 [ + + ]: 4097 : if (result)
90 : : {
91 [ + - ]: 4096 : distanceOffset = distanceOffsetAfterResponseTime + distanceToStop;
92 : : }
93 : :
94 : 4097 : return result;
95 : : }
96 : :
97 : 65591 : bool calculateSafeLongitudinalDistanceSameDirection(VehicleState const &leadingVehicle,
98 : : VehicleState const &followingVehicle,
99 : : Distance &safeDistance)
100 : : {
101 [ + - + + : 65591 : if (!vehicleStateWithinVaildInputRange(leadingVehicle) || !vehicleStateWithinVaildInputRange(followingVehicle))
+ - + + +
+ ]
102 : : {
103 : 12002 : return false;
104 : : }
105 : :
106 : 53589 : Distance distanceStatedBraking = Distance(0.);
107 : :
108 : : bool result = calculateLongitudinalDistanceOffsetAfterStatedBrakingPattern( // LCOV_EXCL_LINE: wrong detection
109 : 53589 : followingVehicle.velocity.speedLon.maximum,
110 : 53589 : followingVehicle.dynamics.maxSpeedOnAcceleration,
111 : 53589 : followingVehicle.dynamics.responseTime,
112 : 53589 : followingVehicle.dynamics.alphaLon.accelMax,
113 [ + - ]: 53589 : followingVehicle.dynamics.alphaLon.brakeMin,
114 : : distanceStatedBraking);
115 : 53589 : Distance distanceMaxBrake = Distance(0.);
116 : 53589 : result = result
117 [ + - + - : 53589 : && calculateStoppingDistance(
+ - ]
118 : : leadingVehicle.velocity.speedLon.minimum, leadingVehicle.dynamics.alphaLon.brakeMax, distanceMaxBrake);
119 : :
120 [ + - ]: 53589 : if (result)
121 : : {
122 [ + - ]: 53589 : safeDistance = distanceStatedBraking - distanceMaxBrake;
123 [ + - ]: 53589 : safeDistance = std::max(safeDistance, Distance(0.));
124 : : }
125 : :
126 : 53589 : return result;
127 : : }
128 : :
129 : 65590 : bool checkSafeLongitudinalDistanceSameDirection(VehicleState const &leadingVehicle,
130 : : VehicleState const &followingVehicle,
131 : : Distance const &vehicleDistance,
132 : : Distance &safeDistance,
133 : : bool &isDistanceSafe)
134 : : {
135 [ + - + + ]: 65590 : if (vehicleDistance < Distance(0.))
136 : : {
137 : 1 : return false;
138 : : }
139 : :
140 : 65589 : isDistanceSafe = false;
141 : 65589 : safeDistance = std::numeric_limits<physics::Distance>::max();
142 : :
143 : 65589 : bool const result = calculateSafeLongitudinalDistanceSameDirection(leadingVehicle, followingVehicle, safeDistance);
144 : :
145 [ + + ]: 65589 : if (vehicleDistance > safeDistance)
146 : : {
147 : 51690 : isDistanceSafe = true;
148 : : }
149 : 65589 : return result;
150 : : }
151 : :
152 : 1070 : bool calculateSafeLongitudinalDistanceOppositeDirection(VehicleState const &correctVehicle,
153 : : VehicleState const &oppositeVehicle,
154 : : Distance &safeDistance)
155 : : {
156 [ + - + + : 1070 : if (!vehicleStateWithinVaildInputRange(correctVehicle) || !vehicleStateWithinVaildInputRange(oppositeVehicle))
+ - + + +
+ ]
157 : : {
158 : 4 : return false;
159 : : }
160 : :
161 : 1066 : Distance distanceStatedBrakingCorrect = Distance(0.);
162 : :
163 : : bool result = calculateLongitudinalDistanceOffsetAfterStatedBrakingPattern( // LCOV_EXCL_LINE: wrong detection
164 : 1066 : correctVehicle.velocity.speedLon.maximum,
165 : 1066 : correctVehicle.dynamics.maxSpeedOnAcceleration,
166 : 1066 : correctVehicle.dynamics.responseTime,
167 : 1066 : correctVehicle.dynamics.alphaLon.accelMax,
168 [ + - ]: 1066 : correctVehicle.dynamics.alphaLon.brakeMinCorrect,
169 : : distanceStatedBrakingCorrect);
170 : :
171 : 1066 : Distance distanceStatedBrakingOpposite = Distance(0.);
172 : :
173 [ + - ]: 1066 : if (result)
174 : : {
175 : : result = calculateLongitudinalDistanceOffsetAfterStatedBrakingPattern( // LCOV_EXCL_LINE: wrong detection
176 : 1066 : oppositeVehicle.velocity.speedLon.maximum,
177 : 1066 : oppositeVehicle.dynamics.maxSpeedOnAcceleration,
178 : 1066 : oppositeVehicle.dynamics.responseTime,
179 : 1066 : oppositeVehicle.dynamics.alphaLon.accelMax,
180 [ + - ]: 1066 : oppositeVehicle.dynamics.alphaLon.brakeMin,
181 : : distanceStatedBrakingOpposite);
182 : : }
183 : :
184 [ + - ]: 1066 : if (result)
185 : : {
186 [ + - ]: 1066 : safeDistance = distanceStatedBrakingCorrect + distanceStatedBrakingOpposite;
187 : : }
188 : :
189 : 1066 : return result;
190 : : }
191 : :
192 : 1069 : bool checkSafeLongitudinalDistanceOppositeDirection(VehicleState const &correctVehicle,
193 : : VehicleState const &oppositeVehicle,
194 : : Distance const &vehicleDistance,
195 : : Distance &safeDistance,
196 : : bool &isDistanceSafe)
197 : : {
198 [ + - + + ]: 1069 : if (vehicleDistance < Distance(0.))
199 : : {
200 : 1 : return false;
201 : : }
202 : :
203 : 1068 : isDistanceSafe = false;
204 : 1068 : safeDistance = std::numeric_limits<physics::Distance>::max();
205 : 1068 : bool const result = calculateSafeLongitudinalDistanceOppositeDirection(correctVehicle, oppositeVehicle, safeDistance);
206 : :
207 [ + + ]: 1068 : if (vehicleDistance > safeDistance)
208 : : {
209 : 391 : isDistanceSafe = true;
210 : : }
211 : 1068 : return result;
212 : : }
213 : :
214 : 848 : bool checkStopInFrontIntersection(VehicleState const &vehicle, Distance &safeDistance, bool &isDistanceSafe)
215 : : {
216 [ + + ]: 848 : if (!vehicleStateWithinVaildInputRange(vehicle))
217 : : {
218 : 1 : return false;
219 : : }
220 : :
221 : 847 : isDistanceSafe = false;
222 : :
223 : 847 : safeDistance = Distance(0.);
224 : : bool result = calculateLongitudinalDistanceOffsetAfterStatedBrakingPattern( // LCOV_EXCL_LINE: wrong detection
225 : 847 : vehicle.velocity.speedLon.maximum,
226 : 847 : vehicle.dynamics.maxSpeedOnAcceleration,
227 : 847 : vehicle.dynamics.responseTime,
228 : 847 : vehicle.dynamics.alphaLon.accelMax,
229 : 847 : vehicle.dynamics.alphaLon.brakeMin,
230 : : safeDistance);
231 : :
232 [ + + ]: 847 : if (safeDistance < vehicle.distanceToEnterIntersection)
233 : : {
234 : 333 : isDistanceSafe = true;
235 : : }
236 : :
237 : 847 : return result;
238 : : }
239 : :
240 : 2043 : bool calculateSafeLateralDistance(VehicleState const &leftVehicle,
241 : : VehicleState const &rightVehicle,
242 : : Distance &safeDistance)
243 : : {
244 [ + - + + : 2043 : if (!vehicleStateWithinVaildInputRange(leftVehicle) || !vehicleStateWithinVaildInputRange(rightVehicle))
+ - + + +
+ ]
245 : : {
246 : 3 : return false;
247 : : }
248 : :
249 : 2040 : bool result = false;
250 : 2040 : Distance distanceOffsetStatedBrakingLeft = Distance(0.);
251 : 2040 : Distance distanceOffsetStatedBrakingRight = Distance(0.);
252 : :
253 : : result = calculateLateralDistanceOffsetAfterStatedBrakingPattern( // LCOV_EXCL_LINE: wrong detection
254 : 2040 : leftVehicle.velocity.speedLat.maximum,
255 : 2040 : leftVehicle.dynamics.responseTime,
256 : 2040 : leftVehicle.dynamics.alphaLat.accelMax,
257 [ + - ]: 2040 : leftVehicle.dynamics.alphaLat.brakeMin,
258 : : distanceOffsetStatedBrakingLeft);
259 : :
260 : 2040 : result = result
261 : : && calculateLateralDistanceOffsetAfterStatedBrakingPattern( // LCOV_EXCL_LINE: wrong detection
262 : 2040 : rightVehicle.velocity.speedLat.minimum,
263 [ + - ]: 2040 : rightVehicle.dynamics.responseTime,
264 [ + - ]: 2040 : -rightVehicle.dynamics.alphaLat.accelMax,
265 [ + - ]: 2040 : -rightVehicle.dynamics.alphaLat.brakeMin,
266 : : distanceOffsetStatedBrakingRight);
267 : :
268 [ + - ]: 2040 : if (result)
269 : : {
270 : : // safe distance is the difference of both distances
271 [ + - ]: 2040 : safeDistance = distanceOffsetStatedBrakingLeft - distanceOffsetStatedBrakingRight;
272 : : // plus the lateral fluctuation margin: here we use the 0.5*my of both
273 : : safeDistance
274 [ + - + - : 2040 : += 0.5 * (leftVehicle.dynamics.lateralFluctuationMargin + rightVehicle.dynamics.lateralFluctuationMargin);
+ - ]
275 [ + - ]: 2040 : safeDistance = std::max(safeDistance, Distance(0.));
276 : : }
277 : 2040 : return result;
278 : : }
279 : :
280 : 2008 : bool checkSafeLateralDistance(VehicleState const &leftVehicle,
281 : : VehicleState const &rightVehicle,
282 : : Distance const &vehicleDistance,
283 : : Distance &safeDistance,
284 : : bool &isDistanceSafe)
285 : : {
286 [ + - + + ]: 2008 : if (vehicleDistance < Distance(0.))
287 : : {
288 : 1 : return false;
289 : : }
290 : :
291 : 2007 : isDistanceSafe = false;
292 : 2007 : safeDistance = std::numeric_limits<physics::Distance>::max();
293 : 2007 : bool const result = calculateSafeLateralDistance(leftVehicle, rightVehicle, safeDistance);
294 : :
295 [ + + ]: 2007 : if (vehicleDistance > safeDistance)
296 : : {
297 : 1355 : isDistanceSafe = true;
298 : : }
299 : :
300 : 2007 : return result;
301 : : }
302 : :
303 : : } // namespace situation
304 : : } // namespace rss
305 : : } // namespace ad
|
