fix(voxel_based_compare_map): temporary fix pointcloud transform lookup (#10299)

badai-nguyen · web-flow · commit 081457708528 · 2025-03-25T21:35:43.000+09:00
* fix(voxel_based_compare_map): temporary fix pointcloud transform lookup_time

Signed-off-by: badai-nguyen &lt;dai.nguyen@tier4.jp&gt;

* pre-commit

Signed-off-by: badai-nguyen &lt;dai.nguyen@tier4.jp&gt;

* chore: reduce timeout

Signed-off-by: badai-nguyen &lt;dai.nguyen@tier4.jp&gt;

* fix: misalignment when tranform back output

Signed-off-by: badai-nguyen &lt;dai.nguyen@tier4.jp&gt;

* fix: typo

Signed-off-by: badai-nguyen &lt;dai.nguyen@tier4.jp&gt;

---------

Signed-off-by: badai-nguyen &lt;dai.nguyen@tier4.jp&gt;
diff --git a/perception/autoware_compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp b/perception/autoware_compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp
@@ -20,16 +20,26 @@
 #include <pcl/kdtree/kdtree_flann.h>
 #include <pcl/search/kdtree.h>
 #include <pcl/segmentation/segment_differences.h>
-
+#ifdef ROS_DISTRO_GALACTIC
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
+#else
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#include <tf2_sensor_msgs/tf2_sensor_msgs.hpp>
+#endif
 #include <memory>
 #include <string>
+#include <utility>
+#include <vector>
 
 namespace autoware::compare_map_segmentation
 {
 
 VoxelBasedCompareMapFilterComponent::VoxelBasedCompareMapFilterComponent(
   const rclcpp::NodeOptions & options)
-: Filter("VoxelBasedCompareMapFilter", options)
+: Filter("VoxelBasedCompareMapFilter", options),
+  tf_buffer_(this->get_clock()),
+  tf_listener_(tf_buffer_)
 {
   // initialize debug tool
   {
@@ -62,6 +72,111 @@ VoxelBasedCompareMapFilterComponent::VoxelBasedCompareMapFilterComponent(
   RCLCPP_INFO(this->get_logger(), "tf_map_input_frame: %s", tf_input_frame_.c_str());
 }
 
+// TODO(badai-nguyen): Temporary Implementation of input_indices_callback and  convert_output_costly
+// functions; Delete this override function when autoware_utils refactor
+// (https://github.com/autowarefoundation/autoware_utils/pull/50) or new ManagedTransformBuffer lib
+// is deployed for autoware
+void VoxelBasedCompareMapFilterComponent::input_indices_callback(
+  const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloud, const PointIndicesConstPtr indices)
+{
+  // If cloud is given, check if it's valid
+  if (!isValid(cloud)) {
+    RCLCPP_ERROR(this->get_logger(), "[input_indices_callback] Invalid input!");
+    return;
+  }
+  // If indices are given, check if they are valid
+  if (indices && !isValid(indices)) {
+    RCLCPP_ERROR(this->get_logger(), "[input_indices_callback] Invalid indices!");
+    return;
+  }
+
+  // Check whether the user has given a different input TF frame
+  tf_input_orig_frame_ = cloud->header.frame_id;
+  PointCloud2ConstPtr cloud_tf;
+  if (cloud->header.frame_id != tf_input_frame_) {
+    RCLCPP_DEBUG(
+      this->get_logger(), "[input_indices_callback] Transforming input dataset from %s to %s.",
+      cloud->header.frame_id.c_str(), tf_input_frame_.c_str());
+    // Save the original frame ID
+    // Convert the cloud into the different frame
+    PointCloud2 cloud_transformed;
+
+    try {
+      // Lookup the transform from input frame to "map"
+      geometry_msgs::msg::TransformStamped transform_stamped = tf_buffer_.lookupTransform(
+        tf_input_frame_, tf_input_orig_frame_, rclcpp::Time(cloud->header.stamp),
+        rclcpp::Duration::from_seconds(0.0));
+
+      // Transform the point cloud
+      tf2::doTransform(*cloud, cloud_transformed, transform_stamped);
+      cloud_tf = std::make_shared<PointCloud2>(cloud_transformed);
+    } catch (tf2::TransformException & ex) {
+      RCLCPP_WARN_THROTTLE(
+        this->get_logger(), *this->get_clock(), 5000, "Could not transform pointcloud: %s",
+        ex.what());
+      cloud_tf = cloud;  // Fallback to original data
+    }
+  } else {
+    cloud_tf = cloud;
+  }
+  // Need setInputCloud () here because we have to extract x/y/z
+  IndicesPtr vindices;
+  if (indices) {
+    vindices.reset(new std::vector<int>(indices->indices));
+  }
+
+  computePublish(cloud_tf, vindices);
+}
+
+bool VoxelBasedCompareMapFilterComponent::convert_output_costly(
+  std::unique_ptr<PointCloud2> & output)
+{
+  if (!output || output->data.empty() || output->fields.empty()) {
+    RCLCPP_ERROR(this->get_logger(), "Invalid output point cloud!");
+    return false;
+  }
+  if (
+    pcl::getFieldIndex(*output, "x") == -1 || pcl::getFieldIndex(*output, "y") == -1 ||
+    pcl::getFieldIndex(*output, "z") == -1) {
+    RCLCPP_ERROR(this->get_logger(), "Input pointcloud does not have xyz fields");
+    return false;
+  }
+  if (!tf_output_frame_.empty() && output->header.frame_id != tf_output_frame_) {
+    auto cloud_transformed = std::make_unique<PointCloud2>();
+    try {
+      geometry_msgs::msg::TransformStamped transform_stamped = tf_buffer_.lookupTransform(
+        tf_output_frame_, output->header.frame_id, rclcpp::Time(output->header.stamp),
+        rclcpp::Duration::from_seconds(0.0));
+      tf2::doTransform(*output, *cloud_transformed, transform_stamped);
+      cloud_transformed->header.frame_id = tf_output_frame_;
+      output = std::move(cloud_transformed);
+    } catch (tf2::TransformException & e) {
+      RCLCPP_WARN_THROTTLE(
+        this->get_logger(), *this->get_clock(), 5000, "Could not transform pointcloud: %s",
+        e.what());
+      return false;
+    }
+  }
+
+  if (tf_output_frame_.empty() && output->header.frame_id != tf_input_orig_frame_) {
+    auto cloud_transformed = std::make_unique<PointCloud2>();
+    try {
+      geometry_msgs::msg::TransformStamped transform_stamped = tf_buffer_.lookupTransform(
+        tf_input_orig_frame_, output->header.frame_id, rclcpp::Time(output->header.stamp),
+        rclcpp::Duration::from_seconds(0.0));
+      tf2::doTransform(*output, *cloud_transformed, transform_stamped);
+      cloud_transformed->header.frame_id = tf_input_orig_frame_;
+      output = std::move(cloud_transformed);
+    } catch (tf2::TransformException & e) {
+      RCLCPP_WARN_THROTTLE(
+        this->get_logger(), *this->get_clock(), 5000, "Could not transform pointcloud: %s",
+        e.what());
+      return false;
+    }
+  }
+  return true;
+}
+
 void VoxelBasedCompareMapFilterComponent::filter(
   const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices,
   PointCloud2 & output)
diff --git a/perception/autoware_compare_map_segmentation/src/voxel_based_compare_map_filter/node.hpp b/perception/autoware_compare_map_segmentation/src/voxel_based_compare_map_filter/node.hpp
@@ -27,16 +27,29 @@ namespace autoware::compare_map_segmentation
 {
 class VoxelBasedCompareMapFilterComponent : public autoware::pointcloud_preprocessor::Filter
 {
+  using PointCloud2 = sensor_msgs::msg::PointCloud2;
+  using PointCloud2ConstPtr = sensor_msgs::msg::PointCloud2::ConstSharedPtr;
+
+  using PointIndices = pcl_msgs::msg::PointIndices;
+  using PointIndicesPtr = PointIndices::SharedPtr;
+  using PointIndicesConstPtr = PointIndices::ConstSharedPtr;
+
 protected:
   void filter(
     const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) override;
+  void input_indices_callback(
+    const PointCloud2ConstPtr cloud, const PointIndicesConstPtr indices) override;
+
+  bool convert_output_costly(std::unique_ptr<PointCloud2> & output) override;
 
 private:
   // pcl::SegmentDifferences<pcl::PointXYZ> impl_;
   std::unique_ptr<VoxelGridMapLoader> voxel_grid_map_loader_;
   rclcpp::Subscription<PointCloud2>::SharedPtr sub_map_;
   double distance_threshold_;
   bool set_map_in_voxel_grid_;
+  tf2_ros::Buffer tf_buffer_;
+  tf2_ros::TransformListener tf_listener_;
 
 public:
   PCL_MAKE_ALIGNED_OPERATOR_NEW
diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/filter.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/filter.hpp
@@ -206,7 +206,11 @@ class Filter : public rclcpp::Node
    * \param input the input point cloud dataset.
    * \param indices a pointer to the vector of point indices to use.
    */
-  void computePublish(const PointCloud2ConstPtr & input, const IndicesPtr & indices);
+  virtual void computePublish(const PointCloud2ConstPtr & input, const IndicesPtr & indices);
+  /** \brief PointCloud2 + Indices data callback. */
+  virtual void input_indices_callback(
+    const PointCloud2ConstPtr cloud, const PointIndicesConstPtr indices);
+  virtual bool convert_output_costly(std::unique_ptr<PointCloud2> & output);
 
   //////////////////////
   // from PCLNodelet //
@@ -279,16 +283,11 @@ class Filter : public rclcpp::Node
   std::shared_ptr<ExactTimeSyncPolicy> sync_input_indices_e_;
   std::shared_ptr<ApproximateTimeSyncPolicy> sync_input_indices_a_;
 
-  /** \brief PointCloud2 + Indices data callback. */
-  void input_indices_callback(const PointCloud2ConstPtr cloud, const PointIndicesConstPtr indices);
-
   /** \brief Get a matrix for conversion from the original frame to the target frame */
   bool calculate_transform_matrix(
     const std::string & target_frame, const sensor_msgs::msg::PointCloud2 & from,
     TransformInfo & transform_info /*output*/);
 
-  bool convert_output_costly(std::unique_ptr<PointCloud2> & output);
-
   // TODO(sykwer): Temporary Implementation: Remove this interface when all the filter nodes conform
   // to new API.
   void faster_input_indices_callback(
