feat(autoware_detected_object_validation): add height filter in lanel…

…et filtering (autowarefoundation#10003) * feat: add height filter param Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * feat: use ego base height Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * fix: build error Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * feat: add lanelet filter test Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * feat: add height filter test Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * docs: update README and lanelet filter Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> * fix: do not getTransform when flag is off Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com> --------- Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>
tier4 · Jan 22, 2025 · 30ab90e · 30ab90e
1 parent 8ee41be
commit 30ab90e
Show file tree

Hide file tree

Showing 8 changed files with 461 additions and 5 deletions.
diff --git a/perception/autoware_detected_object_validation/CMakeLists.txt b/perception/autoware_detected_object_validation/CMakeLists.txt
@@ -83,6 +83,10 @@ if(BUILD_TESTING)
     test/test_utils.cpp
     test/object_position_filter/test_object_position_filter.cpp
   )
+  ament_auto_add_gtest(object_lanelet_filter_tests
+    test/test_utils.cpp
+    test/lanelet_filter/test_lanelet_filter.cpp
+  )
 endif()
 
 ament_auto_package(INSTALL_TO_SHARE

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] from the base_link height in map frame
+      min_height_threshold: -1.0 # [m] from the base_link height in map frame
diff --git a/perception/autoware_detected_object_validation/object-lanelet-filter.md b/perception/autoware_detected_object_validation/object-lanelet-filter.md
@@ -24,10 +24,19 @@ The objects only inside of the vector map will be published.
 
 ## Parameters
 
-| Name                   | Type     | Description                                                                          |
-| ---------------------- | -------- | ------------------------------------------------------------------------------------ |
-| `debug`                | `bool`   | if true, publish debug markers                                                       |
-| `lanelet_extra_margin` | `double` | `if > 0` lanelet polygons are expanded by extra margin, `if <= 0` margin is disabled |
+Description of the `filter_settings` in the parameters of the `object_lanelet_filter` node.
+
+| Name                                              | Type     | Description                                                                                                                           |
+| ------------------------------------------------- | -------- | ------------------------------------------------------------------------------------------------------------------------------------- |
+| `debug`                                           | `bool`   | If `true`, publishes additional debug information, including visualization markers on the `~/debug/marker` topic for tools like RViz. |
+| `lanelet_extra_margin`                            | `double` | If `> 0`, expands the lanelet polygons used for overlap checks by this margin (in meters). If `<= 0`, polygon expansion is disabled.  |
+| `polygon_overlap_filter.enabled`                  | `bool`   | If `true`, enables filtering of objects based on their overlap with lanelet polygons.                                                 |
+| `lanelet_direction_filter.enabled`                | `bool`   | If `true`, enables filtering of objects based on their velocity direction relative to the lanelet.                                    |
+| `lanelet_direction_filter.velocity_yaw_threshold` | `double` | The yaw angle difference threshold (in radians) between the object’s velocity vector and the lanelet direction.                       |
+| `lanelet_direction_filter.object_speed_threshold` | `double` | The minimum speed (in m/s) of an object required for the direction filter to be applied.                                              |
+| `use_height_threshold`                            | `bool`   | If `true`, enables filtering of objects based on their height relative to the base_link frame.                                        |
+| `max_height_threshold`                            | `double` | The maximum allowable height (in meters) of an object relative to the base_link in the map frame.                                     |
+| `min_height_threshold`                            | `double` | The minimum allowable height (in meters) of an object relative to the base_link in the map frame.                                     |
 
 ### Core Parameters
 

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>(
@@ -146,6 +152,19 @@ void ObjectLaneletFilterNode::objectCallback(
     RCLCPP_ERROR(get_logger(), "Failed transform to %s.", lanelet_frame_id_.c_str());
     return;
   }
+  // vehicle base pose :map -> base_link
+  if (filter_settings_.use_height_threshold) {
+    try {
+      ego_base_height_ = tf_buffer_
+                           .lookupTransform(
+                             lanelet_frame_id_, "base_link", transformed_objects.header.stamp,
+                             rclcpp::Duration::from_seconds(0.5))
+                           .transform.translation.z;
+    } catch (const tf2::TransformException & ex) {
+      RCLCPP_ERROR_STREAM(get_logger(), "Failed to get transform: " << ex.what());
+      return;
+    }
+  }
 
   if (!transformed_objects.objects.empty()) {
     // calculate convex hull
@@ -199,6 +218,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 > ego_base_height_ + filter_settings_.max_height_threshold ||
+        object_height < ego_base_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>
@@ -88,6 +89,7 @@ class ObjectLaneletFilterNode : public rclcpp::Node
   tf2_ros::TransformListener tf_listener_;
 
   utils::FilterTargetLabel filter_target_;
+  double ego_base_height_ = 0.0;
   struct FilterSettings
   {
     bool polygon_overlap_filter;
@@ -96,6 +98,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(