fix(voxel_based_compare_map): temporary fix pointcloud transform lookup_time

Signed-off-by: badai-nguyen <dai.nguyen@tier4.jp>

Author: badai-nguyen

perception/autoware_compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp

@@ -20,7 +20,13 @@
 #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>
 
@@ -29,7 +35,9 @@ 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 +70,60 @@ VoxelBasedCompareMapFilterComponent::VoxelBasedCompareMapFilterComponent(
   RCLCPP_INFO(this->get_logger(), "tf_map_input_frame: %s", tf_input_frame_.c_str());
 }
 
+// TODO(badai-nguyen): Temporary Implementation: 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(1.0));
+
+      // Transform the point cloud
+      tf2::doTransform(*cloud, cloud_transformed, transform_stamped);
+      cloud_transformed.header.frame_id = tf_input_frame_;  // Update frame ID to "map"
+      cloud_tf = std::make_shared<PointCloud2>(cloud_transformed);
+    } catch (tf2::TransformException & ex) {
+      RCLCPP_WARN(this->get_logger(), "Could not transform point cloud: %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);
+}
+
 void VoxelBasedCompareMapFilterComponent::filter(
   const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices,
   PointCloud2 & output)

perception/autoware_compare_map_segmentation/src/voxel_based_compare_map_filter/node.hpp

@@ -27,16 +27,27 @@ 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;
 
 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

sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/filter.hpp

@@ -207,6 +207,9 @@ class Filter : public rclcpp::Node
    * \param indices a pointer to the vector of point indices to use.
    */
   void computePublish(const PointCloud2ConstPtr & input, const IndicesPtr & indices);
+  /** \brief PointCloud2 + Indices data callback. */
+  virtual void input_indices_callback(
+    const PointCloud2ConstPtr cloud, const PointIndicesConstPtr indices);
 
   //////////////////////
   // from PCLNodelet //
@@ -279,9 +282,6 @@ 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,