diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 79aca0867cce9..59f52424dede9 100644 --- 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 index 42d3fad3e49bc..ca2c3423fed0a 100644 --- 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/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index 187dca71ae6c8..039f366624f82 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/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; diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 3dd5204794a00..0fb7d7bd2ac89 100644 --- 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(ns + "safety_check_accel_for_rss"); p.safety_check_hysteresis_factor = declare_parameter(ns + "safety_check_hysteresis_factor"); + p.safety_check_ego_offset = declare_parameter(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 index dda39c92463be..c37467c032f1b 100644 --- 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. */