diff --git a/autoware_utils_pcl/include/autoware_utils_pcl/managed_transform_buffer.hpp b/autoware_utils_pcl/include/autoware_utils_pcl/managed_transform_buffer.hpp index c3a2a4a..30c3a3d 100644 --- a/autoware_utils_pcl/include/autoware_utils_pcl/managed_transform_buffer.hpp +++ b/autoware_utils_pcl/include/autoware_utils_pcl/managed_transform_buffer.hpp @@ -63,30 +63,32 @@ constexpr std::array warn_frames = {"map", "odom", "world"} class ManagedTransformBuffer { public: - explicit ManagedTransformBuffer(rclcpp::Node * node, const bool & has_static_tf_only) - : node_(node) + explicit ManagedTransformBuffer( + rclcpp::Node * node, const bool & has_static_tf_only, + const bool & use_pc_stamp_for_lookup = false) + : use_pc_stamp_for_lookup_(use_pc_stamp_for_lookup), node_(node) { if (has_static_tf_only) { get_transform_ = [this]( const std::string & target_frame, const std::string & source_frame, - Eigen::Matrix4f & eigen_transform) { - return get_static_transform(target_frame, source_frame, eigen_transform); + const rclcpp::Time & lookup_time, Eigen::Matrix4f & eigen_transform) { + return get_static_transform(target_frame, source_frame, lookup_time, eigen_transform); }; } else { tf_listener_ = std::make_unique(node); get_transform_ = [this]( const std::string & target_frame, const std::string & source_frame, - Eigen::Matrix4f & eigen_transform) { - return get_dynamic_transform(target_frame, source_frame, eigen_transform); + const rclcpp::Time & lookup_time, Eigen::Matrix4f & eigen_transform) { + return get_dynamic_transform(target_frame, source_frame, lookup_time, eigen_transform); }; } } bool get_transform( const std::string & target_frame, const std::string & source_frame, - Eigen::Matrix4f & eigen_transform) + const rclcpp::Time & lookup_time, Eigen::Matrix4f & eigen_transform) { - return get_transform_(target_frame, source_frame, eigen_transform); + return get_transform_(target_frame, source_frame, lookup_time, eigen_transform); } /** @@ -112,7 +114,11 @@ class ManagedTransformBuffer return true; } Eigen::Matrix4f eigen_transform; - if (!get_transform(target_frame, cloud_in.header.frame_id, eigen_transform)) { + rclcpp::Time lookup_time{rclcpp::Time(0)}; + if (use_pc_stamp_for_lookup_) { + lookup_time = rclcpp::Time(cloud_in.header.stamp); + } + if (!get_transform(target_frame, cloud_in.header.frame_id, lookup_time, eigen_transform)) { return false; } pcl_ros::transformPointCloud(eigen_transform, cloud_in, cloud_out); @@ -131,13 +137,14 @@ class ManagedTransformBuffer * * @param target_frame The target frame. * @param source_frame The source frame. + * @param lookup_time The target timestamp * @param eigen_transform The output Eigen transform matrix. It is set to the identity if the * transform is not found. * @return True if the transform was successfully retrieved, false otherwise. */ bool get_static_transform( const std::string & target_frame, const std::string & source_frame, - Eigen::Matrix4f & eigen_transform) + [[maybe_unused]] const rclcpp::Time & lookup_time, Eigen::Matrix4f & eigen_transform) { if ( std::find(warn_frames.begin(), warn_frames.end(), target_frame) != warn_frames.end() || @@ -198,16 +205,17 @@ class ManagedTransformBuffer * * @param target_frame The target frame. * @param source_frame The source frame. + * @param lookup_time The target timestamp * @param eigen_transform The output Eigen transformation matrix. It is set to the identity if the * transform is not found. * @return True if the transform was successfully retrieved, false otherwise. */ bool get_dynamic_transform( const std::string & target_frame, const std::string & source_frame, - Eigen::Matrix4f & eigen_transform) + const rclcpp::Time & lookup_time, Eigen::Matrix4f & eigen_transform) { auto tf = tf_listener_->get_transform( - target_frame, source_frame, rclcpp::Time(0), rclcpp::Duration(1000ms)); + target_frame, source_frame, lookup_time, rclcpp::Duration(1000ms)); if (tf == nullptr) { eigen_transform = Eigen::Matrix4f::Identity(); return false; @@ -217,9 +225,12 @@ class ManagedTransformBuffer } TFMap buffer_; + bool use_pc_stamp_for_lookup_; rclcpp::Node * const node_; std::unique_ptr tf_listener_; - std::function get_transform_; + std::function + get_transform_; }; } // namespace autoware_utils_pcl