From 79bb93d935b8dbd5dfdbcc03d6e6be20abe4aca1 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Wed, 14 Jun 2023 17:15:13 +0900 Subject: [PATCH] fix(avoidance): don't reset registered shift lines if the base offset is not zero (#3971) * fix(avoidance): break if the shift line start longitudinal is less than zero Signed-off-by: satoshi-ota * feat(avoidance): check base offset before resetting of registered shift lines Signed-off-by: satoshi-ota --------- Signed-off-by: satoshi-ota --- .../avoidance/avoidance_module.hpp | 27 +++++++++++++++---- .../avoidance/avoidance_module.cpp | 8 +++--- 2 files changed, 25 insertions(+), 10 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index 9b39a21adaf24..54bd92bef15aa 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp @@ -311,17 +311,34 @@ class AvoidanceModule : public SceneModuleInterface void insertYieldVelocity(ShiftedPath & shifted_path) const; - void removeAllRegisteredShiftPoints(PathShifter & path_shifter) + /** + * @brief reset registered shift lines. + * @details reset only when the base offset is zero. Otherwise, sudden steering will be caused; + */ + void removeRegisteredShiftLines() { + constexpr double THRESHOLD = 0.1; + if (std::abs(path_shifter_.getBaseOffset()) > THRESHOLD) { + RCLCPP_INFO(getLogger(), "base offset is not zero. can't reset registered shift lines."); + return; + } + + initRTCStatus(); + unlockNewModuleLaunch(); + current_raw_shift_lines_.clear(); registered_raw_shift_lines_.clear(); - path_shifter.setShiftLines(ShiftLineArray{}); + path_shifter_.setShiftLines(ShiftLineArray{}); } - void postProcess(PathShifter & path_shifter) const + /** + * @brief remove behind shift lines. + * @param path shifter. + */ + void postProcess() { - const size_t nearest_idx = planner_data_->findEgoIndex(path_shifter.getReferencePath().points); - path_shifter.removeBehindShiftLineAndSetBaseOffset(nearest_idx); + const size_t idx = planner_data_->findEgoIndex(path_shifter_.getReferencePath().points); + path_shifter_.removeBehindShiftLineAndSetBaseOffset(idx); } boost::optional getFeasibleDecelDistance(const double target_velocity) 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 ede9734d6e5fe..2db0eb23751d7 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 @@ -575,9 +575,7 @@ void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, Shif case AvoidanceState::YIELD: { insertYieldVelocity(path); insertWaitPoint(parameters_->use_constraints_for_decel, path); - initRTCStatus(); - removeAllRegisteredShiftPoints(path_shifter_); - unlockNewModuleLaunch(); + removeRegisteredShiftLines(); break; } case AvoidanceState::AVOID_PATH_NOT_READY: { @@ -2818,7 +2816,7 @@ BehaviorModuleOutput AvoidanceModule::plan() // post processing { - postProcess(path_shifter_); // remove old shift points + postProcess(); // remove old shift points prev_output_ = avoidance_path; prev_linear_shift_path_ = utils::avoidance::toShiftedPath(avoidance_data_.reference_path); path_shifter_.generate(&prev_linear_shift_path_, true, SHIFT_TYPE::LINEAR); @@ -3084,7 +3082,7 @@ AvoidLineArray AvoidanceModule::findNewShiftLine( // this value should be larger than -eps consider path shifter calculation error. const double eps = 0.01; if (candidate.start_longitudinal < -eps) { - continue; + break; } // TODO(Horibe): this code prohibits the changes on ego pose. Think later.