Skip to content

Commit c6377b0

Browse files
satoshi-otaKhalilSelyan
authored and
KhalilSelyan
committed
fix(avoidance): improve avoidance safety check logic (#6974)
* fix(avoidance): don't ignore stopped object unless it's a parked vehilce Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix(avoidance): rebuild prepare distance calculation/validation logic Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> --------- Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
1 parent d357383 commit c6377b0

File tree

4 files changed

+23
-13
lines changed

4 files changed

+23
-13
lines changed

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp

+13-4
Original file line numberDiff line numberDiff line change
@@ -97,16 +97,18 @@ class AvoidanceHelper
9797
return std::max(getEgoSpeed(), values.at(idx));
9898
}
9999

100-
double getMinimumPrepareDistance() const
100+
double getNominalPrepareDistance(const double velocity) const
101101
{
102102
const auto & p = parameters_;
103-
return std::max(getEgoSpeed() * p->min_prepare_time, p->min_prepare_distance);
103+
const auto & values = p->velocity_map;
104+
const auto idx = getConstraintsMapIndex(velocity, values);
105+
return std::max(values.at(idx) * p->max_prepare_time, p->min_prepare_distance);
104106
}
105107

106108
double getNominalPrepareDistance() const
107109
{
108110
const auto & p = parameters_;
109-
return std::max(getEgoSpeed() * p->max_prepare_time, p->min_prepare_distance);
111+
return std::max(getAvoidanceEgoSpeed() * p->max_prepare_time, p->min_prepare_distance);
110112
}
111113

112114
double getNominalAvoidanceDistance(const double shift_length) const
@@ -298,6 +300,13 @@ class AvoidanceHelper
298300
return std::numeric_limits<double>::max();
299301
}
300302

303+
bool isEnoughPrepareDistance(const double prepare_distance) const
304+
{
305+
const auto & p = parameters_;
306+
return prepare_distance >
307+
std::max(getEgoSpeed() * p->min_prepare_distance, p->min_prepare_distance);
308+
}
309+
301310
bool isComfortable(const AvoidLineArray & shift_lines) const
302311
{
303312
const auto JERK_BUFFER = 0.1; // [m/sss]
@@ -328,7 +337,7 @@ class AvoidanceHelper
328337
const auto desire_shift_length =
329338
getShiftLength(object, is_object_on_right, object.avoid_margin.value());
330339

331-
const auto prepare_distance = getMinimumPrepareDistance();
340+
const auto prepare_distance = getNominalPrepareDistance(0.0);
332341
const auto constant_distance = getFrontConstantDistance(object);
333342
const auto avoidance_distance = getMinAvoidanceDistance(desire_shift_length);
334343

planning/behavior_path_avoidance_module/src/scene.cpp

+5-4
Original file line numberDiff line numberDiff line change
@@ -409,9 +409,9 @@ bool AvoidanceModule::canYieldManeuver(const AvoidancePlanningData & data) const
409409
const auto registered_lines = path_shifter_.getShiftLines();
410410
if (!registered_lines.empty()) {
411411
const size_t idx = planner_data_->findEgoIndex(path_shifter_.getReferencePath().points);
412-
const auto to_shift_start_point = motion_utils::calcSignedArcLength(
412+
const auto prepare_distance = motion_utils::calcSignedArcLength(
413413
path_shifter_.getReferencePath().points, idx, registered_lines.front().start_idx);
414-
if (to_shift_start_point < helper_->getMinimumPrepareDistance()) {
414+
if (!helper_->isEnoughPrepareDistance(prepare_distance)) {
415415
RCLCPP_DEBUG(
416416
getLogger(),
417417
"Distance to shift start point is less than minimum prepare distance. The distance is not "
@@ -1412,10 +1412,11 @@ double AvoidanceModule::calcDistanceToStopLine(const ObjectData & object) const
14121412
const auto avoidance_distance = helper_->getMinAvoidanceDistance(
14131413
helper_->getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin));
14141414
const auto constant_distance = helper_->getFrontConstantDistance(object);
1415+
const auto prepare_distance = helper_->getNominalPrepareDistance(0.0);
14151416

14161417
return object.longitudinal -
14171418
std::min(
1418-
avoidance_distance + constant_distance + p->min_prepare_distance + p->stop_buffer,
1419+
avoidance_distance + constant_distance + prepare_distance + p->stop_buffer,
14191420
p->stop_max_distance);
14201421
}
14211422

@@ -1444,7 +1445,7 @@ void AvoidanceModule::insertReturnDeadLine(
14441445
const auto buffer = std::max(0.0, to_shifted_path_end - to_reference_path_end);
14451446

14461447
const auto min_return_distance =
1447-
helper_->getMinAvoidanceDistance(shift_length) + helper_->getMinimumPrepareDistance();
1448+
helper_->getMinAvoidanceDistance(shift_length) + helper_->getNominalPrepareDistance(0.0);
14481449
const auto to_stop_line = data.to_return_point - min_return_distance - buffer;
14491450

14501451
// If we don't need to consider deceleration constraints, insert a deceleration point

planning/behavior_path_avoidance_module/src/shift_line_generator.cpp

+4-4
Original file line numberDiff line numberDiff line change
@@ -929,7 +929,7 @@ void ShiftLineGenerator::applySmallShiftFilter(
929929
continue;
930930
}
931931

932-
if (s.start_longitudinal + 1e-3 < helper_->getMinimumPrepareDistance()) {
932+
if (!helper_->isEnoughPrepareDistance(s.start_longitudinal)) {
933933
continue;
934934
}
935935

@@ -1173,13 +1173,13 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine(
11731173
std::max(nominal_prepare_distance - last_sl_distance, 0.0);
11741174

11751175
double prepare_distance_scaled = std::max(
1176-
helper_->getMinimumPrepareDistance(), std::max(nominal_prepare_distance, last_sl_distance));
1176+
helper_->getNominalPrepareDistance(), std::max(nominal_prepare_distance, last_sl_distance));
11771177
double avoid_distance_scaled = nominal_avoid_distance;
11781178
if (remaining_distance < prepare_distance_scaled + avoid_distance_scaled) {
11791179
const auto scale = (remaining_distance - last_sl_distance) /
11801180
std::max(nominal_avoid_distance + variable_prepare_distance, 0.1);
11811181
prepare_distance_scaled = std::max(
1182-
helper_->getMinimumPrepareDistance(), last_sl_distance + scale * nominal_prepare_distance);
1182+
helper_->getNominalPrepareDistance(), last_sl_distance + scale * nominal_prepare_distance);
11831183
avoid_distance_scaled *= scale;
11841184
}
11851185

@@ -1292,7 +1292,7 @@ AvoidLineArray ShiftLineGenerator::findNewShiftLine(
12921292
const auto & candidate = shift_lines.at(i);
12931293

12941294
// prevent sudden steering.
1295-
if (candidate.start_longitudinal + 1e-3 < helper_->getMinimumPrepareDistance()) {
1295+
if (!helper_->isEnoughPrepareDistance(candidate.start_longitudinal)) {
12961296
break;
12971297
}
12981298

planning/behavior_path_avoidance_module/src/utils.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -1916,7 +1916,7 @@ std::vector<ExtendedPredictedObject> getSafetyCheckTargetObjects(
19161916
std::for_each(objects.begin(), objects.end(), [&p, &ret, &parameters](const auto & object) {
19171917
if (filtering_utils::isSafetyCheckTargetObjectType(object.object, parameters)) {
19181918
// check only moving objects
1919-
if (filtering_utils::isMovingObject(object, parameters)) {
1919+
if (filtering_utils::isMovingObject(object, parameters) || !object.is_parked) {
19201920
ret.objects.push_back(object.object);
19211921
}
19221922
}

0 commit comments

Comments
 (0)