feat(avoidance): can set stop/move judge threshold for each object cl…

…ass (autowarefoundation#3998)

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

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

@@ -27,49 +27,65 @@
       # avoidance is performed for the object type with true
       target_object:
         car:
-          enable: true
-          max_expand_ratio: 0.0
-          envelope_buffer_margin: 0.3
-          safety_buffer_lateral: 0.7
-          safety_buffer_longitudinal: 0.0
+          enable: true                                 # [-]
+          moving_speed_threshold: 1.0                  # [m/s]
+          moving_time_threshold: 1.0                   # [s]
+          max_expand_ratio: 0.0                        # [-]
+          envelope_buffer_margin: 0.3                  # [m]
+          safety_buffer_lateral: 0.7                   # [m]
+          safety_buffer_longitudinal: 0.0              # [m]
         truck:
           enable: true
+          moving_speed_threshold: 1.0
+          moving_time_threshold: 1.0
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.3
           safety_buffer_lateral: 0.7
           safety_buffer_longitudinal: 0.0
         bus:
           enable: true
+          moving_speed_threshold: 1.0
+          moving_time_threshold: 1.0
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.3
           safety_buffer_lateral: 0.7
           safety_buffer_longitudinal: 0.0
         trailer:
           enable: true
+          moving_speed_threshold: 1.0
+          moving_time_threshold: 1.0
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.3
           safety_buffer_lateral: 0.7
           safety_buffer_longitudinal: 0.0
         unknown:
           enable: false
+          moving_speed_threshold: 1.0
+          moving_time_threshold: 1.0
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.3
           safety_buffer_lateral: 0.7
           safety_buffer_longitudinal: 0.0
         bicycle:
           enable: false
+          moving_speed_threshold: 1.0
+          moving_time_threshold: 1.0
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.8
           safety_buffer_lateral: 1.0
           safety_buffer_longitudinal: 1.0
         motorcycle:
           enable: false
+          moving_speed_threshold: 1.0
+          moving_time_threshold: 1.0
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.8
           safety_buffer_lateral: 1.0
           safety_buffer_longitudinal: 1.0
         pedestrian:
           enable: false
+          moving_speed_threshold: 1.0
+          moving_time_threshold: 1.0
           max_expand_ratio: 0.0
           envelope_buffer_margin: 0.8
           safety_buffer_lateral: 1.0
@@ -80,8 +96,6 @@
       # For target object filtering
       target_filtering:
         # filtering moving objects
-        threshold_speed_object_is_stopped: 1.0          # [m/s]
-        threshold_time_object_is_moving: 1.0            # [s]
         threshold_time_force_avoidance_for_stopped_vehicle: 10.0 # [s]
         # detection range
         object_check_force_avoidance_clearance: 30.0    # [m]

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

@@ -50,6 +50,10 @@ struct ObjectParameter
 {
   bool enable{false};
 
+  double moving_speed_threshold{0.0};
+
+  double moving_time_threshold{1.0};
+
   double max_expand_ratio{0.0};
 
   double envelope_buffer_margin{0.0};
@@ -146,9 +150,6 @@ struct AvoidanceParameters
   // minimum road shoulder width. maybe 0.5 [m]
   double object_check_min_road_shoulder_width;
 
-  // vehicles which is moving more than this parameter will not be avoided
-  double threshold_time_object_is_moving;
-
   // force avoidance
   double threshold_time_force_avoidance_for_stopped_vehicle;
 

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -528,6 +528,8 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam()
     const auto get_object_param = [&](std::string && ns) {
       ObjectParameter param{};
       param.enable = declare_parameter<bool>(ns + "enable");
+      param.moving_speed_threshold = declare_parameter<double>(ns + "moving_speed_threshold");
+      param.moving_time_threshold = declare_parameter<double>(ns + "moving_time_threshold");
       param.max_expand_ratio = declare_parameter<double>(ns + "max_expand_ratio");
       param.envelope_buffer_margin = declare_parameter<double>(ns + "envelope_buffer_margin");
       param.safety_buffer_lateral = declare_parameter<double>(ns + "safety_buffer_lateral");
@@ -557,10 +559,6 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam()
   // target filtering
   {
     std::string ns = "avoidance.target_filtering.";
-    p.threshold_speed_object_is_stopped =
-      declare_parameter<double>(ns + "threshold_speed_object_is_stopped");
-    p.threshold_time_object_is_moving =
-      declare_parameter<double>(ns + "threshold_time_object_is_moving");
     p.threshold_time_force_avoidance_for_stopped_vehicle =
       declare_parameter<double>(ns + "threshold_time_force_avoidance_for_stopped_vehicle");
     p.object_check_force_avoidance_clearance =

planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp

@@ -48,6 +48,8 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector<rclcpp::Parame
                                      const auto & semantic, const std::string & ns) {
     auto & config = p->object_parameters.at(semantic);
     updateParam<bool>(parameters, ns + "enable", config.enable);
+    updateParam<double>(parameters, ns + "moving_speed_threshold", config.moving_speed_threshold);
+    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 + "safety_buffer_lateral", config.safety_buffer_lateral);

planning/behavior_path_planner/src/utils/avoidance/utils.cpp

@@ -845,9 +845,12 @@ void fillObjectMovingTime(
   ObjectData & object_data, ObjectDataArray & stopped_objects,
   const std::shared_ptr<AvoidanceParameters> & parameters)
 {
+  const auto t = utils::getHighestProbLabel(object_data.object.classification);
+  const auto object_parameter = parameters->object_parameters.at(t);
+
   const auto & object_vel =
     object_data.object.kinematics.initial_twist_with_covariance.twist.linear.x;
-  const auto is_faster_than_threshold = object_vel > parameters->threshold_speed_object_is_stopped;
+  const auto is_faster_than_threshold = object_vel > object_parameter.moving_speed_threshold;
 
   const auto id = object_data.object.object_id;
   const auto same_id_obj = std::find_if(
@@ -884,7 +887,7 @@ void fillObjectMovingTime(
   object_data.move_time = (now - same_id_obj->last_stop).seconds();
   object_data.stop_time = 0.0;
 
-  if (object_data.move_time > parameters->threshold_time_object_is_moving) {
+  if (object_data.move_time > object_parameter.moving_time_threshold) {
     stopped_objects.erase(same_id_obj);
   }
 }
@@ -1079,7 +1082,10 @@ void filterTargetObjects(
       continue;
     }
 
-    if (o.move_time > parameters->threshold_time_object_is_moving) {
+    // if following condition are satisfied, ignored the objects as moving objects.
+    // 1. speed is higher than threshold.
+    // 2. keep that speed longer than the time threshold.
+    if (o.move_time > object_parameter.moving_time_threshold) {
       o.reason = AvoidanceDebugFactor::MOVING_OBJECT;
       data.other_objects.push_back(o);
       continue;