@@ -185,29 +185,36 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
185
185
}
186
186
187
187
// prepare distance is not enough. unavoidable.
188
- if (remaining_distance < 1e-3 ) {
188
+ if (avoidance_distance < 1e-3 ) {
189
189
object.reason = AvoidanceDebugFactor::REMAINING_DISTANCE_LESS_THAN_ZERO;
190
190
return std::nullopt;
191
191
}
192
192
193
193
// calculate lateral jerk.
194
194
const auto required_jerk = PathShifter::calcJerkFromLatLonDistance (
195
- avoiding_shift, remaining_distance , helper_->getAvoidanceEgoSpeed ());
195
+ avoiding_shift, avoidance_distance , helper_->getAvoidanceEgoSpeed ());
196
196
197
197
// relax lateral jerk limit. avoidable.
198
198
if (required_jerk < helper_->getLateralMaxJerkLimit ()) {
199
199
return std::make_pair (desire_shift_length, avoidance_distance);
200
200
}
201
201
202
+ constexpr double LON_DIST_BUFFER = 1e-3 ;
203
+
202
204
// avoidance distance is not enough. unavoidable.
203
205
if (!isBestEffort (parameters_->policy_deceleration )) {
204
- object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK;
205
- return std::nullopt;
206
+ if (avoidance_distance < helper_->getMinAvoidanceDistance (avoiding_shift) + LON_DIST_BUFFER) {
207
+ object.reason = AvoidanceDebugFactor::REMAINING_DISTANCE_LESS_THAN_ZERO;
208
+ return std::nullopt;
209
+ } else {
210
+ object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK;
211
+ return std::nullopt;
212
+ }
206
213
}
207
214
208
215
// output avoidance path under lateral jerk constraints.
209
216
const auto feasible_relative_shift_length = PathShifter::calcLateralDistFromJerk (
210
- remaining_distance , helper_->getLateralMaxJerkLimit (), helper_->getAvoidanceEgoSpeed ());
217
+ avoidance_distance , helper_->getLateralMaxJerkLimit (), helper_->getAvoidanceEgoSpeed ());
211
218
212
219
if (std::abs (feasible_relative_shift_length) < parameters_->lateral_execution_threshold ) {
213
220
object.reason = " LessThanExecutionThreshold" ;
@@ -218,16 +225,25 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
218
225
desire_shift_length > 0.0 ? feasible_relative_shift_length + current_ego_shift
219
226
: -1.0 * feasible_relative_shift_length + current_ego_shift;
220
227
228
+ if (
229
+ avoidance_distance <
230
+ helper_->getMinAvoidanceDistance (feasible_shift_length) + LON_DIST_BUFFER) {
231
+ object.reason = AvoidanceDebugFactor::REMAINING_DISTANCE_LESS_THAN_ZERO;
232
+ return std::nullopt;
233
+ }
234
+
235
+ const double LAT_DIST_BUFFER = desire_shift_length > 0.0 ? 1e-3 : -1e-3 ;
236
+
221
237
const auto infeasible =
222
- std::abs (feasible_shift_length - object.overhang_dist ) <
238
+ std::abs (feasible_shift_length - object.overhang_dist ) - LAT_DIST_BUFFER <
223
239
0.5 * data_->parameters .vehicle_width + object_parameter.safety_buffer_lateral ;
224
240
if (infeasible) {
225
241
RCLCPP_DEBUG (rclcpp::get_logger (" " ), " feasible shift length is not enough to avoid. " );
226
242
object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK;
227
243
return std::nullopt;
228
244
}
229
245
230
- return std::make_pair (feasible_shift_length, avoidance_distance);
246
+ return std::make_pair (feasible_shift_length - LAT_DIST_BUFFER , avoidance_distance);
231
247
};
232
248
233
249
const auto is_forward_object = [](const auto & object) { return object.longitudinal > 0.0 ; };
0 commit comments