Change params to set the ignore vel threshold for each label

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

Author: maxime-clem

planning/behavior_velocity_crosswalk_module/config/crosswalk.param.yaml

@@ -81,6 +81,8 @@
         occupied_min: 58    # [-] minimum value of an occupied cell in the occupancy grid
         ignore_with_traffic_light: true  # [-] if true, occlusions at crosswalks with traffic lights are ignored
         ignore_behind_predicted_objects: true  # [-] if true, occlusions behind predicted objects are ignored
-        do_not_ignore_behind_pedestrians: true  # [-] if true, occlusions behind pedestrians are not ignored
-        min_objects_velocity: 0.5  # [m/s] minimum velocity for a predicted object to be ignored
+        ignore_velocity_thresholds:
+          default: 0.5  # [m/s] occlusions are only ignored behind objects with a higher or equal velocity
+          custom_labels: ["PEDESTRIAN"]  # labels for which to define a non-default velocity threshold (see autoware_auto_perception_msgs::msg::ObjectClassification for all the labels)
+          custom_thresholds: [0.0]  # velocities of the custom labels
         extra_objects_size: 0.5  # [m] extra size added to the objects when masking the occlusions

planning/behavior_velocity_crosswalk_module/src/manager.cpp

@@ -17,6 +17,7 @@
 #include <behavior_velocity_planner_common/utilization/util.hpp>
 #include <tier4_autoware_utils/ros/parameter.hpp>
 
+#include <algorithm>
 #include <limits>
 #include <memory>
 #include <set>
@@ -140,10 +141,29 @@ CrosswalkModuleManager::CrosswalkModuleManager(rclcpp::Node & node)
     getOrDeclareParameter<bool>(node, ns + ".occlusion.ignore_with_traffic_light");
   cp.occlusion_ignore_behind_predicted_objects =
     getOrDeclareParameter<bool>(node, ns + ".occlusion.ignore_behind_predicted_objects");
-  cp.occlusion_do_not_ignore_behind_pedestrians =
-    getOrDeclareParameter<bool>(node, ns + ".occlusion.do_not_ignore_behind_pedestrians");
-  cp.occlusion_ignore_velocity_threshold =
-    getOrDeclareParameter<double>(node, ns + ".occlusion.ignore_velocity_threshold");
+
+  cp.occlusion_ignore_velocity_thresholds.resize(
+    autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN + 1,
+    getOrDeclareParameter<double>(node, ns + ".occlusion.ignore_velocity_thresholds.default"));
+  const auto get_label = [](const std::string & s) {
+    if (s == "CAR") return autoware_auto_perception_msgs::msg::ObjectClassification::CAR;
+    if (s == "TRUCK") return autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK;
+    if (s == "BUS") return autoware_auto_perception_msgs::msg::ObjectClassification::BUS;
+    if (s == "TRAILER") return autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER;
+    if (s == "MOTORCYCLE")
+      return autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE;
+    if (s == "BICYCLE") return autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE;
+    if (s == "PEDESTRIAN")
+      return autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN;
+    return autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN;
+  };
+  const auto custom_labels = getOrDeclareParameter<std::vector<std::string>>(
+    node, ns + ".occlusion.ignore_velocity_thresholds.custom_labels");
+  const auto custom_velocities = getOrDeclareParameter<std::vector<double>>(
+    node, ns + ".occlusion.ignore_velocity_thresholds.custom_thresholds");
+  for (auto idx = 0UL; idx < std::min(custom_labels.size(), custom_velocities.size()); ++idx) {
+    cp.occlusion_ignore_velocity_thresholds[get_label(custom_labels[idx])] = custom_velocities[idx];
+  }
   cp.occlusion_extra_objects_size =
     getOrDeclareParameter<double>(node, ns + ".occlusion.extra_predicted_objects_size");
 }

planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp

@@ -96,16 +96,12 @@ std::vector<lanelet::BasicPolygon2d> calculate_detection_areas(
 
 std::vector<autoware_auto_perception_msgs::msg::PredictedObject> select_and_inflate_objects(
   const std::vector<autoware_auto_perception_msgs::msg::PredictedObject> & objects,
-  const double velocity_threshold, const bool skip_pedestrians, const double inflate_size)
+  const std::vector<double> velocity_thresholds, const double inflate_size)
 {
   std::vector<autoware_auto_perception_msgs::msg::PredictedObject> selected_objects;
   for (const auto & o : objects) {
-    const auto skip =
-      (skip_pedestrians &&
-       o.classification.front().label ==
-         autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN) ||
-      o.kinematics.initial_twist_with_covariance.twist.linear.x >= velocity_threshold;
-    if (!skip) {
+    const auto vel_threshold = velocity_thresholds[o.classification.front().label];
+    if (o.kinematics.initial_twist_with_covariance.twist.linear.x >= vel_threshold) {
       auto selected_object = o;
       selected_object.shape.dimensions.x += inflate_size;
       selected_object.shape.dimensions.y += inflate_size;
@@ -161,8 +157,8 @@ bool is_crosswalk_occluded(
 
   if (params.occlusion_ignore_behind_predicted_objects) {
     const auto objects = select_and_inflate_objects(
-      dynamic_objects, params.occlusion_ignore_velocity_threshold,
-      params.occlusion_do_not_ignore_behind_pedestrians, params.occlusion_extra_objects_size);
+      dynamic_objects, params.occlusion_ignore_velocity_thresholds,
+      params.occlusion_extra_objects_size);
     clear_occlusions_behind_objects(grid_map, objects);
   }
   const auto min_nb_of_cells = std::ceil(params.occlusion_min_size / grid_map.getResolution());

planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.hpp

@@ -165,8 +165,7 @@ class CrosswalkModule : public SceneModuleInterface
     int occlusion_occupied_min;
     bool occlusion_ignore_with_traffic_light;
     bool occlusion_ignore_behind_predicted_objects;
-    bool occlusion_do_not_ignore_behind_pedestrians;
-    double occlusion_ignore_velocity_threshold;
+    std::vector<double> occlusion_ignore_velocity_thresholds;
     double occlusion_extra_objects_size;
   };