Skip to content

Commit 47da0e7

Browse files
authored
fix(avoidance): unexpected stop decision in avoidance module (#6320)
Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
1 parent f7d386a commit 47da0e7

File tree

2 files changed

+25
-8
lines changed

2 files changed

+25
-8
lines changed

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp

+2-1
Original file line numberDiff line numberDiff line change
@@ -261,10 +261,11 @@ class AvoidanceHelper
261261

262262
bool isComfortable(const AvoidLineArray & shift_lines) const
263263
{
264+
const auto JERK_BUFFER = 0.1; // [m/sss]
264265
return std::all_of(shift_lines.begin(), shift_lines.end(), [&](const auto & line) {
265266
return PathShifter::calcJerkFromLatLonDistance(
266267
line.getRelativeLength(), line.getRelativeLongitudinal(), getAvoidanceEgoSpeed()) <
267-
getLateralMaxJerkLimit();
268+
getLateralMaxJerkLimit() + JERK_BUFFER;
268269
});
269270
}
270271

planning/behavior_path_avoidance_module/src/shift_line_generator.cpp

+23-7
Original file line numberDiff line numberDiff line change
@@ -185,29 +185,36 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
185185
}
186186

187187
// prepare distance is not enough. unavoidable.
188-
if (remaining_distance < 1e-3) {
188+
if (avoidance_distance < 1e-3) {
189189
object.reason = AvoidanceDebugFactor::REMAINING_DISTANCE_LESS_THAN_ZERO;
190190
return std::nullopt;
191191
}
192192

193193
// calculate lateral jerk.
194194
const auto required_jerk = PathShifter::calcJerkFromLatLonDistance(
195-
avoiding_shift, remaining_distance, helper_->getAvoidanceEgoSpeed());
195+
avoiding_shift, avoidance_distance, helper_->getAvoidanceEgoSpeed());
196196

197197
// relax lateral jerk limit. avoidable.
198198
if (required_jerk < helper_->getLateralMaxJerkLimit()) {
199199
return std::make_pair(desire_shift_length, avoidance_distance);
200200
}
201201

202+
constexpr double LON_DIST_BUFFER = 1e-3;
203+
202204
// avoidance distance is not enough. unavoidable.
203205
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+
}
206213
}
207214

208215
// output avoidance path under lateral jerk constraints.
209216
const auto feasible_relative_shift_length = PathShifter::calcLateralDistFromJerk(
210-
remaining_distance, helper_->getLateralMaxJerkLimit(), helper_->getAvoidanceEgoSpeed());
217+
avoidance_distance, helper_->getLateralMaxJerkLimit(), helper_->getAvoidanceEgoSpeed());
211218

212219
if (std::abs(feasible_relative_shift_length) < parameters_->lateral_execution_threshold) {
213220
object.reason = "LessThanExecutionThreshold";
@@ -218,16 +225,25 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
218225
desire_shift_length > 0.0 ? feasible_relative_shift_length + current_ego_shift
219226
: -1.0 * feasible_relative_shift_length + current_ego_shift;
220227

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+
221237
const auto infeasible =
222-
std::abs(feasible_shift_length - object.overhang_dist) <
238+
std::abs(feasible_shift_length - object.overhang_dist) - LAT_DIST_BUFFER <
223239
0.5 * data_->parameters.vehicle_width + object_parameter.safety_buffer_lateral;
224240
if (infeasible) {
225241
RCLCPP_DEBUG(rclcpp::get_logger(""), "feasible shift length is not enough to avoid. ");
226242
object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK;
227243
return std::nullopt;
228244
}
229245

230-
return std::make_pair(feasible_shift_length, avoidance_distance);
246+
return std::make_pair(feasible_shift_length - LAT_DIST_BUFFER, avoidance_distance);
231247
};
232248

233249
const auto is_forward_object = [](const auto & object) { return object.longitudinal > 0.0; };

0 commit comments

Comments
 (0)