fix(static_obstacle_avoidance): change avoidance condition (autowaref…

…oundation#8433) Signed-off-by: satoshi-ota <[email protected]> Signed-off-by: Mukunda Bharatheesha <[email protected]>
nobleo · Aug 16, 2024 · 0d21797 · 0d21797
1 parent c3767a9
commit 0d21797
Showing 1 changed file with 40 additions and 0 deletions.
diff --git a/...havior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp b/...havior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp
@@ -772,6 +772,46 @@ bool isObviousAvoidanceTarget(
     }
   }
 
+  if (object.behavior == ObjectData::Behavior::MERGING) {
+    object.info = ObjectInfo::MERGING_TO_EGO_LANE;
+    if (
+      isOnRight(object) && !object.is_on_ego_lane &&
+      object.overhang_points.front().first < parameters->th_overhang_distance) {
+      RCLCPP_DEBUG(
+        rclcpp::get_logger(logger_namespace),
+        "merging vehicle. but overhang distance is less than threshold.");
+      return true;
+    }
+    if (
+      !isOnRight(object) && !object.is_on_ego_lane &&
+      object.overhang_points.front().first > -1.0 * parameters->th_overhang_distance) {
+      RCLCPP_DEBUG(
+        rclcpp::get_logger(logger_namespace),
+        "merging vehicle. but overhang distance is less than threshold.");
+      return true;
+    }
+  }
+
+  if (object.behavior == ObjectData::Behavior::DEVIATING) {
+    object.info = ObjectInfo::DEVIATING_FROM_EGO_LANE;
+    if (
+      isOnRight(object) && !object.is_on_ego_lane &&
+      object.overhang_points.front().first < parameters->th_overhang_distance) {
+      RCLCPP_DEBUG(
+        rclcpp::get_logger(logger_namespace),
+        "deviating vehicle. but overhang distance is less than threshold.");
+      return true;
+    }
+    if (
+      !isOnRight(object) && !object.is_on_ego_lane &&
+      object.overhang_points.front().first > -1.0 * parameters->th_overhang_distance) {
+      RCLCPP_DEBUG(
+        rclcpp::get_logger(logger_namespace),
+        "deviating vehicle. but overhang distance is less than threshold.");
+      return true;
+    }
+  }
+
   if (!object.is_parked) {
     object.info = ObjectInfo::IS_NOT_PARKING_OBJECT;
   }