feat(behavior_path_planner/route_handler): better avoidance drivable areas extension in behavior path planning

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

@@ -7,11 +7,15 @@
       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: 2.0               # [m]
+      lateral_collision_margin: 1.5               # [m]
+      lateral_collision_safety_buffer: 0.5        # [m]
 
       prepare_time: 2.0                           # [s]
       min_prepare_distance: 1.0                   # [m]
@@ -20,6 +24,8 @@
       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
 
@@ -30,7 +36,7 @@
 
       # For prevention of large acceleration while avoidance
       min_avoidance_speed_for_acc_prevention: 3.0  # [m/s]
-      max_avoidance_acceleration: 0.5  # [m/ss]
+      max_avoidance_acceleration: 0.5  # [m/s2]
 
       # for debug
       publish_debug_marker: false

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

@@ -121,7 +121,7 @@ class AvoidanceModule : public SceneModuleInterface
 
   // -- for shift point operations --
   void alignShiftPointsOrder(
-    AvoidPointArray & shift_points, const bool recalc_start_length = true) const;
+    AvoidPointArray & shift_points, const bool recalculate_start_length = true) const;
   AvoidPointArray fillAdditionalInfo(const AvoidPointArray & shift_points) const;
   AvoidPoint fillAdditionalInfo(const AvoidPoint & shift_point) const;
   void fillAdditionalInfoFromPoint(AvoidPointArray & shift_points) const;
@@ -135,7 +135,7 @@ class AvoidanceModule : public SceneModuleInterface
 
   // -- path generation --
   ShiftedPath generateAvoidancePath(PathShifter & shifter) const;
-  void generateExtendedDrivableArea(ShiftedPath * shifted_path, double margin) const;
+  void generateExtendedDrivableArea(ShiftedPath * shifted_path) const;
 
   // -- velocity planning --
   void modifyPathVelocityToPreventAccelerationOnAvoidance(ShiftedPath & shifted_path) const;

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

@@ -52,6 +52,13 @@ struct AvoidanceParameters
   // lanelet expand length for left side to find avoidance target vehicles
   double detection_area_left_expand_dist = 1.0;
 
+  // enable avoidance to be perform only in lane with same direction
+  bool enable_avoidance_over_same_direction{true};
+
+  // enable avoidance to be perform in opposite lane direction
+  // to use this, enable_avoidance_over_same_direction need to be set to true.
+  bool enable_avoidance_over_opposite_direction{true};
+
   // Vehicles whose distance to the center of the path is
   // less than this will not be considered for avoidance.
   double threshold_distance_object_is_on_center;
@@ -67,6 +74,9 @@ struct AvoidanceParameters
 
   // 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
+  // don't ever set this value to 0
+  double lateral_collision_safety_buffer{0.5};
 
   // when complete avoidance motion, there is a distance margin with the object
   // for longitudinal direction
@@ -96,6 +106,10 @@ struct AvoidanceParameters
   // distance for avoidance. Need a sharp avoidance path to avoid the object.
   double min_sharp_avoidance_speed;
 
+  // The margin is configured so that the generated avoidance trajectory does not come near to the
+  // road shoulder.
+  double road_shoulder_safety_margin{1.0};
+
   // Even if the obstacle is very large, it will not avoid more than this length for right direction
   double max_right_shift_length;
 
@@ -152,6 +166,15 @@ struct ObjectData  // avoidance target
 
   // count up when object disappeared. Removed when it exceeds threshold.
   int lost_count = 0;
+
+  // store the information of the lanelet which the object's overhang is currently occupying
+  lanelet::ConstLanelet overhang_lanelet;
+
+  // the position of the overhang
+  Pose overhang_pose;
+
+  // lateral distance from overhang to the road shoulder
+  double to_road_shoulder_distance{0.0};
 };
 using ObjectDataArray = std::vector<ObjectData>;
 

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

@@ -46,7 +46,8 @@ void clipByMinStartIdx(const AvoidPointArray & shift_points, PathWithLaneId & pa
 double calcDistanceToClosestFootprintPoint(
   const PathWithLaneId & path, const PredictedObject & object, const Point & ego_pos);
 
-double calcOverhangDistance(const ObjectData & object_data, const Pose & base_pose);
+double calcOverhangDistance(
+  const ObjectData & object_data, const Pose & base_pose, Point & overhang_pose);
 
 void setEndData(
   AvoidPoint & ap, const double length, const geometry_msgs::msg::Pose & end, const size_t end_idx,

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -207,12 +207,15 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam()
   p.resample_interval_for_output = dp("resample_interval_for_output", 3.0);
   p.detection_area_right_expand_dist = dp("detection_area_right_expand_dist", 0.0);
   p.detection_area_left_expand_dist = dp("detection_area_left_expand_dist", 1.0);
+  p.enable_avoidance_over_same_direction = dp("enable_avoidance_over_same_direction", true);
+  p.enable_avoidance_over_opposite_direction = dp("enable_avoidance_over_opposite_direction", true);
 
   p.threshold_distance_object_is_on_center = dp("threshold_distance_object_is_on_center", 1.0);
   p.threshold_speed_object_is_stopped = dp("threshold_speed_object_is_stopped", 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.lateral_collision_margin = dp("lateral_collision_margin", 2.0);
+  p.lateral_collision_safety_buffer = dp("lateral_collision_safety_buffer", 0.5);
 
   p.prepare_time = dp("prepare_time", 3.0);
   p.min_prepare_distance = dp("min_prepare_distance", 10.0);
@@ -221,6 +224,8 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam()
   p.min_nominal_avoidance_speed = dp("min_nominal_avoidance_speed", 5.0);
   p.min_sharp_avoidance_speed = dp("min_sharp_avoidance_speed", 1.0);
 
+  p.road_shoulder_safety_margin = dp("road_shoulder_safety_margin", 0.5);
+
   p.max_right_shift_length = dp("max_right_shift_length", 1.5);
   p.max_left_shift_length = dp("max_left_shift_length", 1.5);