From 53ddf84fd2a0db971f28734aa1072c15e549121b Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Tue, 14 Nov 2023 12:42:36 +0900 Subject: [PATCH 1/4] fix(avoidance): prevent sudden steering at yield maneuver Signed-off-by: satoshi-ota --- .../config/avoidance/avoidance.param.yaml | 3 ++- .../utils/avoidance/avoidance_module_data.hpp | 3 ++- .../utils/avoidance/helper.hpp | 8 ++++++- .../avoidance/avoidance_module.cpp | 22 ++++++++++++++++--- .../src/scene_module/avoidance/manager.cpp | 6 +++-- 5 files changed, 34 insertions(+), 8 deletions(-) diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 96b6ca452bbcc..ad06a1a9f6a10 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -190,7 +190,8 @@ max_deviation_from_lane: 0.5 # [m] # avoidance distance parameters longitudinal: - prepare_time: 2.0 # [s] + min_prepare_time: 1.0 # [s] + max_prepare_time: 2.0 # [s] min_prepare_distance: 1.0 # [m] min_slow_down_speed: 1.38 # [m/s] buf_slow_down_speed: 0.56 # [m/s] diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index c8b87c7b4759f..60cf9b1244236 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -211,7 +211,8 @@ struct AvoidanceParameters double stop_buffer{0.0}; // start avoidance after this time to avoid sudden path change - double prepare_time{0.0}; + double min_prepare_time{0.0}; + double max_prepare_time{0.0}; // Even if the vehicle speed is zero, avoidance will start after a distance of this much. double min_prepare_distance{0.0}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp index 11388dd6bb74a..9b1e435aec6be 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/helper.hpp @@ -91,10 +91,16 @@ class AvoidanceHelper return std::max(getEgoSpeed(), values.at(idx)); } + double getMinimumPrepareDistance() const + { + const auto & p = parameters_; + return std::max(getEgoSpeed() * p->min_prepare_time, p->min_prepare_distance); + } + double getNominalPrepareDistance() const { const auto & p = parameters_; - return std::max(getEgoSpeed() * p->prepare_time, p->min_prepare_distance); + return std::max(getEgoSpeed() * p->max_prepare_time, p->min_prepare_distance); } double getNominalAvoidanceDistance(const double shift_length) const diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 46dfd93d83432..2fb68ca00d5ba 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -437,10 +437,23 @@ bool AvoidanceModule::canYieldManeuver(const AvoidancePlanningData & data) const { // transit yield maneuver only when the avoidance maneuver is not initiated. if (helper_.isShifted()) { + RCLCPP_DEBUG(getLogger(), "avoidance maneuver already initiated."); return false; } + // prevent sudden steering. + const auto registered_lines = path_shifter_.getShiftLines(); + if (!registered_lines.empty()) { + const size_t idx = planner_data_->findEgoIndex(path_shifter_.getReferencePath().points); + const auto to_shift_start_point = calcSignedArcLength( + path_shifter_.getReferencePath().points, idx, registered_lines.front().start_idx); + if (to_shift_start_point < helper_.getMinimumPrepareDistance()) { + return false; + } + } + if (!data.stop_target_object) { + RCLCPP_DEBUG(getLogger(), "can pass by the object safely without avoidance maneuver."); return true; } @@ -1652,6 +1665,10 @@ void AvoidanceModule::applySmallShiftFilter( continue; } + if (s.start_longitudinal < helper_.getMinimumPrepareDistance()) { + continue; + } + shift_lines.push_back(s); } } @@ -2405,9 +2422,8 @@ AvoidLineArray AvoidanceModule::findNewShiftLine( for (size_t i = 0; i < shift_lines.size(); ++i) { const auto & candidate = shift_lines.at(i); - // new shift points must exist in front of Ego - // this value should be larger than -eps consider path shifter calculation error. - if (candidate.start_idx < avoid_data_.ego_closest_path_index) { + // prevent sudden steering. + if (candidate.start_longitudinal < helper_.getMinimumPrepareDistance()) { break; } diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index 6ed67fc817f28..f71024dcc5034 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -221,7 +221,8 @@ AvoidanceModuleManager::AvoidanceModuleManager( // avoidance maneuver (longitudinal) { std::string ns = "avoidance.avoidance.longitudinal."; - p.prepare_time = getOrDeclareParameter(*node, ns + "prepare_time"); + p.min_prepare_time = getOrDeclareParameter(*node, ns + "min_prepare_time"); + p.max_prepare_time = getOrDeclareParameter(*node, ns + "max_prepare_time"); p.min_prepare_distance = getOrDeclareParameter(*node, ns + "min_prepare_distance"); p.min_slow_down_speed = getOrDeclareParameter(*node, ns + "min_slow_down_speed"); p.buf_slow_down_speed = getOrDeclareParameter(*node, ns + "buf_slow_down_speed"); @@ -390,7 +391,8 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector(parameters, ns + "prepare_time", p->prepare_time); + updateParam(parameters, ns + "min_prepare_time", p->min_prepare_time); + updateParam(parameters, ns + "max_prepare_time", p->max_prepare_time); updateParam(parameters, ns + "min_slow_down_speed", p->min_slow_down_speed); updateParam(parameters, ns + "buf_slow_down_speed", p->buf_slow_down_speed); } From 0d8c9cdae82ed07891daaf4affa775129e92d542 Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Thu, 16 Nov 2023 15:19:55 +0900 Subject: [PATCH 2/4] feat(avoidance): output debug info Signed-off-by: satoshi-ota --- .../src/scene_module/avoidance/avoidance_module.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 2fb68ca00d5ba..33d10a5cc0ce7 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -448,6 +448,7 @@ bool AvoidanceModule::canYieldManeuver(const AvoidancePlanningData & data) const const auto to_shift_start_point = calcSignedArcLength( path_shifter_.getReferencePath().points, idx, registered_lines.front().start_idx); if (to_shift_start_point < helper_.getMinimumPrepareDistance()) { + RCLCPP_DEBUG(getLogger(), "prevent sudden steering."); return false; } } From f48b3e71ac8dd33a9f5cd5936d5227453934090f Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 16 Nov 2023 15:30:00 +0900 Subject: [PATCH 3/4] Update planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp Co-authored-by: Fumiya Watanabe --- .../src/scene_module/avoidance/avoidance_module.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 33d10a5cc0ce7..014d43c02739f 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -448,7 +448,7 @@ bool AvoidanceModule::canYieldManeuver(const AvoidancePlanningData & data) const const auto to_shift_start_point = calcSignedArcLength( path_shifter_.getReferencePath().points, idx, registered_lines.front().start_idx); if (to_shift_start_point < helper_.getMinimumPrepareDistance()) { - RCLCPP_DEBUG(getLogger(), "prevent sudden steering."); + RCLCPP_DEBUG(getLogger(), "Distance to shift start point is less than minimum prepare distance. The distance is not enough."); return false; } } From 7657be1dd5aaef865a196f5779a3a7f62e02143b Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 16 Nov 2023 06:33:04 +0000 Subject: [PATCH 4/4] style(pre-commit): autofix --- .../src/scene_module/avoidance/avoidance_module.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 014d43c02739f..1be7a9b232ae5 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -448,7 +448,10 @@ bool AvoidanceModule::canYieldManeuver(const AvoidancePlanningData & data) const const auto to_shift_start_point = calcSignedArcLength( path_shifter_.getReferencePath().points, idx, registered_lines.front().start_idx); if (to_shift_start_point < helper_.getMinimumPrepareDistance()) { - RCLCPP_DEBUG(getLogger(), "Distance to shift start point is less than minimum prepare distance. The distance is not enough."); + RCLCPP_DEBUG( + getLogger(), + "Distance to shift start point is less than minimum prepare distance. The distance is not " + "enough."); return false; } }