fix(avoidance): prevent sudden steering at yield maneuver

Signed-off-by: satoshi-ota <[email protected]>
autowarefoundation · Nov 16, 2023 · 79baead · 79baead
1 parent f7d6c26
commit 79baead
Showing 1 changed file with 12 additions and 0 deletions.
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
@@ -427,6 +427,18 @@ bool AvoidanceModule::canYieldManeuver(const AvoidancePlanningData & data) const
     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);
+    constexpr double FIXED_PATH_TIME = 1.0;
+    if (to_shift_start_point < FIXED_PATH_TIME * getEgoSpeed()) {
+      return false;
+    }
+  }
+
   if (!data.stop_target_object) {
     return true;
   }