From 63cea6da822a33af11d581a50f83f7590d5dfba9 Mon Sep 17 00:00:00 2001
From: yoshiri <yoshiyoshidetteiu@gmail.com>
Date: Thu, 6 Jun 2024 18:25:08 +0900
Subject: [PATCH] feat: add node param to filter by z

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>
---
 .../vector_map_inside_area_filter.hpp         |  2 ++
 .../vector_map_inside_area_filter.cpp         | 29 +++++++++++++++++--
 2 files changed, 28 insertions(+), 3 deletions(-)

diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp
index 74209c1a22e4c..03b0d452bc07e 100644
--- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp
+++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/vector_map_inside_area_filter.hpp
@@ -43,6 +43,8 @@ class VectorMapInsideAreaFilterComponent : public pointcloud_preprocessor::Filte
 
   // parameter
   std::string polygon_type_;
+  bool use_z_filter_ = false;
+  float z_threshold_;
 
 public:
   PCL_MAKE_ALIGNED_OPERATOR_NEW
diff --git a/sensing/pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp b/sensing/pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp
index 5b8755714b375..7d63c3379129c 100644
--- a/sensing/pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp
+++ b/sensing/pointcloud_preprocessor/src/vector_map_filter/vector_map_inside_area_filter.cpp
@@ -40,7 +40,8 @@ lanelet::ConstPolygons3d calcIntersectedPolygons(
 }
 
 pcl::PointCloud<pcl::PointXYZ> removePointsWithinPolygons(
-  const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud_in, const lanelet::ConstPolygons3d & polygons)
+  const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud_in, const lanelet::ConstPolygons3d & polygons,
+  const std::optional<float> & z_threshold_)
 {
   std::vector<PolygonCgal> cgal_polys;
 
@@ -53,7 +54,7 @@ pcl::PointCloud<pcl::PointXYZ> removePointsWithinPolygons(
 
   pcl::PointCloud<pcl::PointXYZ> filtered_cloud;
   pointcloud_preprocessor::utils::remove_polygon_cgal_from_cloud(
-    *cloud_in, cgal_polys, filtered_cloud);
+    *cloud_in, cgal_polys, filtered_cloud, z_threshold_);
 
   return filtered_cloud;
 }
@@ -74,6 +75,10 @@ VectorMapInsideAreaFilterComponent::VectorMapInsideAreaFilterComponent(
   map_sub_ = this->create_subscription<autoware_auto_mapping_msgs::msg::HADMapBin>(
     "input/vector_map", rclcpp::QoS{1}.transient_local(),
     std::bind(&VectorMapInsideAreaFilterComponent::mapCallback, this, _1));
+
+  // Set parameters
+  use_z_filter_ = declare_parameter<bool>("use_z_filter", false);
+  z_threshold_ = declare_parameter<float>("z_threshold", 0.0f);  // defined in the base_link frame
 }
 
 void VectorMapInsideAreaFilterComponent::filter(
@@ -99,7 +104,25 @@ void VectorMapInsideAreaFilterComponent::filter(
   const auto intersected_lanelets = calcIntersectedPolygons(bounding_box, polygon_lanelets_);
 
   // filter pointcloud by lanelet
-  const auto filtered_pc = removePointsWithinPolygons(pc_input, intersected_lanelets);
+  std::optional<float> z_threshold_in_base_link = std::nullopt;
+  if (use_z_filter_) {
+    // assume z_max is defined in the base_link frame
+    const std::string base_link_frame = "base_link";
+    z_threshold_in_base_link = z_threshold_;
+    if (input->header.frame_id != base_link_frame) {
+      try {
+        // get z difference from baselink to input frame
+        const auto transform =
+          tf_buffer_->lookupTransform(input->header.frame_id, base_link_frame, input->header.stamp);
+        *z_threshold_in_base_link += transform.transform.translation.z;
+      } catch (const tf2::TransformException & e) {
+        RCLCPP_WARN(get_logger(), "Failed to transform z_threshold to base_link frame");
+        z_threshold_in_base_link = std::nullopt;
+      }
+    }
+  }
+  const auto filtered_pc =
+    removePointsWithinPolygons(pc_input, intersected_lanelets, z_threshold_in_base_link);
 
   // convert to ROS message
   pcl::toROSMsg(filtered_pc, output);