fix(avoidance): add target filtering threshold for merging/deviating …

…vehicle Signed-off-by: satoshi-ota <[email protected]>
autowarefoundation · Apr 12, 2024 · ced46ed · ced46ed
1 parent 682a814
commit ced46ed
Show file tree

Hide file tree

Showing 5 changed files with 61 additions and 0 deletions.
diff --git a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml
@@ -132,6 +132,10 @@
           th_shiftable_ratio: 0.8                       # [-]
           min_road_shoulder_width: 0.5                  # [m] FOR DEVELOPER
 
+        # for merging/deviating vehicle
+        merging_vehicle:
+          th_overhang_distance: 0.5                     # [m]
+
         # params for avoidance of vehicle type objects that are ambiguous as to whether they are parked.
         avoidance_for_ambiguous_vehicle:
           enable: true                                  # [-]

diff --git a/...ng/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp b/...ng/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp
@@ -189,6 +189,9 @@ struct AvoidanceParameters
   double time_threshold_for_ambiguous_vehicle{0.0};
   double distance_threshold_for_ambiguous_vehicle{0.0};
 
+  // for merging/deviating vehicle
+  double th_overhang_distance{0.0};
+
   // parameters for safety check area
   bool enable_safety_check{false};
   bool check_current_lane{false};

diff --git a/planning/behavior_path_avoidance_module/schema/avoidance.schema.json b/planning/behavior_path_avoidance_module/schema/avoidance.schema.json
@@ -681,6 +681,18 @@
               ],
               "additionalProperties": false
             },
+            "merging_vehicle": {
+              "type": "object",
+              "properties": {
+                "th_overhang_distance": {
+                  "type": "number",
+                  "description": "Distance threshold between overhang point and ego lane's centerline. If the nearest overhang point of merging/deviating vehicle is less than this param, the module never avoid it. (Basically, the ego stopps behind of it.)",
+                  "default": 0.5
+                }
+              },
+              "required": ["th_overhang_distance"],
+              "additionalProperties": false
+            },
             "parked_vehicle": {
               "type": "object",
               "properties": {
@@ -803,6 +815,7 @@
             "object_check_return_pose_distance",
             "max_compensation_time",
             "detection_area",
+            "merging_vehicle",
             "parked_vehicle",
             "avoidance_for_ambiguous_vehicle",
             "intersection"

diff --git a/planning/behavior_path_avoidance_module/src/debug.cpp b/planning/behavior_path_avoidance_module/src/debug.cpp
@@ -578,6 +578,7 @@ MarkerArray createDebugMarkerArray(
     addObjects(data.other_objects, std::string("TooNearToGoal"));
     addObjects(data.other_objects, std::string("ParallelToEgoLane"));
     addObjects(data.other_objects, std::string("MergingToEgoLane"));
+    addObjects(data.other_objects, std::string("DeviatingFromEgoLane"));
     addObjects(data.other_objects, std::string("UnstableObject"));
     addObjects(data.other_objects, std::string("AmbiguousStoppedVehicle"));
     addObjects(data.other_objects, std::string("AmbiguousStoppedVehicle(wait-and-see)"));

diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp
@@ -574,6 +574,46 @@ bool isNeverAvoidanceTarget(
     }
   }
 
+  if (object.behavior == ObjectData::Behavior::MERGING) {
+    object.reason = "MergingToEgoLane";
+    if (
+      isOnRight(object) &&
+      object.overhang_points.front().first > parameters->th_overhang_distance) {
+      RCLCPP_DEBUG(
+        rclcpp::get_logger(__func__),
+        "merging vehicle. but overhang distance is larger than threshold.");
+      return true;
+    }
+    if (
+      !isOnRight(object) &&
+      object.overhang_points.front().first < -1.0 * parameters->th_overhang_distance) {
+      RCLCPP_DEBUG(
+        rclcpp::get_logger(__func__),
+        "merging vehicle. but overhang distance is larger than threshold.");
+      return true;
+    }
+  }
+
+  if (object.behavior == ObjectData::Behavior::DEVIATING) {
+    object.reason = "DeviatingFromEgoLane";
+    if (
+      isOnRight(object) &&
+      object.overhang_points.front().first > parameters->th_overhang_distance) {
+      RCLCPP_DEBUG(
+        rclcpp::get_logger(__func__),
+        "deviating vehicle. but overhang distance is larger than threshold.");
+      return true;
+    }
+    if (
+      !isOnRight(object) &&
+      object.overhang_points.front().first < -1.0 * parameters->th_overhang_distance) {
+      RCLCPP_DEBUG(
+        rclcpp::get_logger(__func__),
+        "deviating vehicle. but overhang distance is larger than threshold.");
+      return true;
+    }
+  }
+
   if (object.is_on_ego_lane) {
     const auto right_lane =
       planner_data->route_handler->getRightLanelet(object.overhang_lanelet, true, false);