From bc9727a872d7c9c034a81263d7a6ea375b65e656 Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Mon, 17 Mar 2025 17:50:39 +0900 Subject: [PATCH 1/4] fix: add selectable lookup time for dynamic transform Signed-off-by: badai-nguyen --- .../ros/managed_transform_buffer.hpp | 29 +++++++++++-------- 1 file changed, 17 insertions(+), 12 deletions(-) diff --git a/autoware_utils/include/autoware_utils/ros/managed_transform_buffer.hpp b/autoware_utils/include/autoware_utils/ros/managed_transform_buffer.hpp index a14dbf8..498e140 100644 --- a/autoware_utils/include/autoware_utils/ros/managed_transform_buffer.hpp +++ b/autoware_utils/include/autoware_utils/ros/managed_transform_buffer.hpp @@ -64,30 +64,32 @@ constexpr std::array warn_frames = {"map", "odom", "world"} class ManagedTransformBuffer { public: - explicit ManagedTransformBuffer(rclcpp::Node * node, const bool & has_static_tf_only) + explicit ManagedTransformBuffer( + rclcpp::Node * node, const bool & has_static_tf_only, + [[maybe_unused]] const rclcpp::Time & lookup_time = rclcpp::Time(0)) : 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); } /** @@ -113,7 +115,8 @@ 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(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); @@ -138,7 +141,7 @@ class ManagedTransformBuffer */ 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() || @@ -205,10 +208,10 @@ class ManagedTransformBuffer */ 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; @@ -220,7 +223,9 @@ class ManagedTransformBuffer TFMap buffer_; rclcpp::Node * const node_; std::unique_ptr tf_listener_; - std::function get_transform_; + std::function + get_transform_; }; } // namespace autoware_utils From 1fbfb072f99c7afccd9e7a407e1dfda3d4193cbc Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Tue, 18 Mar 2025 00:31:44 +0900 Subject: [PATCH 2/4] fix: change to select option Signed-off-by: badai-nguyen --- .../autoware_utils/ros/managed_transform_buffer.hpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/autoware_utils/include/autoware_utils/ros/managed_transform_buffer.hpp b/autoware_utils/include/autoware_utils/ros/managed_transform_buffer.hpp index 498e140..ea5901b 100644 --- a/autoware_utils/include/autoware_utils/ros/managed_transform_buffer.hpp +++ b/autoware_utils/include/autoware_utils/ros/managed_transform_buffer.hpp @@ -66,8 +66,8 @@ class ManagedTransformBuffer public: explicit ManagedTransformBuffer( rclcpp::Node * node, const bool & has_static_tf_only, - [[maybe_unused]] const rclcpp::Time & lookup_time = rclcpp::Time(0)) - : node_(node) + 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]( @@ -115,7 +115,10 @@ class ManagedTransformBuffer return true; } Eigen::Matrix4f eigen_transform; - rclcpp::Time lookup_time = rclcpp::Time(cloud_in.header.stamp); + 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; } @@ -221,6 +224,7 @@ class ManagedTransformBuffer } TFMap buffer_; + bool use_pc_stamp_for_lookup_; rclcpp::Node * const node_; std::unique_ptr tf_listener_; std::function Date: Tue, 18 Mar 2025 00:39:57 +0900 Subject: [PATCH 3/4] chore: docs Signed-off-by: badai-nguyen --- .../include/autoware_utils/ros/managed_transform_buffer.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/autoware_utils/include/autoware_utils/ros/managed_transform_buffer.hpp b/autoware_utils/include/autoware_utils/ros/managed_transform_buffer.hpp index ea5901b..4706020 100644 --- a/autoware_utils/include/autoware_utils/ros/managed_transform_buffer.hpp +++ b/autoware_utils/include/autoware_utils/ros/managed_transform_buffer.hpp @@ -138,6 +138,7 @@ 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. @@ -205,6 +206,7 @@ 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. From 6ca266c9e5ae0c2b9d48c820d2ab150e62713e0c Mon Sep 17 00:00:00 2001 From: badai-nguyen Date: Tue, 18 Mar 2025 11:05:11 +0900 Subject: [PATCH 4/4] fix: add selectable lookup time for dynamic transform Signed-off-by: badai-nguyen --- .../managed_transform_buffer.hpp | 37 ++++++++++++------- 1 file changed, 24 insertions(+), 13 deletions(-) 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