feat(avoidance): check base offset before resetting of registered shi…

…ft lines

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

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

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