Merge pull request #1280 from tier4/hotfix/v0.26.1/cherry-pick-avoidance-pr

fix(avoidance): cherry pick PRs

Author: shmpwk

common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml

@@ -37,6 +37,8 @@ Planning:
   behavior_path_planner_avoidance:
     - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner
       logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.avoidance
+    - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner
+      logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.avoidance.utils
 
   behavior_path_planner_goal_planner:
     - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner

planning/behavior_path_avoidance_module/README.md

@@ -28,7 +28,6 @@
             hard_margin_for_parked_vehicle: 0.7         # [m]
           max_expand_ratio: 0.0                         # [-] FOR DEVELOPER
           envelope_buffer_margin: 0.5                   # [m] FOR DEVELOPER
-          use_conservative_buffer_longitudinal: true    # [-] When set to true, the base_link2front is added to the longitudinal buffer before avoidance.
         truck:
           th_moving_speed: 1.0
           th_moving_time: 2.0
@@ -39,7 +38,6 @@
             hard_margin_for_parked_vehicle: 0.7
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.5
-          use_conservative_buffer_longitudinal: true
         bus:
           th_moving_speed: 1.0
           th_moving_time: 2.0
@@ -50,7 +48,6 @@
             hard_margin_for_parked_vehicle: 0.7
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.5
-          use_conservative_buffer_longitudinal: true
         trailer:
           th_moving_speed: 1.0
           th_moving_time: 2.0
@@ -61,7 +58,6 @@
             hard_margin_for_parked_vehicle: 0.7
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.5
-          use_conservative_buffer_longitudinal: true
         unknown:
           th_moving_speed: 0.28
           th_moving_time: 1.0
@@ -72,7 +68,6 @@
             hard_margin_for_parked_vehicle: -0.2
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.1
-          use_conservative_buffer_longitudinal: true
         bicycle:
           th_moving_speed: 0.28
           th_moving_time: 1.0
@@ -83,7 +78,6 @@
             hard_margin_for_parked_vehicle: 0.5
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.5
-          use_conservative_buffer_longitudinal: true
         motorcycle:
           th_moving_speed: 1.0
           th_moving_time: 1.0
@@ -94,7 +88,6 @@
             hard_margin_for_parked_vehicle: 0.3
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.5
-          use_conservative_buffer_longitudinal: true
         pedestrian:
           th_moving_speed: 0.28
           th_moving_time: 1.0
@@ -105,7 +98,6 @@
             hard_margin_for_parked_vehicle: 0.5
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.5
-          use_conservative_buffer_longitudinal: true
         lower_distance_for_polygon_expansion: 30.0      # [m] FOR DEVELOPER
         upper_distance_for_polygon_expansion: 100.0     # [m] FOR DEVELOPER
 
@@ -140,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.0                     # [m]
+
         # params for avoidance of vehicle type objects that are ambiguous as to whether they are parked.
         avoidance_for_ambiguous_vehicle:
           enable: true                                  # [-]
@@ -222,6 +218,8 @@
           min_slow_down_speed: 1.38                     # [m/s]
           buf_slow_down_speed: 0.56                     # [m/s] FOR DEVELOPER
           nominal_avoidance_speed: 8.33                 # [m/s] FOR DEVELOPER
+          consider_front_overhang: true                 # [-]
+          consider_rear_overhang: true                  # [-]
         # return dead line
         return_dead_line:
           goal:
@@ -296,4 +294,3 @@
       # for debug
       debug:
         marker: false
-        console: false

planning/behavior_path_avoidance_module/config/avoidance.param.yaml

@@ -76,8 +76,6 @@ struct ObjectParameter
   double lateral_hard_margin_for_parked_vehicle{1.0};
 
   double longitudinal_margin{0.0};
-
-  bool use_conservative_buffer_longitudinal{true};
 };
 
 struct AvoidanceParameters
@@ -191,13 +189,8 @@ struct AvoidanceParameters
   double time_threshold_for_ambiguous_vehicle{0.0};
   double distance_threshold_for_ambiguous_vehicle{0.0};
 
-  // when complete avoidance motion, there is a distance margin with the object
-  // for longitudinal direction
-  double longitudinal_collision_margin_min_distance{0.0};
-
-  // when complete avoidance motion, there is a time margin with the object
-  // for longitudinal direction
-  double longitudinal_collision_margin_time{0.0};
+  // for merging/deviating vehicle
+  double th_overhang_distance{0.0};
 
   // parameters for safety check area
   bool enable_safety_check{false};
@@ -219,6 +212,9 @@ struct AvoidanceParameters
   size_t hysteresis_factor_safe_count;
   double hysteresis_factor_expand_rate{0.0};
 
+  bool consider_front_overhang{true};
+  bool consider_rear_overhang{true};
+
   // maximum stop distance
   double stop_max_distance{0.0};
 
@@ -329,7 +325,6 @@ struct AvoidanceParameters
 
   // debug
   bool publish_debug_marker = false;
-  bool print_debug_info = false;
 };
 
 struct ObjectData  // avoidance target

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp

@@ -148,16 +148,19 @@ class AvoidanceHelper
   {
     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.longitudinal_margin + additional_buffer_longitudinal;
+    if (!parameters_->consider_front_overhang) {
+      return object_parameter.longitudinal_margin;
+    }
+    return object_parameter.longitudinal_margin + data_->parameters.base_link2front;
   }
 
   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);
+    if (!parameters_->consider_rear_overhang) {
+      return object_parameter.longitudinal_margin;
+    }
     return object_parameter.longitudinal_margin + data_->parameters.base_link2rear + object.length;
   }
 

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp

@@ -70,8 +70,6 @@ AvoidanceParameters getParameter(rclcpp::Node * node)
       param.lateral_hard_margin_for_parked_vehicle =
         getOrDeclareParameter<double>(*node, ns + "lateral_margin.hard_margin_for_parked_vehicle");
       param.longitudinal_margin = getOrDeclareParameter<double>(*node, ns + "longitudinal_margin");
-      param.use_conservative_buffer_longitudinal =
-        getOrDeclareParameter<bool>(*node, ns + "use_conservative_buffer_longitudinal");
       return param;
     };
 
@@ -133,6 +131,11 @@ AvoidanceParameters getParameter(rclcpp::Node * node)
       getOrDeclareParameter<double>(*node, ns + "min_road_shoulder_width");
   }
 
+  {
+    const std::string ns = "avoidance.target_filtering.merging_vehicle.";
+    p.th_overhang_distance = getOrDeclareParameter<double>(*node, ns + "th_overhang_distance");
+  }
+
   {
     const std::string ns = "avoidance.target_filtering.avoidance_for_ambiguous_vehicle.";
     p.enable_avoidance_for_ambiguous_vehicle = getOrDeclareParameter<bool>(*node, ns + "enable");
@@ -270,6 +273,8 @@ AvoidanceParameters getParameter(rclcpp::Node * node)
     p.buf_slow_down_speed = getOrDeclareParameter<double>(*node, ns + "buf_slow_down_speed");
     p.nominal_avoidance_speed =
       getOrDeclareParameter<double>(*node, ns + "nominal_avoidance_speed");
+    p.consider_front_overhang = getOrDeclareParameter<bool>(*node, ns + "consider_front_overhang");
+    p.consider_rear_overhang = getOrDeclareParameter<bool>(*node, ns + "consider_rear_overhang");
   }
 
   // avoidance maneuver (return shift dead line)
@@ -379,7 +384,6 @@ AvoidanceParameters getParameter(rclcpp::Node * node)
   {
     const std::string ns = "avoidance.debug.";
     p.publish_debug_marker = getOrDeclareParameter<bool>(*node, ns + "marker");
-    p.print_debug_info = getOrDeclareParameter<bool>(*node, ns + "console");
   }
 
   return p;

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp

@@ -424,9 +424,6 @@ class AvoidanceModule : public SceneModuleInterface
   // TODO(Satoshi OTA) create detected object manager.
   ObjectDataArray registered_objects_;
 
-  // TODO(Satoshi OTA) remove mutable.
-  mutable ObjectDataArray detected_objects_;
-
   // TODO(Satoshi OTA) remove this variable.
   mutable ObjectDataArray ego_stopped_objects_;
 

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp

@@ -31,6 +31,9 @@ using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityAndPoly
 using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped;
 using behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygon;
 
+static constexpr const char * logger_namespace =
+  "planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.avoidance.utils";
+
 bool isOnRight(const ObjectData & obj);
 
 double calcShiftLength(
@@ -112,8 +115,6 @@ void fillObjectStoppableJudge(
   ObjectData & object_data, const ObjectDataArray & registered_objects,
   const double feasible_stop_distance, const std::shared_ptr<AvoidanceParameters> & parameters);
 
-void fillInitialPose(ObjectData & object_data, ObjectDataArray & detected_objects);
-
 void updateRegisteredObject(
   ObjectDataArray & registered_objects, const ObjectDataArray & now_objects,
   const std::shared_ptr<AvoidanceParameters> & parameters);