feat(avoidance): configurable object type for safety check (#5699)

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

@@ -30,7 +30,6 @@
       # avoidance is performed for the object type with true
       target_object:
         car:
-          is_target: true                              # [-]
           execute_num: 1                               # [-]
           moving_speed_threshold: 1.0                  # [m/s]
           moving_time_threshold: 1.0                   # [s]
@@ -40,7 +39,6 @@
           safety_buffer_lateral: 0.7                   # [m]
           safety_buffer_longitudinal: 0.0              # [m]
         truck:
-          is_target: true
           execute_num: 1
           moving_speed_threshold: 1.0
           moving_time_threshold: 1.0
@@ -50,7 +48,6 @@
           safety_buffer_lateral: 0.7
           safety_buffer_longitudinal: 0.0
         bus:
-          is_target: true
           execute_num: 1
           moving_speed_threshold: 1.0
           moving_time_threshold: 1.0
@@ -60,7 +57,6 @@
           safety_buffer_lateral: 0.7
           safety_buffer_longitudinal: 0.0
         trailer:
-          is_target: true
           execute_num: 1
           moving_speed_threshold: 1.0
           moving_time_threshold: 1.0
@@ -70,7 +66,6 @@
           safety_buffer_lateral: 0.7
           safety_buffer_longitudinal: 0.0
         unknown:
-          is_target: false
           execute_num: 1
           moving_speed_threshold: 1.0
           moving_time_threshold: 1.0
@@ -80,7 +75,6 @@
           safety_buffer_lateral: 0.7
           safety_buffer_longitudinal: 0.0
         bicycle:
-          is_target: false
           execute_num: 1
           moving_speed_threshold: 1.0
           moving_time_threshold: 1.0
@@ -90,7 +84,6 @@
           safety_buffer_lateral: 1.0
           safety_buffer_longitudinal: 1.0
         motorcycle:
-          is_target: false
           execute_num: 1
           moving_speed_threshold: 1.0
           moving_time_threshold: 1.0
@@ -100,7 +93,6 @@
           safety_buffer_lateral: 1.0
           safety_buffer_longitudinal: 1.0
         pedestrian:
-          is_target: false
           execute_num: 1
           moving_speed_threshold: 1.0
           moving_time_threshold: 1.0
@@ -114,6 +106,16 @@
 
       # For target object filtering
       target_filtering:
+        # avoidance target type
+        target_type:
+          car: true                                      # [-]
+          truck: true                                    # [-]
+          bus: true                                      # [-]
+          trailer: true                                  # [-]
+          unknown: true                                  # [-]
+          bicycle: true                                  # [-]
+          motorcycle: true                               # [-]
+          pedestrian: true                               # [-]
         # params for avoidance of not-parked objects
         threshold_time_force_avoidance_for_stopped_vehicle: 10.0    # [s]
         object_ignore_section_traffic_light_in_front_distance: 30.0 # [m]
@@ -132,6 +134,16 @@
 
       # For safety check
       safety_check:
+        # safety check target type
+        target_type:
+          car: true                                      # [-]
+          truck: true                                    # [-]
+          bus: true                                      # [-]
+          trailer: true                                  # [-]
+          unknown: false                                 # [-]
+          bicycle: true                                  # [-]
+          motorcycle: true                               # [-]
+          pedestrian: true                               # [-]
         # safety check configuration
         enable: true                                     # [-]
         check_current_lane: false                        # [-]

planning/behavior_path_planner/config/avoidance_by_lc/avoidance_by_lc.param.yaml

@@ -7,7 +7,6 @@
       # avoidance is performed for the object type with true
       target_object:
         car:
-          is_target: true                              # [-]
           execute_num: 2                               # [-]
           moving_speed_threshold: 1.0                  # [m/s]
           moving_time_threshold: 1.0                   # [s]
@@ -16,7 +15,6 @@
           avoid_margin_lateral: 1.0                    # [m]
           safety_buffer_lateral: 0.7                   # [m]
         truck:
-          is_target: true
           execute_num: 2
           moving_speed_threshold: 1.0                  # 3.6km/h
           moving_time_threshold: 1.0
@@ -25,7 +23,6 @@
           avoid_margin_lateral: 1.0
           safety_buffer_lateral: 0.7
         bus:
-          is_target: true
           execute_num: 2
           moving_speed_threshold: 1.0                  # 3.6km/h
           moving_time_threshold: 1.0
@@ -34,7 +31,6 @@
           avoid_margin_lateral: 1.0
           safety_buffer_lateral: 0.7
         trailer:
-          is_target: true
           execute_num: 2
           moving_speed_threshold: 1.0                  # 3.6km/h
           moving_time_threshold: 1.0
@@ -43,7 +39,6 @@
           avoid_margin_lateral: 1.0
           safety_buffer_lateral: 0.7
         unknown:
-          is_target: true
           execute_num: 1
           moving_speed_threshold: 0.28                 # 1.0km/h
           moving_time_threshold: 1.0
@@ -52,7 +47,6 @@
           avoid_margin_lateral: 1.0
           safety_buffer_lateral: 0.0
         bicycle:
-          is_target: true
           execute_num: 2
           moving_speed_threshold: 0.28                 # 1.0km/h
           moving_time_threshold: 1.0
@@ -61,7 +55,6 @@
           avoid_margin_lateral: 1.0
           safety_buffer_lateral: 1.0
         motorcycle:
-          is_target: true
           execute_num: 2
           moving_speed_threshold: 1.0                  # 3.6km/h
           moving_time_threshold: 1.0
@@ -70,7 +63,6 @@
           avoid_margin_lateral: 1.0
           safety_buffer_lateral: 1.0
         pedestrian:
-          is_target: true
           execute_num: 2
           moving_speed_threshold: 0.28                 # 1.0km/h
           moving_time_threshold: 1.0
@@ -80,3 +72,16 @@
           safety_buffer_lateral: 1.0
         lower_distance_for_polygon_expansion: 0.0      # [m]
         upper_distance_for_polygon_expansion: 1.0      # [m]
+
+      # For target object filtering
+      target_filtering:
+        # avoidance target type
+        target_type:
+          car: true                                    # [-]
+          truck: true                                  # [-]
+          bus: true                                    # [-]
+          trailer: true                                # [-]
+          unknown: true                                # [-]
+          bicycle: false                               # [-]
+          motorcycle: false                            # [-]
+          pedestrian: false                            # [-]

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

@@ -52,7 +52,9 @@ using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebug;
 
 struct ObjectParameter
 {
-  bool is_target{false};
+  bool is_avoidance_target{false};
+
+  bool is_safety_check_target{false};
 
   size_t execute_num{1};
 

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

@@ -80,7 +80,6 @@ AvoidanceModuleManager::AvoidanceModuleManager(
   {
     const auto get_object_param = [&](std::string && ns) {
       ObjectParameter param{};
-      param.is_target = get_parameter<bool>(node, ns + "is_target");
       param.execute_num = get_parameter<int>(node, ns + "execute_num");
       param.moving_speed_threshold = get_parameter<double>(node, ns + "moving_speed_threshold");
       param.moving_time_threshold = get_parameter<double>(node, ns + "moving_time_threshold");
@@ -113,7 +112,23 @@ AvoidanceModuleManager::AvoidanceModuleManager(
 
   // target filtering
   {
+    const auto set_target_flag = [&](const uint8_t & object_type, const std::string & ns) {
+      if (p.object_parameters.count(object_type) == 0) {
+        return;
+      }
+      p.object_parameters.at(object_type).is_avoidance_target = get_parameter<bool>(node, ns);
+    };
+
     std::string ns = "avoidance.target_filtering.";
+    set_target_flag(ObjectClassification::CAR, ns + "target_type.car");
+    set_target_flag(ObjectClassification::TRUCK, ns + "target_type.truck");
+    set_target_flag(ObjectClassification::TRAILER, ns + "target_type.trailer");
+    set_target_flag(ObjectClassification::BUS, ns + "target_type.bus");
+    set_target_flag(ObjectClassification::PEDESTRIAN, ns + "target_type.pedestrian");
+    set_target_flag(ObjectClassification::BICYCLE, ns + "target_type.bicycle");
+    set_target_flag(ObjectClassification::MOTORCYCLE, ns + "target_type.motorcycle");
+    set_target_flag(ObjectClassification::UNKNOWN, ns + "target_type.unknown");
+
     p.threshold_time_force_avoidance_for_stopped_vehicle =
       get_parameter<double>(node, ns + "threshold_time_force_avoidance_for_stopped_vehicle");
     p.object_ignore_section_traffic_light_in_front_distance =
@@ -138,7 +153,23 @@ AvoidanceModuleManager::AvoidanceModuleManager(
 
   // safety check general params
   {
+    const auto set_target_flag = [&](const uint8_t & object_type, const std::string & ns) {
+      if (p.object_parameters.count(object_type) == 0) {
+        return;
+      }
+      p.object_parameters.at(object_type).is_safety_check_target = get_parameter<bool>(node, ns);
+    };
+
     std::string ns = "avoidance.safety_check.";
+    set_target_flag(ObjectClassification::CAR, ns + "target_type.car");
+    set_target_flag(ObjectClassification::TRUCK, ns + "target_type.truck");
+    set_target_flag(ObjectClassification::TRAILER, ns + "target_type.trailer");
+    set_target_flag(ObjectClassification::BUS, ns + "target_type.bus");
+    set_target_flag(ObjectClassification::PEDESTRIAN, ns + "target_type.pedestrian");
+    set_target_flag(ObjectClassification::BICYCLE, ns + "target_type.bicycle");
+    set_target_flag(ObjectClassification::MOTORCYCLE, ns + "target_type.motorcycle");
+    set_target_flag(ObjectClassification::UNKNOWN, ns + "target_type.unknown");
+
     p.enable_safety_check = get_parameter<bool>(node, ns + "enable");
     p.check_current_lane = get_parameter<bool>(node, ns + "check_current_lane");
     p.check_shift_side_lane = get_parameter<bool>(node, ns + "check_shift_side_lane");
@@ -303,7 +334,6 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector<rclcpp::Parame
   const auto update_object_param = [&p, &parameters](
                                      const auto & semantic, const std::string & ns) {
     auto & config = p->object_parameters.at(semantic);
-    updateParam<bool>(parameters, ns + "is_target", config.is_target);
     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);

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

@@ -237,7 +237,6 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager(
   {
     const auto get_object_param = [&](std::string && ns) {
       ObjectParameter param{};
-      param.is_target = get_parameter<bool>(node, ns + "is_target");
       param.execute_num = get_parameter<int>(node, ns + "execute_num");
       param.moving_speed_threshold = get_parameter<double>(node, ns + "moving_speed_threshold");
       param.moving_time_threshold = get_parameter<double>(node, ns + "moving_time_threshold");
@@ -268,7 +267,23 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager(
 
   // target filtering
   {
+    const auto set_target_flag = [&](const uint8_t & object_type, const std::string & ns) {
+      if (p.object_parameters.count(object_type) == 0) {
+        return;
+      }
+      p.object_parameters.at(object_type).is_avoidance_target = get_parameter<bool>(node, ns);
+    };
+
     std::string ns = "avoidance.target_filtering.";
+    set_target_flag(ObjectClassification::CAR, ns + "target_type.car");
+    set_target_flag(ObjectClassification::TRUCK, ns + "target_type.truck");
+    set_target_flag(ObjectClassification::TRAILER, ns + "target_type.trailer");
+    set_target_flag(ObjectClassification::BUS, ns + "target_type.bus");
+    set_target_flag(ObjectClassification::PEDESTRIAN, ns + "target_type.pedestrian");
+    set_target_flag(ObjectClassification::BICYCLE, ns + "target_type.bicycle");
+    set_target_flag(ObjectClassification::MOTORCYCLE, ns + "target_type.motorcycle");
+    set_target_flag(ObjectClassification::UNKNOWN, ns + "target_type.unknown");
+
     p.threshold_time_force_avoidance_for_stopped_vehicle =
       get_parameter<double>(node, ns + "threshold_time_force_avoidance_for_stopped_vehicle");
     p.object_ignore_section_traffic_light_in_front_distance =

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

@@ -76,7 +76,7 @@ bool isOnRight(const ObjectData & obj)
   return obj.lateral < 0.0;
 }
 
-bool isTargetObjectType(
+bool isAvoidanceTargetObjectType(
   const PredictedObject & object, const std::shared_ptr<AvoidanceParameters> & parameters)
 {
   const auto object_type = utils::getHighestProbLabel(object.classification);
@@ -85,7 +85,19 @@ bool isTargetObjectType(
     return false;
   }
 
-  return parameters->object_parameters.at(object_type).is_target;
+  return parameters->object_parameters.at(object_type).is_avoidance_target;
+}
+
+bool isSafetyCheckTargetObjectType(
+  const PredictedObject & object, const std::shared_ptr<AvoidanceParameters> & parameters)
+{
+  const auto object_type = utils::getHighestProbLabel(object.classification);
+
+  if (parameters->object_parameters.count(object_type) == 0) {
+    return false;
+  }
+
+  return parameters->object_parameters.at(object_type).is_safety_check_target;
 }
 
 bool isVehicleTypeObject(const ObjectData & object)
@@ -854,7 +866,7 @@ void filterTargetObjects(
     const auto object_type = utils::getHighestProbLabel(o.object.classification);
     const auto object_parameter = parameters->object_parameters.at(object_type);
 
-    if (!isTargetObjectType(o.object, parameters)) {
+    if (!isAvoidanceTargetObjectType(o.object, parameters)) {
       o.reason = AvoidanceDebugFactor::OBJECT_IS_NOT_TYPE;
       data.other_objects.push_back(o);
       continue;
@@ -1458,8 +1470,10 @@ std::vector<ExtendedPredictedObject> getSafetyCheckTargetObjects(
 
   const auto append_target_objects = [&](const auto & check_lanes, const auto & objects) {
     std::for_each(objects.begin(), objects.end(), [&](const auto & object) {
-      if (isCentroidWithinLanelets(object.object, check_lanes)) {
-        target_objects.push_back(utils::avoidance::transform(object.object, p));
+      if (isSafetyCheckTargetObjectType(object.object, parameters)) {
+        if (isCentroidWithinLanelets(object.object, check_lanes)) {
+          target_objects.push_back(utils::avoidance::transform(object.object, p));
+        }
       }
     });
   };