feat(avoidance): change lateral margin based on if it's parked vehicle (autowarefoundation#6520)

satoshi-ota · TomohitoAndo · commit a6312e343074 · 2024-04-01T14:06:41.000+09:00
* feat(avoidance): check if it's parked vehicle

Signed-off-by: satoshi-ota &lt;satoshi.ota928@gmail.com&gt;

* feat(avoidance): change lateral margin based on if it's parked vehicle

Signed-off-by: satoshi-ota &lt;satoshi.ota928@gmail.com&gt;

---------

Signed-off-by: satoshi-ota &lt;satoshi.ota928@gmail.com&gt;
diff --git a/planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lc.param.yaml b/planning/behavior_path_avoidance_by_lane_change_module/config/avoidance_by_lc.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]
 
diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/manager.cpp b/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;
     };
 
diff --git a/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/behavior_path_avoidance_by_lane_change_module/src/scene.cpp
@@ -86,9 +86,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(
diff --git a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml b/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]
diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp b/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};
 
@@ -415,6 +417,9 @@ struct ObjectData  // avoidance target
   // is within intersection area
   bool is_within_intersection{false};
 
+  // is parked vehicle on road shoulder
+  bool is_parked{false};
+
   // object direction.
   Direction direction{Direction::NONE};
 
diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp b/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 =
diff --git a/planning/behavior_path_avoidance_module/src/debug.cpp b/planning/behavior_path_avoidance_module/src/debug.cpp
@@ -176,7 +176,7 @@ MarkerArray createObjectInfoMarkerArray(const ObjectDataArray & objects, std::st
       marker.id = uuidToInt32(object.object.object_id);
       marker.pose.position.z += 2.0;
       std::ostringstream string_stream;
-      string_stream << object.reason;
+      string_stream << object.reason << (object.is_parked ? "(PARKED)" : "");
       marker.text = string_stream.str();
       marker.color = createMarkerColor(1.0, 1.0, 1.0, 0.999);
       marker.scale = createMarkerScale(0.6, 0.6, 0.6);
diff --git a/planning/behavior_path_avoidance_module/src/manager.cpp b/planning/behavior_path_avoidance_module/src/manager.cpp
@@ -60,8 +60,11 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector<rclcpp::Parame
     updateParam<double>(parameters, ns + "moving_time_threshold", config.moving_time_threshold);
     updateParam<double>(parameters, ns + "max_expand_ratio", config.max_expand_ratio);
     updateParam<double>(parameters, ns + "envelope_buffer_margin", config.envelope_buffer_margin);
-    updateParam<double>(parameters, ns + "avoid_margin_lateral", config.avoid_margin_lateral);
-    updateParam<double>(parameters, ns + "safety_buffer_lateral", config.safety_buffer_lateral);
+    updateParam<double>(parameters, ns + "lateral_margin.soft_margin", config.lateral_soft_margin);
+    updateParam<double>(parameters, ns + "lateral_margin.hard_margin", config.lateral_hard_margin);
+    updateParam<double>(
+      parameters, ns + "lateral_margin.hard_margin_for_parked_vehicle",
+      config.lateral_hard_margin_for_parked_vehicle);
     updateParam<double>(
       parameters, ns + "safety_buffer_longitudinal", config.safety_buffer_longitudinal);
     updateParam<bool>(
diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp
@@ -635,8 +635,8 @@ void AvoidanceModule::fillDebugData(
       : 0.0;
   const auto & vehicle_width = planner_data_->parameters.vehicle_width;
 
-  const auto max_avoid_margin = object_parameter.safety_buffer_lateral * o_front.distance_factor +
-                                object_parameter.avoid_margin_lateral + 0.5 * vehicle_width;
+  const auto max_avoid_margin = object_parameter.lateral_hard_margin * o_front.distance_factor +
+                                object_parameter.lateral_soft_margin + 0.5 * vehicle_width;
 
   const auto variable = helper_->getSharpAvoidanceDistance(
     helper_->getShiftLength(o_front, utils::avoidance::isOnRight(o_front), max_avoid_margin));
@@ -1408,8 +1408,8 @@ double AvoidanceModule::calcDistanceToStopLine(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 avoid_margin = object_parameter.safety_buffer_lateral * object.distance_factor +
-                            object_parameter.avoid_margin_lateral + 0.5 * vehicle_width;
+  const auto avoid_margin = object_parameter.lateral_hard_margin * object.distance_factor +
+                            object_parameter.lateral_soft_margin + 0.5 * vehicle_width;
   const auto variable = helper_->getMinAvoidanceDistance(
     helper_->getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin));
   const auto & additional_buffer_longitudinal =
@@ -1638,8 +1638,8 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const
   const auto & vehicle_width = planner_data_->parameters.vehicle_width;
   const auto object_type = utils::getHighestProbLabel(object.object.classification);
   const auto object_parameter = parameters_->object_parameters.at(object_type);
-  const auto avoid_margin = object_parameter.safety_buffer_lateral * object.distance_factor +
-                            object_parameter.avoid_margin_lateral + 0.5 * vehicle_width;
+  const auto avoid_margin = object_parameter.lateral_hard_margin * object.distance_factor +
+                            object_parameter.lateral_soft_margin + 0.5 * vehicle_width;
   const auto shift_length =
     helper_->getShiftLength(object, utils::avoidance::isOnRight(object), avoid_margin);
 
diff --git a/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_avoidance_module/src/shift_line_generator.cpp
@@ -234,9 +234,12 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
 
     const double LAT_DIST_BUFFER = desire_shift_length > 0.0 ? 1e-3 : -1e-3;
 
+    const auto lateral_hard_margin = object.is_parked
+                                       ? object_parameter.lateral_hard_margin_for_parked_vehicle
+                                       : object_parameter.lateral_hard_margin;
     const auto infeasible =
       std::abs(feasible_shift_length - object.overhang_points.front().first) - LAT_DIST_BUFFER <
-      0.5 * data_->parameters.vehicle_width + object_parameter.safety_buffer_lateral;
+      0.5 * data_->parameters.vehicle_width + lateral_hard_margin;
     if (infeasible) {
       RCLCPP_DEBUG(rclcpp::get_logger(""), "feasible shift length is not enough to avoid. ");
       object.reason = AvoidanceDebugFactor::TOO_LARGE_JERK;
diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp
