fix(avoidance): ignore unavoidable objects around the goal (autowaref…

…oundation#2612) * fix(avoidance): ignore unavoidable objects around the goal Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * 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 <satoshi.ota928@gmail.com> Co-authored-by: Kosuke Takeuchi <kosuke.tnp@gmail.com>
sujithvemi · Jan 5, 2023 · 316f5e9 · 316f5e9
1 parent fe91a4a
commit 316f5e9
Show file tree

Hide file tree

Showing 4 changed files with 13 additions and 0 deletions.
diff --git 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: 20.0            # [m]
 
       threshold_distance_object_is_on_center: 1.0 # [m]
 

diff --git a/...th_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp b/...th_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
@@ -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", 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
@@ -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;