fix(avoidance): don't reset registered shift lines if the base offset…

… is not zero (autowarefoundation#3971) * fix(avoidance): break if the shift line start longitudinal is less than zero Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * feat(avoidance): check base offset before resetting of registered shift lines Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> --------- Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
satoshi-ota · Jun 20, 2023 · 151def8 · 151def8
1 parent 2a603c3
commit 151def8
Show file tree

Hide file tree

Showing 2 changed files with 18 additions and 11 deletions.
diff --git a/...or_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/...or_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 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{});
   }
 
   /**
    * @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
@@ -620,9 +620,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: {
@@ -2512,7 +2510,7 @@ BehaviorModuleOutput AvoidanceModule::plan()
 
   // post processing
   {
-    postProcess(path_shifter_);  // remove old shift points
+    postProcess();  // remove old shift points
   }
 
   // set previous data
@@ -2780,7 +2778,7 @@ AvoidLineArray AvoidanceModule::findNewShiftLine(const AvoidLineArray & candidat
     // this value should be larger than -eps consider path shifter calculation error.
     const double eps = 0.01;
     if (candidate.start_longitudinal < -eps) {
-      continue;
+      break;
     }
 
     if (!is_ignore_shift(candidate)) {