Skip to content

Commit aad79a4

Browse files
committed
fix(avoidance): don't slow down if avoidance is NOT definitely necessary during unsafe condition (autowarefoundation#6355)
* fix(avoidance): don't slow down if avoidance is NOT definitely necessary during unsafe condition Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix(avoidance): don't insert stop point when the path is invalid Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * refactor(avoidance): update parameter namespace Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> --------- Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
1 parent b8225f0 commit aad79a4

File tree

5 files changed

+42
-39
lines changed

5 files changed

+42
-39
lines changed

planning/behavior_path_avoidance_module/config/avoidance.param.yaml

+2-3
Original file line numberDiff line numberDiff line change
@@ -9,8 +9,6 @@
99

1010
# avoidance module common setting
1111
enable_bound_clipping: false
12-
enable_yield_maneuver: true
13-
enable_yield_maneuver_during_shifting: false
1412
enable_cancel_maneuver: true
1513
disable_path_update: false
1614

@@ -248,7 +246,8 @@
248246

249247
# For yield maneuver
250248
yield:
251-
yield_velocity: 2.78 # [m/s]
249+
enable: true # [-]
250+
enable_during_shifting: false # [-]
252251

253252
# For stop maneuver
254253
stop:

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp

-3
Original file line numberDiff line numberDiff line change
@@ -215,9 +215,6 @@ struct AvoidanceParameters
215215
size_t hysteresis_factor_safe_count;
216216
double hysteresis_factor_expand_rate{0.0};
217217

218-
// keep target velocity in yield maneuver
219-
double yield_velocity{0.0};
220-
221218
// maximum stop distance
222219
double stop_max_distance{0.0};
223220

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp

+3-4
Original file line numberDiff line numberDiff line change
@@ -41,9 +41,6 @@ AvoidanceParameters getParameter(rclcpp::Node * node)
4141
getOrDeclareParameter<double>(*node, ns + "resample_interval_for_output");
4242
p.enable_bound_clipping = getOrDeclareParameter<bool>(*node, ns + "enable_bound_clipping");
4343
p.enable_cancel_maneuver = getOrDeclareParameter<bool>(*node, ns + "enable_cancel_maneuver");
44-
p.enable_yield_maneuver = getOrDeclareParameter<bool>(*node, ns + "enable_yield_maneuver");
45-
p.enable_yield_maneuver_during_shifting =
46-
getOrDeclareParameter<bool>(*node, ns + "enable_yield_maneuver_during_shifting");
4744
p.disable_path_update = getOrDeclareParameter<bool>(*node, ns + "disable_path_update");
4845
p.publish_debug_marker = getOrDeclareParameter<bool>(*node, ns + "publish_debug_marker");
4946
p.print_debug_info = getOrDeclareParameter<bool>(*node, ns + "print_debug_info");
@@ -294,7 +291,9 @@ AvoidanceParameters getParameter(rclcpp::Node * node)
294291
// yield
295292
{
296293
const std::string ns = "avoidance.yield.";
297-
p.yield_velocity = getOrDeclareParameter<double>(*node, ns + "yield_velocity");
294+
p.enable_yield_maneuver = getOrDeclareParameter<bool>(*node, ns + "enable");
295+
p.enable_yield_maneuver_during_shifting =
296+
getOrDeclareParameter<bool>(*node, ns + "enable_during_shifting");
298297
}
299298

300299
// stop

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp

-7
Original file line numberDiff line numberDiff line change
@@ -235,13 +235,6 @@ class AvoidanceModule : public SceneModuleInterface
235235
*/
236236
void insertPrepareVelocity(ShiftedPath & shifted_path) const;
237237

238-
/**
239-
* @brief insert decel point in output path in order to yield. the ego decelerates within
240-
* accel/jerk constraints.
241-
* @param target path.
242-
*/
243-
void insertYieldVelocity(ShiftedPath & shifted_path) const;
244-
245238
/**
246239
* @brief calculate stop distance based on object's overhang.
247240
* @param stop distance.

planning/behavior_path_avoidance_module/src/scene.cpp

+37-22
Original file line numberDiff line numberDiff line change
@@ -687,7 +687,6 @@ void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, Shif
687687
break;
688688
}
689689
case AvoidanceState::YIELD: {
690-
insertYieldVelocity(path);
691690
insertWaitPoint(isBestEffort(parameters_->policy_deceleration), path);
692691
break;
693692
}
@@ -1503,6 +1502,11 @@ void AvoidanceModule::insertWaitPoint(
15031502
{
15041503
const auto & data = avoid_data_;
15051504

1505+
// If avoidance path is NOT valid, don't insert any stop points.
1506+
if (!data.valid) {
1507+
return;
1508+
}
1509+
15061510
if (!data.stop_target_object) {
15071511
return;
15081512
}
@@ -1589,28 +1593,20 @@ void AvoidanceModule::insertStopPoint(
15891593
getEgoPosition(), stop_distance - MARGIN, 0.0, shifted_path.path, stop_pose_);
15901594
}
15911595

1592-
void AvoidanceModule::insertYieldVelocity(ShiftedPath & shifted_path) const
1596+
void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const
15931597
{
1594-
const auto & p = parameters_;
15951598
const auto & data = avoid_data_;
15961599

1597-
if (data.target_objects.empty()) {
1600+
// If avoidance path is NOT safe, don't insert any slow down sections.
1601+
if (!data.safe && !data.stop_target_object) {
15981602
return;
15991603
}
16001604

1601-
if (helper_->isShifted()) {
1605+
// If avoidance path is NOT safe, don't insert any slow down sections.
1606+
if (!data.valid) {
16021607
return;
16031608
}
16041609

1605-
const auto decel_distance = helper_->getFeasibleDecelDistance(p->yield_velocity, false);
1606-
utils::avoidance::insertDecelPoint(
1607-
getEgoPosition(), decel_distance, p->yield_velocity, shifted_path.path, slow_pose_);
1608-
}
1609-
1610-
void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const
1611-
{
1612-
const auto & data = avoid_data_;
1613-
16141610
// do nothing if there is no avoidance target.
16151611
if (data.target_objects.empty()) {
16161612
return;
@@ -1626,34 +1622,53 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const
16261622
return;
16271623
}
16281624

1629-
const auto object = data.target_objects.front();
1625+
const auto itr = std::find_if(
1626+
data.target_objects.begin(), data.target_objects.end(),
1627+
[](const auto & o) { return o.avoid_required; });
1628+
1629+
const auto object = [&]() -> std::optional<ObjectData> {
1630+
if (!data.yield_required) {
1631+
return data.target_objects.front();
1632+
}
1633+
1634+
if (itr == data.target_objects.end()) {
1635+
return std::nullopt;
1636+
}
1637+
1638+
return *itr;
1639+
}();
1640+
1641+
// do nothing if it can't avoid at the moment and avoidance is NOT definitely necessary.
1642+
if (!object.has_value()) {
1643+
return;
1644+
}
16301645

16311646
const auto enough_space =
1632-
object.is_avoidable || object.reason == AvoidanceDebugFactor::TOO_LARGE_JERK;
1647+
object.value().is_avoidable || object.value().reason == AvoidanceDebugFactor::TOO_LARGE_JERK;
16331648
if (!enough_space) {
16341649
return;
16351650
}
16361651

16371652
// calculate shift length for front object.
16381653
const auto & vehicle_width = planner_data_->parameters.vehicle_width;
1639-
const auto object_type = utils::getHighestProbLabel(object.object.classification);
1654+
const auto object_type = utils::getHighestProbLabel(object.value().object.classification);
16401655
const auto object_parameter = parameters_->object_parameters.at(object_type);
1641-
const auto avoid_margin = object_parameter.lateral_hard_margin * object.distance_factor +
1656+
const auto avoid_margin = object_parameter.lateral_hard_margin * object.value().distance_factor +
16421657
object_parameter.lateral_soft_margin + 0.5 * vehicle_width;
1643-
const auto shift_length =
1644-
helper_->getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin);
1658+
const auto shift_length = helper_->getShiftLength(
1659+
object.value(), utils::avoidance::isOnRight(object.value()), avoid_margin);
16451660

16461661
// check slow down feasibility
16471662
const auto min_avoid_distance = helper_->getMinAvoidanceDistance(shift_length);
1648-
const auto distance_to_object = object.longitudinal;
1663+
const auto distance_to_object = object.value().longitudinal;
16491664
const auto remaining_distance = distance_to_object - min_avoid_distance;
16501665
const auto decel_distance = helper_->getFeasibleDecelDistance(parameters_->velocity_map.front());
16511666
if (remaining_distance < decel_distance) {
16521667
return;
16531668
}
16541669

16551670
// decide slow down lower limit.
1656-
const auto lower_speed = object.avoid_required ? 0.0 : parameters_->min_slow_down_speed;
1671+
const auto lower_speed = object.value().avoid_required ? 0.0 : parameters_->min_slow_down_speed;
16571672

16581673
// insert slow down speed.
16591674
const double current_target_velocity = PathShifter::calcFeasibleVelocityFromJerk(

0 commit comments

Comments
 (0)