fix(avoidance): don't output new candidate path if there is huge offs…

…et between the ego and previous output path

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

planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp

@@ -187,6 +187,9 @@ struct AvoidanceParameters
   // transit hysteresis (unsafe to safe)
   double safety_check_hysteresis_factor;
 
+  // don't output new candidate path if the offset between ego and path is larger than this.
+  double safety_check_ego_offset;
+
   // keep target velocity in yield maneuver
   double yield_velocity;
 

planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp

@@ -2489,6 +2489,23 @@ BehaviorModuleOutput AvoidanceModule::plan()
   resetPathCandidate();
   resetPathReference();
 
+  // don't output new candidate path if the offset between the ego and previous output path is
+  // larger than threshold.
+  // TODO(Satoshi OTA): remove this workaround
+  {
+    const size_t ego_seg_idx =
+      planner_data_->findEgoSegmentIndex(helper_.getPreviousSplineShiftPath().path.points);
+    const auto offset = std::abs(motion_utils::calcLateralOffset(
+      helper_.getPreviousSplineShiftPath().path.points, getEgoPosition(), ego_seg_idx));
+
+    if (offset > parameters_->safety_check_ego_offset) {
+      avoidance_data_.safe_new_sl.clear();
+      RCLCPP_WARN_THROTTLE(
+        getLogger(), *clock_, 1000,
+        "there is huge offset between ego and previous path. don't throw new candidate path.");
+    }
+  }
+
   /**
    * Has new shift point?
    *   Yes -> Is it approved?

planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp

@@ -142,6 +142,7 @@ AvoidanceModuleManager::AvoidanceModuleManager(
     p.safety_check_accel_for_rss = get_parameter<double>(node, ns + "safety_check_accel_for_rss");
     p.safety_check_hysteresis_factor =
       get_parameter<double>(node, ns + "safety_check_hysteresis_factor");
+    p.safety_check_ego_offset = get_parameter<double>(node, ns + "safety_check_ego_offset");
   }
 
   // avoidance maneuver (lateral)