From 4aead92f35846751e1b1403e093a3dbed7f924ef Mon Sep 17 00:00:00 2001 From: satoshi-ota Date: Thu, 5 Jan 2023 15:09:44 +0900 Subject: [PATCH 1/3] fix(avoidance): ignore unavoidable objects around the goal Signed-off-by: satoshi-ota --- .../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 e96b254900869..6c33807a33dd7 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -18,6 +18,7 @@ object_check_forward_distance: 150.0 # [m] object_check_backward_distance: 2.0 # [m] + object_check_goal_distance: 0.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 90e5a2acf902d..fb3560a6bfe90 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 @@ -86,6 +86,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 562d3cc7b1513..affae10e8b8be 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -295,6 +295,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", 0.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 cc0f880223e65..c71ec9b3a2bbe 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 @@ -320,6 +320,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; From f21f41755cc4b78087701e25ceff250ca4f73d0b Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Thu, 5 Jan 2023 16:38:42 +0900 Subject: [PATCH 2/3] Update planning/behavior_path_planner/config/avoidance/avoidance.param.yaml --- .../behavior_path_planner/config/avoidance/avoidance.param.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 6c33807a33dd7..9d19382184adc 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -18,7 +18,7 @@ object_check_forward_distance: 150.0 # [m] object_check_backward_distance: 2.0 # [m] - object_check_goal_distance: 0.0 # [m] + object_check_goal_distance: 20.0 # [m] threshold_distance_object_is_on_center: 1.0 # [m] From f0e7112fca079469f2dff1266aff5a899530e2f9 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Thu, 5 Jan 2023 16:38:49 +0900 Subject: [PATCH 3/3] Update planning/behavior_path_planner/src/behavior_path_planner_node.cpp --- .../behavior_path_planner/src/behavior_path_planner_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 affae10e8b8be..d00e810e1f8f2 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -295,7 +295,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", 0.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);