From 0dc505cbbd83002f4d16d64a68a58034544e36ad Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Thu, 5 Jan 2023 19:20:14 +0900 Subject: [PATCH] fix(avoidance): ignore unavoidable objects around the goal (#2612) (#234) * fix(avoidance): ignore unavoidable objects around the goal Signed-off-by: satoshi-ota * Update planning/behavior_path_planner/config/avoidance/avoidance.param.yaml * Update planning/behavior_path_planner/src/behavior_path_planner_node.cpp Signed-off-by: satoshi-ota Co-authored-by: Kosuke Takeuchi Signed-off-by: satoshi-ota Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> --- .../config/avoidance/avoidance.param.yaml | 1 + .../scene_module/avoidance/avoidance_module_data.hpp | 4 ++++ .../src/behavior_path_planner_node.cpp | 1 + .../src/scene_module/avoidance/avoidance_module.cpp | 7 +++++++ 4 files changed, 13 insertions(+) diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 5e2e4673166da..8a2216c237572 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -17,6 +17,7 @@ object_check_forward_distance: 150.0 # [m] object_check_backward_distance: 2.0 # [m] + object_check_goal_distance: 20.0 # [m] threshold_distance_object_is_on_center: 1.0 # [m] diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp index 25b29352973c1..dfee79a09bb1e 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp @@ -81,6 +81,10 @@ struct AvoidanceParameters // continue to detect backward vehicles as avoidance targets until they are this distance away double object_check_backward_distance; + // if the distance between object and goal position is less than this parameter, the module ignore + // the object. + double object_check_goal_distance; + // use in judge whether the vehicle is parking object on road shoulder double object_check_shiftable_ratio; 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 e1f64eb04770a..80c2e16f9e9a2 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -294,6 +294,7 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam() p.threshold_time_object_is_moving = dp("threshold_time_object_is_moving", 1.0); p.object_check_forward_distance = dp("object_check_forward_distance", 150.0); p.object_check_backward_distance = dp("object_check_backward_distance", 2.0); + p.object_check_goal_distance = dp("object_check_goal_distance", 20.0); p.object_check_shiftable_ratio = dp("object_check_shiftable_ratio", 1.0); p.object_check_min_road_shoulder_width = dp("object_check_min_road_shoulder_width", 0.5); p.object_envelope_buffer = dp("object_envelope_buffer", 0.1); 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 1f7979c9b726c..2cbf6af88247a 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 @@ -286,6 +286,13 @@ void AvoidanceModule::fillAvoidanceTargetObjects( continue; } + if (object_data.longitudinal + parameters_->object_check_goal_distance > dist_to_goal) { + avoidance_debug_array_false_and_push_back("TooNearToGoal"); + object_data.reason = "TooNearToGoal"; + data.other_objects.push_back(object_data); + continue; + } + // Calc lateral deviation from path to target object. object_data.lateral = calcLateralDeviation(object_closest_pose, object_pose.position); avoidance_debug_msg.lateral_distance_from_centerline = object_data.lateral;