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

…et between the ego and previous output path (autowarefoundation#4150) * fix(avoidance): don't output new candidate path if there is huge offset between the ego and previous output path Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * docs(avoidance): add parameter description Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> --------- Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
satoshi-ota · Jul 5, 2023 · 952f590 · 952f590
1 parent da10a8f
commit 952f590
Show file tree

Hide file tree

Showing 5 changed files with 28 additions and 7 deletions.
diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml
@@ -127,6 +127,7 @@
         safety_check_idling_time: 1.5                    # [s]
         safety_check_accel_for_rss: 2.5                  # [m/ss]
         safety_check_hysteresis_factor: 2.0              # [-]
+        safety_check_ego_offset: 1.0                     # [m]
 
       # For avoidance maneuver
       avoidance:

diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md
@@ -634,13 +634,14 @@ namespace: `avoidance.target_filtering.`
 
 namespace: `avoidance.safety_check.`
 
-| Name                           | Unit   | Type   | Description                                                                       | Default value |
-| :----------------------------- | ------ | ------ | --------------------------------------------------------------------------------- | ------------- |
-| safety_check_backward_distance | [m]    | double | Backward distance to search the dynamic objects.                                  | 50.0          |
-| safety_check_time_horizon      | [s]    | double | Time horizon to check lateral/longitudinal margin is enough or not.               | 10.0          |
-| safety_check_idling_time       | [t]    | double | Time delay constant that be use for longitudinal margin calculation based on RSS. | 1.5           |
-| safety_check_accel_for_rss     | [m/ss] | double | Accel constant that be used for longitudinal margin calculation based on RSS.     | 2.5           |
-| safety_check_hysteresis_factor | [-]    | double | Hysteresis factor that be used for chattering prevention.                         | 2.0           |
+| Name                           | Unit   | Type   | Description                                                                                                | Default value |
+| :----------------------------- | ------ | ------ | ---------------------------------------------------------------------------------------------------------- | ------------- |
+| safety_check_backward_distance | [m]    | double | Backward distance to search the dynamic objects.                                                           | 50.0          |
+| safety_check_time_horizon      | [s]    | double | Time horizon to check lateral/longitudinal margin is enough or not.                                        | 10.0          |
+| safety_check_idling_time       | [t]    | double | Time delay constant that be use for longitudinal margin calculation based on RSS.                          | 1.5           |
+| safety_check_accel_for_rss     | [m/ss] | double | Accel constant that be used for longitudinal margin calculation based on RSS.                              | 2.5           |
+| safety_check_hysteresis_factor | [-]    | double | Hysteresis factor that be used for chattering prevention.                                                  | 2.0           |
+| safety_check_ego_offset        | [m]    | double | Output new avoidance path **only when** the offset between ego and previous output path is less than this. | 1.0           |
 
 ### Avoidance maneuver parameters
 

diff --git a/...vior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/...vior_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;
 

diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -621,6 +621,7 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam()
     p.safety_check_accel_for_rss = declare_parameter<double>(ns + "safety_check_accel_for_rss");
     p.safety_check_hysteresis_factor =
       declare_parameter<double>(ns + "safety_check_hysteresis_factor");
+    p.safety_check_ego_offset = declare_parameter<double>(ns + "safety_check_ego_offset");
   }
 
   // avoidance maneuver (lateral)

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
@@ -548,6 +548,21 @@ void AvoidanceModule::fillEgoStatus(
 
   const auto can_yield_maneuver = canYieldManeuver(data);
 
+  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));
+
+  // 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
+  if (offset > parameters_->safety_check_ego_offset) {
+    data.safe_new_sl.clear();
+    data.candidate_path = helper_.getPreviousSplineShiftPath();
+    RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 500, "unsafe. canceling candidate path...");
+    return;
+  }
+
   /**
    * If the avoidance path is safe, use unapproved_new_sl for avoidance path generation.
    */