fix: add select option for transform lookup time

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

Author: badai-nguyen

launch/tier4_perception_launch/launch/object_recognition/detection/filter/pointcloud_map_filter.launch.py

@@ -110,6 +110,7 @@ def create_compare_map_pipeline(self):
                     self.pointcloud_map_filter_param,
                     {
                         "input_frame": "map",
+                        "use_pc_stamp_for_dynamic_transform_lookup": True,
                     },
                 ],
                 extra_arguments=[

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

@@ -172,6 +172,8 @@ class Filter : public rclcpp::Node
   /** \brief The flag to indicate if only static TF are used. */
   bool has_static_tf_only_;
 
+  bool use_pc_stamp_for_dynamic_transform_lookup_{false};
+
   /** \brief Internal mutex. */
   std::mutex mutex_;
 

sensing/autoware_pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp

@@ -89,8 +89,8 @@ void DistortionCorrectorBase::get_imu_transformation(
   }
 
   Eigen::Matrix4f eigen_imu_to_base_link;
-  imu_transform_exists_ =
-    managed_tf_buffer_->get_transform(base_frame, imu_frame, eigen_imu_to_base_link);
+  imu_transform_exists_ = managed_tf_buffer_->get_transform(
+    base_frame, imu_frame, rclcpp::Time(0), eigen_imu_to_base_link);
   tf2::Transform tf2_imu_to_base_link = convert_matrix_to_transform(eigen_imu_to_base_link);
 
   geometry_imu_to_base_link_ptr_ = std::make_shared<geometry_msgs::msg::TransformStamped>();
@@ -431,8 +431,8 @@ void DistortionCorrector2D::set_pointcloud_transform(
   }
 
   Eigen::Matrix4f eigen_lidar_to_base_link;
-  pointcloud_transform_exists_ =
-    managed_tf_buffer_->get_transform(base_frame, lidar_frame, eigen_lidar_to_base_link);
+  pointcloud_transform_exists_ = managed_tf_buffer_->get_transform(
+    base_frame, lidar_frame, rclcpp::Time(0), eigen_lidar_to_base_link);
   tf2_lidar_to_base_link_ = convert_matrix_to_transform(eigen_lidar_to_base_link);
   tf2_base_link_to_lidar_ = tf2_lidar_to_base_link_.inverse();
   pointcloud_transform_needed_ = base_frame != lidar_frame && pointcloud_transform_exists_;
@@ -445,8 +445,8 @@ void DistortionCorrector3D::set_pointcloud_transform(
     return;
   }
 
-  pointcloud_transform_exists_ =
-    managed_tf_buffer_->get_transform(base_frame, lidar_frame, eigen_lidar_to_base_link_);
+  pointcloud_transform_exists_ = managed_tf_buffer_->get_transform(
+    base_frame, lidar_frame, rclcpp::Time(0), eigen_lidar_to_base_link_);
   eigen_base_link_to_lidar_ = eigen_lidar_to_base_link_.inverse();
   pointcloud_transform_needed_ = base_frame != lidar_frame && pointcloud_transform_exists_;
 }

sensing/autoware_pointcloud_preprocessor/src/filter.cpp

@@ -73,6 +73,8 @@ autoware::pointcloud_preprocessor::Filter::Filter(
     tf_input_frame_ = static_cast<std::string>(declare_parameter("input_frame", ""));
     tf_output_frame_ = static_cast<std::string>(declare_parameter("output_frame", ""));
     has_static_tf_only_ = static_cast<bool>(declare_parameter("has_static_tf_only", false));
+    use_pc_stamp_for_dynamic_transform_lookup_ =
+      static_cast<bool>(declare_parameter("use_pc_stamp_for_dynamic_transform_lookup", false));
     max_queue_size_ = static_cast<std::size_t>(declare_parameter("max_queue_size", 5));
 
     // ---[ Optional parameters
@@ -123,8 +125,8 @@ void autoware::pointcloud_preprocessor::Filter::setupTF()
     }
     has_static_tf_only_ = true;
   }
-  managed_tf_buffer_ =
-    std::make_unique<autoware_utils::ManagedTransformBuffer>(this, has_static_tf_only_);
+  managed_tf_buffer_ = std::make_unique<autoware_utils::ManagedTransformBuffer>(
+    this, has_static_tf_only_, use_pc_stamp_for_dynamic_transform_lookup_);
 }
 
 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -307,9 +309,12 @@ bool autoware::pointcloud_preprocessor::Filter::calculate_transform_matrix(
   RCLCPP_DEBUG(
     this->get_logger(), "[get_transform_matrix] Transforming input dataset from %s to %s.",
     from.header.frame_id.c_str(), target_frame.c_str());
-
+  rclcpp::Time lookup_time = rclcpp::Time(0);
+  if (use_pc_stamp_for_dynamic_transform_lookup_) {
+    lookup_time = rclcpp::Time(from.header.stamp);
+  }
   if (!managed_tf_buffer_->get_transform(
-        target_frame, from.header.frame_id, transform_info.eigen_transform)) {
+        target_frame, from.header.frame_id, lookup_time, transform_info.eigen_transform)) {
     return false;
   }