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 3d7479f54ed11..e17c1e189ed8a 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 @@ -578,23 +578,32 @@ class AvoidanceModule : public SceneModuleInterface /** * @brief reset registered shift lines. - * @param path shifter. + * @details reset only when the base offset is zero. Otherwise, sudden steering will be caused; */ - void removeAllRegisteredShiftPoints(PathShifter & path_shifter) + void removeRegisteredShifLines() { + constexpr double THRESHOLD = 0.1; + if (std::abs(path_shifter_.getBaseOffset()) > THRESHOLD) { + RCLCPP_INFO(getLogger(), "base offset is not zero. can't reset resistered shift lines."); + return; + } + + initRTCStatus(); + unlockNewModuleLaunch(); + current_raw_shift_lines_.clear(); registered_raw_shift_lines_.clear(); - path_shifter.setShiftLines(ShiftLineArray{}); + path_shifter_.setShiftLines(ShiftLineArray{}); } /** * @brief remove behind shift lines. * @param path shifter. */ - void postProcess(PathShifter & path_shifter) const + 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); } // misc functions 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 00668afe6c67a..b15e56c11a6aa 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 @@ -653,9 +653,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(); + removeRegisteredShifLines(); break; } case AvoidanceState::AVOID_PATH_NOT_READY: { @@ -2555,7 +2553,7 @@ BehaviorModuleOutput AvoidanceModule::plan() // post processing { - postProcess(path_shifter_); // remove old shift points + postProcess(); // remove old shift points } // set previous data