fix(avoidance): ignore unavoidable objects around the goal

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]
 

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;
 

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);

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;