feat: add height filter param

Signed-off-by: yoshiri <[email protected]>
autowarefoundation · Jan 22, 2025 · f83aa01 · f83aa01
1 parent 75549a6
commit f83aa01
Show file tree

Hide file tree

Showing 4 changed files with 57 additions and 1 deletion.
diff --git a/perception/autoware_detected_object_validation/config/object_lanelet_filter.param.yaml b/perception/autoware_detected_object_validation/config/object_lanelet_filter.param.yaml
@@ -21,3 +21,6 @@
         object_speed_threshold: 3.0 # [m/s]
       debug: false
       lanelet_extra_margin: 0.0
+      use_height_threshold: false
+      max_height_threshold: 10.0 # [m]
+      min_height_threshold: -1.0 # [m]
diff --git a/perception/autoware_detected_object_validation/schema/object_lanelet_filter.schema.json b/perception/autoware_detected_object_validation/schema/object_lanelet_filter.schema.json
@@ -95,9 +95,42 @@
                 }
               },
               "required": ["enabled", "velocity_yaw_threshold", "object_speed_threshold"]
+            },
+            "debug": {
+              "type": "boolean",
+              "default": false,
+              "description": "If true, debug information is enabled."
+            },
+            "lanelet_extra_margin": {
+              "type": "number",
+              "default": 0.0,
+              "description": "Extra margin added to the lanelet boundaries."
+            },
+            "use_height_threshold": {
+              "type": "boolean",
+              "default": false,
+              "description": "If true, height thresholds are used to filter objects."
+            },
+            "max_height_threshold": {
+              "type": "number",
+              "default": 10.0,
+              "description": "Maximum height threshold for filtering objects (in meters)."
+            },
+            "min_height_threshold": {
+              "type": "number",
+              "default": -1.0,
+              "description": "Minimum height threshold for filtering objects (in meters)."
             }
           },
-          "required": ["polygon_overlap_filter", "lanelet_direction_filter"]
+          "required": [
+            "polygon_overlap_filter",
+            "lanelet_direction_filter",
+            "debug",
+            "lanelet_extra_margin",
+            "use_height_threshold",
+            "max_height_threshold",
+            "min_height_threshold"
+          ]
         }
       },
       "required": ["filter_target_label", "filter_settings"],

diff --git a/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp b/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.cpp
@@ -66,6 +66,12 @@ ObjectLaneletFilterNode::ObjectLaneletFilterNode(const rclcpp::NodeOptions & nod
   filter_settings_.debug = declare_parameter<bool>("filter_settings.debug");
   filter_settings_.lanelet_extra_margin =
     declare_parameter<double>("filter_settings.lanelet_extra_margin");
+  filter_settings_.use_height_threshold =
+    declare_parameter<bool>("filter_settings.use_height_threshold");
+  filter_settings_.max_height_threshold =
+    declare_parameter<double>("filter_settings.max_height_threshold");
+  filter_settings_.min_height_threshold =
+    declare_parameter<double>("filter_settings.min_height_threshold");
 
   // Set publisher/subscriber
   map_sub_ = this->create_subscription<autoware_map_msgs::msg::LaneletMapBin>(
@@ -199,6 +205,16 @@ bool ObjectLaneletFilterNode::filterObject(
       return false;
     }
 
+    // 0. check height threshold
+    if (filter_settings_.use_height_threshold) {
+      const double object_height = transformed_object.shape.dimensions.z;
+      if (
+        object_height > filter_settings_.max_height_threshold ||
+        object_height < filter_settings_.min_height_threshold) {
+        return false;
+      }
+    }
+
     bool filter_pass = true;
     // 1. is polygon overlap with road lanelets or shoulder lanelets
     if (filter_settings_.polygon_overlap_filter) {

diff --git a/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.hpp b/perception/autoware_detected_object_validation/src/lanelet_filter/lanelet_filter.hpp
@@ -35,6 +35,7 @@
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
 
+#include <limits>
 #include <memory>
 #include <string>
 #include <utility>
@@ -96,6 +97,9 @@ class ObjectLaneletFilterNode : public rclcpp::Node
     double lanelet_direction_filter_object_speed_threshold;
     bool debug;
     double lanelet_extra_margin;
+    bool use_height_threshold;
+    double max_height_threshold = std::numeric_limits<double>::infinity();
+    double min_height_threshold = -std::numeric_limits<double>::infinity();
   } filter_settings_;
 
   bool filterObject(