feat(avoidance): update avoidance target detection logic

launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml

@@ -2,49 +2,50 @@
 /**:
   ros__parameters:
     avoidance:
-      resample_interval_for_planning: 0.3
-      resample_interval_for_output: 4.0
-      detection_area_right_expand_dist: 0.0
-      detection_area_left_expand_dist: 1.0
-
-      enable_avoidance_over_same_direction: true
-      enable_avoidance_over_opposite_direction: true
-
-      threshold_distance_object_is_on_center: 1.0 # [m]
-      threshold_speed_object_is_stopped: 1.0      # [m/s]
-      object_check_forward_distance: 150.0        # [m]
-      object_check_backward_distance: 2.0         # [m]
-      lateral_collision_margin: 1.0               # [m]
-      lateral_collision_safety_buffer: 0.7        # [m]
-
-      prepare_time: 2.0                           # [s]
-      min_prepare_distance: 1.0                   # [m]
-      min_avoidance_distance: 10.0                # [m]
-
-      min_nominal_avoidance_speed: 7.0  # [m/s]
-      min_sharp_avoidance_speed: 1.0    # [m/s]
-
-      road_shoulder_safety_margin: 0.5 # [m]
-
-      max_right_shift_length: 5.0
-      max_left_shift_length: 5.0
-
-      nominal_lateral_jerk: 0.2   # [m/s3]
-      max_lateral_jerk: 1.0       # [m/s3]
-
-      object_hold_max_count: 20
+      # For module common params
+      resample_interval_for_planning: 0.3             # [m]
+      resample_interval_for_output: 4.0               # [m]
+      enable_avoidance_over_same_direction: true      # [-]
+      enable_avoidance_over_opposite_direction: true  # [-]
+
+      # For avoidance target detection
+      detection_area_right_expand_dist: 0.0           # [m]
+      detection_area_left_expand_dist: 1.0            # [m]
+      threshold_distance_object_is_on_center: 1.0     # [m]
+      threshold_speed_object_is_stopped: 1.0          # [m/s]
+      threshold_time_object_is_moving: 1.0            # [s]
+      object_check_forward_distance: 150.0            # [m]
+      object_check_backward_distance: 2.0             # [m]
+      object_check_overhang: 1.0                      # [m]
+      object_check_yaw: 0.52                          # [rad]
+      object_check_road_shoulder_ratio: 0.5           # [-]
+      object_last_seen_threshold: 2.0                 # [s]
+
+      # For shift point generation
+      lateral_collision_margin: 1.0                   # [m]
+      lateral_collision_safety_buffer: 0.7            # [m]
+      prepare_time: 2.0                               # [s]
+      min_prepare_distance: 1.0                       # [m]
+      min_avoidance_distance: 10.0                    # [m]
+      min_nominal_avoidance_speed: 7.0                # [m/s]
+      min_sharp_avoidance_speed: 1.0                  # [m/s]
+      road_shoulder_safety_margin: 0.0                # [m]
+      max_right_shift_length: 5.0                     # [m]
+      max_left_shift_length: 5.0                      # [m]
+      nominal_lateral_jerk: 0.2                       # [m/s3]
+      max_lateral_jerk: 1.0                           # [m/s3]
 
       # For prevention of large acceleration while avoidance
-      min_avoidance_speed_for_acc_prevention: 3.0  # [m/s]
-      max_avoidance_acceleration: 0.5  # [m/ss]
+      min_avoidance_speed_for_acc_prevention: 3.0     # [m/s]
+      max_avoidance_acceleration: 0.5                 # [m/s2]
 
       # for debug
       publish_debug_marker: false
       print_debug_info: false
 
       # not enabled yet
-      longitudinal_collision_margin_min_distance: 0.0          # [m]
-      longitudinal_collision_margin_time: 0.0
+      longitudinal_collision_margin_min_distance: 0.0 # [m]
+      longitudinal_collision_margin_time: 0.0         # [s]
 
       # avoidance is performed for the object type with true
       target_object:
@@ -58,4 +59,4 @@
         pedestrian: false
 
       # ---------- advanced parameters ----------
-      avoidance_execution_lateral_threshold: 0.499
+      avoidance_execution_lateral_threshold: 0.499    # [m]

planning/behavior_path_planner/behavior_path_planner_avoidance-design.md

@@ -172,18 +172,48 @@ stop
 
 #### How to decide the target obstacles
 
+The avoidance target is stationary vehicles that are in or vicinity of the Ego path. As a limitation, the avoidance module does **NOT** support avoidance for moving vehicle and non vehicle objects (See following table).
+
+| Object Class | STATIONARY               | MOVING                   |
+| ------------ | ------------------------ | ------------------------ |
+| UNKNOWN      | :heavy_multiplication_x: | :heavy_multiplication_x: |
+| CAR          | :heavy_check_mark:       | :heavy_multiplication_x: |
+| TRUCK        | :heavy_check_mark:       | :heavy_multiplication_x: |
+| BUS          | :heavy_check_mark:       | :heavy_multiplication_x: |
+| TRAILER      | :heavy_check_mark:       | :heavy_multiplication_x: |
+| MOTORCYCLE   | :heavy_check_mark:       | :heavy_multiplication_x: |
+| BICYCLE      | :heavy_multiplication_x: | :heavy_multiplication_x: |
+| PEDESTRIAN   | :heavy_multiplication_x: | :heavy_multiplication_x: |
+
+##### Parking vehicle detection
+
+If the vehicle is waiting at a traffic light, stop line and crosswalk, the vehicle will eventually start moving, so the Ego can follow the target path without avoidance. On the other hand, the Ego has to avoid the vehicle parked on the road shoulder, because it is unlikely that the vehicle will be gone soon. Therefore, it is necessary to judge whether it is the parking vehicle or just a stopping vehicle.
+
+![fig1](./image/avoidance_design/target_vehicle_selection.drawio.svg)
+
+This module calculates maximum length that can be shifted and actual shift length for each vehicles, and uses the ratio of that two length as an indicator to determine whether the vehicle is a roadside parked vehicle or not.
+
+![fig1](./image/avoidance_design/target_vehicle_selection.drawio.svg)
+
+##### Merging vehicle detection
+
 The avoidance target should be limited to stationary objects (you should not avoid a vehicle waiting at a traffic light even if it blocks your path). Therefore, target vehicles for avoidance should meet the following specific conditions.
 
 - It is in the vicinity of your lane (parametrized)
 - It is stopped (estimated speed is lower than threshold)
   - This means that overtaking is not supported (will be supported in the future).
+- It is **NOT** a vehicle merging into the Ego lane.
 - It is a specific class.
+
   - User can limit avoidance targets (e.g. do not avoid unknown-class targets).
+
 - It is not being in the center of the route
   - This means that the vehicle is parked on the edge of the lane. This prevents the vehicle from avoiding a vehicle waiting at a traffic light in the middle of the lane. However, this is not an appropriate implementation for the purpose. Even if a vehicle is in the center of the lane, it should be avoided if it has its hazard lights on, and this is a point that should be improved in the future as the recognition performance improves.
 
 ![fig1](./image/avoidance_design/target_vehicle_selection.drawio.svg)
 
+##### Detection for parking vehicle
+
 ##### Compensation for detection lost
 
 In order to prevent chattering of recognition results, once an obstacle is targeted, it is hold for a while even if it disappears. This is effective when recognition is unstable. However, since it will result in over-detection (increase a number of false-positive), it is necessary to adjust parameters according to the recognition accuracy (if `object_last_seen_threshold = 0.0`, the recognition result is 100% trusted).

planning/behavior_path_planner/config/avoidance/avoidance.param.yaml

@@ -2,49 +2,50 @@
 /**:
   ros__parameters:
     avoidance:
-      resample_interval_for_planning: 0.3
-      resample_interval_for_output: 4.0
-      detection_area_right_expand_dist: 0.0
-      detection_area_left_expand_dist: 1.0
-
-      enable_avoidance_over_same_direction: true
-      enable_avoidance_over_opposite_direction: true
-
-      threshold_distance_object_is_on_center: 1.0 # [m]
-      threshold_speed_object_is_stopped: 1.0      # [m/s]
-      object_check_forward_distance: 150.0        # [m]
-      object_check_backward_distance: 2.0         # [m]
-      lateral_collision_margin: 1.0               # [m]
-      lateral_collision_safety_buffer: 0.7        # [m]
-
-      prepare_time: 2.0                           # [s]
-      min_prepare_distance: 1.0                   # [m]
-      min_avoidance_distance: 10.0                # [m]
-
-      min_nominal_avoidance_speed: 7.0  # [m/s]
-      min_sharp_avoidance_speed: 1.0    # [m/s]
-
-      road_shoulder_safety_margin: 0.0 # [m]
-
-      max_right_shift_length: 5.0
-      max_left_shift_length: 5.0
-
-      nominal_lateral_jerk: 0.2   # [m/s3]
-      max_lateral_jerk: 1.0       # [m/s3]
-
-      object_last_seen_threshold: 2.0
+      # For module common params
+      resample_interval_for_planning: 0.3             # [m]
+      resample_interval_for_output: 4.0               # [m]
+      enable_avoidance_over_same_direction: true      # [-]
+      enable_avoidance_over_opposite_direction: true  # [-]
+
+      # For avoidance target detection
+      detection_area_right_expand_dist: 0.0           # [m]
+      detection_area_left_expand_dist: 1.0            # [m]
+      threshold_distance_object_is_on_center: 1.0     # [m]
+      threshold_speed_object_is_stopped: 1.0          # [m/s]
+      threshold_time_object_is_moving: 1.0            # [s]
+      object_check_forward_distance: 150.0            # [m]
+      object_check_backward_distance: 2.0             # [m]
+      object_check_overhang: 1.0                      # [m]
+      object_check_yaw: 0.52                          # [rad]
+      object_check_road_shoulder_ratio: 0.5           # [-]
+      object_last_seen_threshold: 2.0                 # [s]
+
+      # For shift point generation
+      lateral_collision_margin: 1.0                   # [m]
+      lateral_collision_safety_buffer: 0.7            # [m]
+      prepare_time: 2.0                               # [s]
+      min_prepare_distance: 1.0                       # [m]
+      min_avoidance_distance: 10.0                    # [m]
+      min_nominal_avoidance_speed: 7.0                # [m/s]
+      min_sharp_avoidance_speed: 1.0                  # [m/s]
+      road_shoulder_safety_margin: 0.0                # [m]
+      max_right_shift_length: 5.0                     # [m]
+      max_left_shift_length: 5.0                      # [m]
+      nominal_lateral_jerk: 0.2                       # [m/s3]
+      max_lateral_jerk: 1.0                           # [m/s3]
 
       # For prevention of large acceleration while avoidance
-      min_avoidance_speed_for_acc_prevention: 3.0  # [m/s]
-      max_avoidance_acceleration: 0.5  # [m/s2]
+      min_avoidance_speed_for_acc_prevention: 3.0     # [m/s]
+      max_avoidance_acceleration: 0.5                 # [m/s2]
 
       # for debug
       publish_debug_marker: false
       print_debug_info: false
 
       # not enabled yet
-      longitudinal_collision_margin_min_distance: 0.0          # [m]
-      longitudinal_collision_margin_time: 0.0
+      longitudinal_collision_margin_min_distance: 0.0 # [m]
+      longitudinal_collision_margin_time: 0.0         # [s]
 
       # avoidance is performed for the object type with true
       target_object:
@@ -58,4 +59,4 @@
         pedestrian: false
 
       # ---------- advanced parameters ----------
-      avoidance_execution_lateral_threshold: 0.499
+      avoidance_execution_lateral_threshold: 0.499    # [m]

planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp

@@ -204,14 +204,18 @@ class AvoidanceModule : public SceneModuleInterface
 
   // -- for pre-processing --
   void initVariables();
+
   AvoidancePlanningData calcAvoidancePlanningData(DebugData & debug) const;
-  ObjectDataArray calcAvoidanceTargetObjects(
-    const lanelet::ConstLanelets & lanelets, const PathWithLaneId & reference_path,
-    DebugData & debug) const;
+
+  void fillAvoidanceTargetObjects(AvoidancePlanningData & avoidance_data, DebugData & debug) const;
+
+  void fillObjectEnvelopePolygon(const Pose & closest_pose, ObjectData & object_data) const;
 
   ObjectDataArray registered_objects_;
   void updateRegisteredObject(const ObjectDataArray & objects);
-  void CompensateDetectionLost(ObjectDataArray & objects) const;
+  void compensateDetectionLost(
+    ObjectDataArray & target_objects, ObjectDataArray & ignore_objects) const;
+  void fillObjectMovingTime(ObjectData & object_data) const;
 
   // -- for shift point generation --
   AvoidLineArray calcShiftLines(AvoidLineArray & current_raw_shift_lines, DebugData & debug) const;
@@ -283,7 +287,8 @@ class AvoidanceModule : public SceneModuleInterface
 
   // debug
   mutable DebugData debug_data_;
-  void setDebugData(const PathShifter & shifter, const DebugData & debug);
+  void setDebugData(
+    const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug) const;
   void updateAvoidanceDebugData(std::vector<AvoidanceDebugMsg> & avoidance_debug_msg_array) const;
   mutable std::vector<AvoidanceDebugMsg> debug_avoidance_initializer_for_shift_line_;
   mutable rclcpp::Time debug_avoidance_initializer_for_shift_line_time_;
@@ -313,6 +318,11 @@ class AvoidanceModule : public SceneModuleInterface
   double getCurrentBaseShift() const { return path_shifter_.getBaseOffset(); }
   double getCurrentShift() const;
   double getCurrentLinearShift() const;
+
+  /**
+   * avoidance module misc data
+   */
+  mutable ObjectDataArray stopped_objects_;
 };
 
 }  // namespace behavior_path_planner

planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp

@@ -18,6 +18,7 @@
 #include "behavior_path_planner/scene_module/utils/path_shifter.hpp"
 
 #include <rclcpp/rclcpp.hpp>
+#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
 
 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
@@ -34,10 +35,13 @@ namespace behavior_path_planner
 using autoware_auto_perception_msgs::msg::PredictedObject;
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
 
+using tier4_autoware_utils::Point2d;
+using tier4_autoware_utils::Polygon2d;
 using tier4_planning_msgs::msg::AvoidanceDebugMsgArray;
 
 using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Pose;
+using geometry_msgs::msg::TransformStamped;
 
 struct AvoidanceParameters
 {
@@ -74,6 +78,21 @@ struct AvoidanceParameters
   // continue to detect backward vehicles as avoidance targets until they are this distance away
   double object_check_backward_distance;
 
+  // overhang threshold to judge whether object merges ego lane
+  double object_check_overhang;
+
+  // yaw threshold to detect merging vehicles
+  double object_check_yaw;
+
+  // ratio between object width and distance to shoulder
+  double object_check_road_shoulder_ratio;
+
+  // For object's enveloped polygon
+  double object_envelope_buffer;
+
+  // vehicles which is moving more than this parameter will not be avoided
+  double threshold_time_object_is_moving;
+
   // we want to keep this lateral margin when avoiding
   double lateral_collision_margin;
   // a buffer in case lateral_collision_margin is set to 0. Will throw error
@@ -112,6 +131,9 @@ struct AvoidanceParameters
   // road shoulder.
   double road_shoulder_safety_margin{1.0};
 
+  // minimum road shoulder width. maybe 0.5 [m]
+  double minimum_road_shoulder_width;
+
   // Even if the obstacle is very large, it will not avoid more than this length for right direction
   double max_right_shift_length;
 
@@ -187,6 +209,16 @@ struct ObjectData  // avoidance target
   // lateral distance to the closest footprint, in Frenet coordinate
   double overhang_dist;
 
+  // object yaw relative to path pose [rad]
+  double relative_yaw;
+
+  // lateral offset ratio
+  double offset_ratio{0.0};
+
+  // count up when object moved. Removed when it exceeds threshold.
+  rclcpp::Time last_stop;
+  double move_time{0.0};
+
   // count up when object disappeared. Removed when it exceeds threshold.
   rclcpp::Time last_seen;
   double lost_time{0.0};
@@ -197,8 +229,20 @@ struct ObjectData  // avoidance target
   // the position of the overhang
   Pose overhang_pose;
 
+  // envelope polygon centroid
+  Point2d centroid{};
+
+  // envelope polygon
+  Polygon2d envelope_poly{};
+
   // lateral distance from overhang to the road shoulder
   double to_road_shoulder_distance{0.0};
+
+  // lateral distance from object to the left road boundary
+  double to_road_left_boundary_distance{0.0};
+
+  // ignore reason for debug
+  std::string reason{""};
 };
 using ObjectDataArray = std::vector<ObjectData>;
 
@@ -252,7 +296,10 @@ struct AvoidancePlanningData
   lanelet::ConstLanelets current_lanelets;
 
   // avoidance target objects
-  ObjectDataArray objects;
+  ObjectDataArray target_objects;
+
+  // avoidance ignore objects
+  ObjectDataArray ignore_objects;
 };
 
 /*