Merge pull request #1195 from tier4/hotfix/v0.44.5/avoidance

fix(avoidance): cherry pick PRs for avoidance module

Author: shmpwk

planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lane_change.param.yaml

@@ -12,64 +12,80 @@
           moving_time_threshold: 1.0                   # [s]
           max_expand_ratio: 0.0                        # [-]
           envelope_buffer_margin: 0.3                  # [m]
-          avoid_margin_lateral: 1.0                    # [m]
-          safety_buffer_lateral: 0.7                   # [m]
+          lateral_margin:
+            soft_margin: 0.0                           # [m]
+            hard_margin: 0.0                           # [m]
+            hard_margin_for_parked_vehicle: 0.0        # [m]
         truck:
           execute_num: 2
           moving_speed_threshold: 1.0                  # 3.6km/h
           moving_time_threshold: 1.0
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.3
-          avoid_margin_lateral: 1.0
-          safety_buffer_lateral: 0.7
+          lateral_margin:
+            soft_margin: 0.0                           # [m]
+            hard_margin: 0.0                           # [m]
+            hard_margin_for_parked_vehicle: 0.0        # [m]
         bus:
           execute_num: 2
           moving_speed_threshold: 1.0                  # 3.6km/h
           moving_time_threshold: 1.0
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.3
-          avoid_margin_lateral: 1.0
-          safety_buffer_lateral: 0.7
+          lateral_margin:
+            soft_margin: 0.0                           # [m]
+            hard_margin: 0.0                           # [m]
+            hard_margin_for_parked_vehicle: 0.0        # [m]
         trailer:
           execute_num: 2
           moving_speed_threshold: 1.0                  # 3.6km/h
           moving_time_threshold: 1.0
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.3
-          avoid_margin_lateral: 1.0
-          safety_buffer_lateral: 0.7
+          lateral_margin:
+            soft_margin: 0.0                           # [m]
+            hard_margin: 0.0                           # [m]
+            hard_margin_for_parked_vehicle: 0.0        # [m]
         unknown:
           execute_num: 1
           moving_speed_threshold: 0.28                 # 1.0km/h
           moving_time_threshold: 1.0
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.3
-          avoid_margin_lateral: 1.0
-          safety_buffer_lateral: 0.0
+          lateral_margin:
+            soft_margin: 0.0                           # [m]
+            hard_margin: 0.0                           # [m]
+            hard_margin_for_parked_vehicle: 0.0        # [m]
         bicycle:
           execute_num: 2
           moving_speed_threshold: 0.28                 # 1.0km/h
           moving_time_threshold: 1.0
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.8
-          avoid_margin_lateral: 1.0
-          safety_buffer_lateral: 1.0
+          lateral_margin:
+            soft_margin: 0.0                           # [m]
+            hard_margin: 0.0                           # [m]
+            hard_margin_for_parked_vehicle: 0.0        # [m]
         motorcycle:
           execute_num: 2
           moving_speed_threshold: 1.0                  # 3.6km/h
           moving_time_threshold: 1.0
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.8
-          avoid_margin_lateral: 1.0
-          safety_buffer_lateral: 1.0
+          lateral_margin:
+            soft_margin: 0.0                           # [m]
+            hard_margin: 0.0                           # [m]
+            hard_margin_for_parked_vehicle: 0.0        # [m]
         pedestrian:
           execute_num: 2
           moving_speed_threshold: 0.28                 # 1.0km/h
           moving_time_threshold: 1.0
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.8
-          avoid_margin_lateral: 1.0
-          safety_buffer_lateral: 1.0
+          lateral_margin:
+            soft_margin: 0.0                           # [m]
+            hard_margin: 0.0                           # [m]
+            hard_margin_for_parked_vehicle: 0.0        # [m]
         lower_distance_for_polygon_expansion: 0.0      # [m]
         upper_distance_for_polygon_expansion: 1.0      # [m]
 

planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp

@@ -72,10 +72,12 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node)
       param.max_expand_ratio = getOrDeclareParameter<double>(*node, ns + "max_expand_ratio");
       param.envelope_buffer_margin =
         getOrDeclareParameter<double>(*node, ns + "envelope_buffer_margin");
-      param.avoid_margin_lateral =
-        getOrDeclareParameter<double>(*node, ns + "avoid_margin_lateral");
-      param.safety_buffer_lateral =
-        getOrDeclareParameter<double>(*node, ns + "safety_buffer_lateral");
+      param.lateral_soft_margin =
+        getOrDeclareParameter<double>(*node, ns + "lateral_margin.soft_margin");
+      param.lateral_hard_margin =
+        getOrDeclareParameter<double>(*node, ns + "lateral_margin.hard_margin");
+      param.lateral_hard_margin_for_parked_vehicle =
+        getOrDeclareParameter<double>(*node, ns + "lateral_margin.hard_margin_for_parked_vehicle");
       return param;
     };
 
@@ -130,7 +132,7 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node)
   }
 
   {
-    const std::string ns = "avoidance.target_filtering.force_avoidance.";
+    const std::string ns = "avoidance.target_filtering.avoidance_for_ambiguous_vehicle.";
     p.enable_force_avoidance_for_stopped_vehicle =
       getOrDeclareParameter<bool>(*node, ns + "enable");
     p.threshold_time_force_avoidance_for_stopped_vehicle =

planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp

@@ -89,9 +89,11 @@ bool AvoidanceByLaneChange::specialRequiredCheck() const
   const auto nearest_object_type = utils::getHighestProbLabel(nearest_object.object.classification);
   const auto nearest_object_parameter =
     avoidance_parameters_->object_parameters.at(nearest_object_type);
-  const auto avoid_margin =
-    nearest_object_parameter.safety_buffer_lateral * nearest_object.distance_factor +
-    nearest_object_parameter.avoid_margin_lateral + 0.5 * ego_width;
+  const auto lateral_hard_margin = std::max(
+    nearest_object_parameter.lateral_hard_margin,
+    nearest_object_parameter.lateral_hard_margin_for_parked_vehicle);
+  const auto avoid_margin = lateral_hard_margin * nearest_object.distance_factor +
+                            nearest_object_parameter.lateral_soft_margin + 0.5 * ego_width;
 
   avoidance_helper_->setData(planner_data_);
   const auto shift_length = avoidance_helper_->getShiftLength(

planning/behavior_path_avoidance_module/config/avoidance.param.yaml

@@ -30,81 +30,97 @@
         car:
           execute_num: 1                               # [-]
           moving_speed_threshold: 1.0                  # [m/s]
-          moving_time_threshold: 1.0                   # [s]
+          moving_time_threshold: 2.0                   # [s]
+          lateral_margin:
+            soft_margin: 0.7                           # [m]
+            hard_margin: 0.3                           # [m]
+            hard_margin_for_parked_vehicle: 0.3        # [m]
           max_expand_ratio: 0.0                        # [-]
-          envelope_buffer_margin: 0.3                  # [m]
-          avoid_margin_lateral: 1.0                    # [m]
-          safety_buffer_lateral: 0.7                   # [m]
+          envelope_buffer_margin: 0.5                  # [m]
           safety_buffer_longitudinal: 0.0              # [m]
           use_conservative_buffer_longitudinal: true   # [-] When set to true, the base_link2front is added to the longitudinal buffer before avoidance.
         truck:
           execute_num: 1
           moving_speed_threshold: 1.0                  # 3.6km/h
-          moving_time_threshold: 1.0
+          moving_time_threshold: 2.0
+          lateral_margin:
+            soft_margin: 0.9                           # [m]
+            hard_margin: 0.1                           # [m]
+            hard_margin_for_parked_vehicle: 0.1        # [m]
           max_expand_ratio: 0.0
-          envelope_buffer_margin: 0.3
-          avoid_margin_lateral: 1.0
-          safety_buffer_lateral: 0.7
+          envelope_buffer_margin: 0.5
           safety_buffer_longitudinal: 0.0
           use_conservative_buffer_longitudinal: true
         bus:
           execute_num: 1
           moving_speed_threshold: 1.0                  # 3.6km/h
-          moving_time_threshold: 1.0
+          moving_time_threshold: 2.0
+          lateral_margin:
+            soft_margin: 0.9                           # [m]
+            hard_margin: 0.1                           # [m]
+            hard_margin_for_parked_vehicle: 0.1        # [m]
           max_expand_ratio: 0.0
-          envelope_buffer_margin: 0.3
-          avoid_margin_lateral: 1.0
-          safety_buffer_lateral: 0.7
+          envelope_buffer_margin: 0.5
           safety_buffer_longitudinal: 0.0
           use_conservative_buffer_longitudinal: true
         trailer:
           execute_num: 1
           moving_speed_threshold: 1.0                  # 3.6km/h
-          moving_time_threshold: 1.0
+          moving_time_threshold: 2.0
+          lateral_margin:
+            soft_margin: 0.9                           # [m]
+            hard_margin: 0.1                           # [m]
+            hard_margin_for_parked_vehicle: 0.1        # [m]
           max_expand_ratio: 0.0
-          envelope_buffer_margin: 0.3
-          avoid_margin_lateral: 1.0
-          safety_buffer_lateral: 0.7
+          envelope_buffer_margin: 0.5
           safety_buffer_longitudinal: 0.0
           use_conservative_buffer_longitudinal: true
         unknown:
           execute_num: 1
           moving_speed_threshold: 0.28                 # 1.0km/h
           moving_time_threshold: 1.0
+          lateral_margin:
+            soft_margin: 0.7                           # [m]
+            hard_margin: -0.2                          # [m]
+            hard_margin_for_parked_vehicle: -0.2       # [m]
           max_expand_ratio: 0.0
-          envelope_buffer_margin: 0.3
-          avoid_margin_lateral: 1.0
-          safety_buffer_lateral: 0.7
+          envelope_buffer_margin: 0.1
           safety_buffer_longitudinal: 0.0
           use_conservative_buffer_longitudinal: true
         bicycle:
           execute_num: 1
           moving_speed_threshold: 0.28                 # 1.0km/h
           moving_time_threshold: 1.0
+          lateral_margin:
+            soft_margin: 0.7                           # [m]
+            hard_margin: 0.5                           # [m]
+            hard_margin_for_parked_vehicle: 0.5        # [m]
           max_expand_ratio: 0.0
-          envelope_buffer_margin: 0.8
-          avoid_margin_lateral: 1.0
-          safety_buffer_lateral: 1.0
+          envelope_buffer_margin: 0.5
           safety_buffer_longitudinal: 1.0
           use_conservative_buffer_longitudinal: true
         motorcycle:
           execute_num: 1
           moving_speed_threshold: 1.0                  # 3.6km/h
           moving_time_threshold: 1.0
+          lateral_margin:
+            soft_margin: 0.7                           # [m]
+            hard_margin: 0.3                           # [m]
+            hard_margin_for_parked_vehicle: 0.3        # [m]
           max_expand_ratio: 0.0
-          envelope_buffer_margin: 0.8
-          avoid_margin_lateral: 1.0
-          safety_buffer_lateral: 1.0
+          envelope_buffer_margin: 0.5
           safety_buffer_longitudinal: 1.0
           use_conservative_buffer_longitudinal: true
         pedestrian:
           execute_num: 1
           moving_speed_threshold: 0.28                 # 1.0km/h
           moving_time_threshold: 1.0
+          lateral_margin:
+            soft_margin: 0.7                           # [m]
+            hard_margin: 0.5                           # [m]
+            hard_margin_for_parked_vehicle: 0.5        # [m]
           max_expand_ratio: 0.0
-          envelope_buffer_margin: 0.8
-          avoid_margin_lateral: 1.0
-          safety_buffer_lateral: 1.0
+          envelope_buffer_margin: 0.5
           safety_buffer_longitudinal: 1.0
           use_conservative_buffer_longitudinal: true
         lower_distance_for_polygon_expansion: 30.0      # [m]
@@ -140,7 +156,7 @@
           backward_distance: 10.0                        # [m]
 
         # params for avoidance of vehicle type objects that are ambiguous as to whether they are parked.
-        force_avoidance:
+        avoidance_for_ambiguous_vehicle:
           enable: false                                  # [-]
           time_threshold: 1.0                            # [s]
           distance_threshold: 1.0                        # [m]

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp

@@ -69,9 +69,11 @@ struct ObjectParameter
 
   double envelope_buffer_margin{0.0};
 
-  double avoid_margin_lateral{1.0};
+  double lateral_soft_margin{1.0};
 
-  double safety_buffer_lateral{1.0};
+  double lateral_hard_margin{1.0};
+
+  double lateral_hard_margin_for_parked_vehicle{1.0};
 
   double safety_buffer_longitudinal{0.0};
 
@@ -382,7 +384,8 @@ struct ObjectData  // avoidance target
   rclcpp::Time last_move;
   double stop_time{0.0};
 
-  // store the information of the lanelet which the object's overhang is currently occupying
+  // It is one of the ego driving lanelets (closest lanelet to the object) and used in the logic to
+  // check whether the object is on the ego lane.
   lanelet::ConstLanelet overhang_lanelet;
 
   // the position at the detected moment
@@ -415,6 +418,15 @@ struct ObjectData  // avoidance target
   // is within intersection area
   bool is_within_intersection{false};
 
+  // is parked vehicle on road shoulder
+  bool is_parked{false};
+
+  // is driving on ego current lane
+  bool is_on_ego_lane{false};
+
+  // is ambiguous stopped vehicle.
+  bool is_ambiguous{false};
+
   // object direction.
   Direction direction{Direction::NONE};
 

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp

@@ -17,6 +17,7 @@
 
 #include "behavior_path_avoidance_module/data_structs.hpp"
 #include "behavior_path_avoidance_module/utils.hpp"
+#include "behavior_path_planner_common/utils/utils.hpp"
 
 #include <motion_utils/distance/distance.hpp>
 
@@ -134,6 +135,24 @@ class AvoidanceHelper
       shift_length, getLateralMaxJerkLimit(), getAvoidanceEgoSpeed());
   }
 
+  double getFrontConstantDistance(const ObjectData & object) const
+  {
+    const auto object_type = utils::getHighestProbLabel(object.object.classification);
+    const auto object_parameter = parameters_->object_parameters.at(object_type);
+    const auto & additional_buffer_longitudinal =
+      object_parameter.use_conservative_buffer_longitudinal ? data_->parameters.base_link2front
+                                                            : 0.0;
+    return object_parameter.safety_buffer_longitudinal + additional_buffer_longitudinal;
+  }
+
+  double getRearConstantDistance(const ObjectData & object) const
+  {
+    const auto object_type = utils::getHighestProbLabel(object.object.classification);
+    const auto object_parameter = parameters_->object_parameters.at(object_type);
+    return object_parameter.safety_buffer_longitudinal + data_->parameters.base_link2rear +
+           object.length;
+  }
+
   double getEgoShift() const
   {
     validate();

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp

@@ -72,10 +72,12 @@ AvoidanceParameters getParameter(rclcpp::Node * node)
       param.max_expand_ratio = getOrDeclareParameter<double>(*node, ns + "max_expand_ratio");
       param.envelope_buffer_margin =
         getOrDeclareParameter<double>(*node, ns + "envelope_buffer_margin");
-      param.avoid_margin_lateral =
-        getOrDeclareParameter<double>(*node, ns + "avoid_margin_lateral");
-      param.safety_buffer_lateral =
-        getOrDeclareParameter<double>(*node, ns + "safety_buffer_lateral");
+      param.lateral_soft_margin =
+        getOrDeclareParameter<double>(*node, ns + "lateral_margin.soft_margin");
+      param.lateral_hard_margin =
+        getOrDeclareParameter<double>(*node, ns + "lateral_margin.hard_margin");
+      param.lateral_hard_margin_for_parked_vehicle =
+        getOrDeclareParameter<double>(*node, ns + "lateral_margin.hard_margin_for_parked_vehicle");
       param.safety_buffer_longitudinal =
         getOrDeclareParameter<double>(*node, ns + "safety_buffer_longitudinal");
       param.use_conservative_buffer_longitudinal =
@@ -138,7 +140,7 @@ AvoidanceParameters getParameter(rclcpp::Node * node)
   }
 
   {
-    const std::string ns = "avoidance.target_filtering.force_avoidance.";
+    const std::string ns = "avoidance.target_filtering.avoidance_for_ambiguous_vehicle.";
     p.enable_force_avoidance_for_stopped_vehicle =
       getOrDeclareParameter<bool>(*node, ns + "enable");
     p.threshold_time_force_avoidance_for_stopped_vehicle =