diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 7c785efa0d5a9..8265572ff1dcc 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/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 0b016db9c9c2c..0f142cd4cc848 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/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 587afb1fdee57..c677117e41498 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 @@ -494,6 +494,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. */ diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index f9a652a4cec27..66e3d202648a8 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -142,6 +142,7 @@ AvoidanceModuleManager::AvoidanceModuleManager( p.safety_check_accel_for_rss = get_parameter(node, ns + "safety_check_accel_for_rss"); p.safety_check_hysteresis_factor = get_parameter(node, ns + "safety_check_hysteresis_factor"); + p.safety_check_ego_offset = get_parameter(node, ns + "safety_check_ego_offset"); } // avoidance maneuver (lateral)