From 9cb4145182bdef764dd60f8e1c829e2943acc830 Mon Sep 17 00:00:00 2001 From: Amadeusz Szymko Date: Tue, 25 Feb 2025 09:14:14 +0900 Subject: [PATCH 1/3] feat: reduce impl to single temp listener Signed-off-by: Amadeusz Szymko --- README.md | 2 +- .../example_managed_transform_buffer.cpp | 9 +- .../managed_transform_buffer.hpp | 120 ++++++--- .../src/managed_transform_buffer.cpp | 235 +++++++++++------- .../test/test_managed_transform_buffer.cpp | 198 +++++++++++---- 5 files changed, 377 insertions(+), 187 deletions(-) diff --git a/README.md b/README.md index 34e5591..4cb09d7 100644 --- a/README.md +++ b/README.md @@ -2,7 +2,7 @@ ## Purpose -This package contains a wrapper of ROS 2 TF buffer & listener. It offers better pefromance +This package contains a wrapper of ROS 2 TF buffer & listener. It offers better performance in large systems with multiple TF listeners to static transformations. ![Managed Transform Buffer](https://github.com/user-attachments/assets/b8c29b6a-fc77-4941-a50b-8aa30fdc2e36) diff --git a/managed_transform_buffer/examples/example_managed_transform_buffer.cpp b/managed_transform_buffer/examples/example_managed_transform_buffer.cpp index c24e7e0..38e8b6d 100644 --- a/managed_transform_buffer/examples/example_managed_transform_buffer.cpp +++ b/managed_transform_buffer/examples/example_managed_transform_buffer.cpp @@ -30,7 +30,8 @@ class ExampleNode : public rclcpp::Node { target_frame_ = declare_parameter("target_frame", "dummy_target_frame"); source_frame_ = declare_parameter("source_frame", "dummy_source_frame"); - managed_tf_buffer_ = std::make_unique(this); + managed_tf_buffer_ = + std::make_unique(this->get_clock()); cloud_sub_ = create_subscription( "input/cloud", rclcpp::SensorDataQoS(), std::bind(&ExampleNode::cloud_cb, this, std::placeholders::_1)); @@ -50,7 +51,8 @@ class ExampleNode : public rclcpp::Node void cloud_cb(const sensor_msgs::msg::PointCloud2::SharedPtr msg) { sensor_msgs::msg::PointCloud2 transformed_cloud; - if (managed_tf_buffer_->transformPointcloud(target_frame_, *msg, transformed_cloud)) { + if (managed_tf_buffer_->transformPointcloud( + target_frame_, *msg, transformed_cloud, this->now(), rclcpp::Duration::from_seconds(1))) { RCLCPP_INFO(get_logger(), "Pointcloud transformed"); cloud_pub_->publish(transformed_cloud); } @@ -58,7 +60,8 @@ class ExampleNode : public rclcpp::Node void timer_cb() { - auto tf = managed_tf_buffer_->getTransform(target_frame_, source_frame_); + auto tf = managed_tf_buffer_->getTransform( + target_frame_, source_frame_, this->now(), rclcpp::Duration::from_seconds(1)); if (tf.has_value()) { RCLCPP_INFO( diff --git a/managed_transform_buffer/include/managed_transform_buffer/managed_transform_buffer.hpp b/managed_transform_buffer/include/managed_transform_buffer/managed_transform_buffer.hpp index 2fe0c9d..8483e06 100644 --- a/managed_transform_buffer/include/managed_transform_buffer/managed_transform_buffer.hpp +++ b/managed_transform_buffer/include/managed_transform_buffer/managed_transform_buffer.hpp @@ -22,15 +22,16 @@ #include #include +#include #include #include -#include #include #include #include #include +#include #include #include #include @@ -99,10 +100,12 @@ class ManagedTransformBuffer /** * @brief Construct a new Managed Transform Buffer object * - * @param[in] node the node to use for the transform buffer - * @param[in] managed whether managed buffer feature should be used + * @param[in] clock A clock to use for time and sleeping + * @param[in] cache_time How long to keep a history of transforms */ - explicit ManagedTransformBuffer(rclcpp::Node * node, bool managed = true); + explicit ManagedTransformBuffer( + rclcpp::Clock::SharedPtr clock, + tf2::Duration cache_time = tf2::Duration(tf2::BUFFER_CORE_DEFAULT_CACHE_TIME)); /** @brief Destroy the Managed Transform Buffer object */ ~ManagedTransformBuffer(); @@ -129,21 +132,40 @@ class ManagedTransformBuffer template std::enable_if_t, std::optional> getTransform( - const std::string & target_frame, const std::string & source_frame, - const rclcpp::Time & time = rclcpp::Time(0), - const rclcpp::Duration & timeout = default_timeout); + const std::string & target_frame, const std::string & source_frame, const tf2::TimePoint & time, + const tf2::Duration & timeout); template std::enable_if_t, std::optional> getTransform( - const std::string & target_frame, const std::string & source_frame, - const rclcpp::Time & time = rclcpp::Time(0), - const rclcpp::Duration & timeout = default_timeout); + const std::string & target_frame, const std::string & source_frame, const tf2::TimePoint & time, + const tf2::Duration & timeout); template std::enable_if_t, std::optional> getTransform( - const std::string & target_frame, const std::string & source_frame, - const rclcpp::Time & time = rclcpp::Time(0), - const rclcpp::Duration & timeout = default_timeout); + const std::string & target_frame, const std::string & source_frame, const tf2::TimePoint & time, + const tf2::Duration & timeout); + + /** + * @brief Get the transform between two frames by frame ID. + * + * @sa getTransform(const std::string &, const std::string &, const tf2::TimePoint &, const + * tf2::Duration) + */ + template + std::enable_if_t, std::optional> + getTransform( + const std::string & target_frame, const std::string & source_frame, const rclcpp::Time & time, + const rclcpp::Duration & timeout); + + template + std::enable_if_t, std::optional> getTransform( + const std::string & target_frame, const std::string & source_frame, const rclcpp::Time & time, + const rclcpp::Duration & timeout); + + template + std::enable_if_t, std::optional> getTransform( + const std::string & target_frame, const std::string & source_frame, const rclcpp::Time & time, + const rclcpp::Duration & timeout); /** * @brief Transforms a point cloud from one frame to another. @@ -151,27 +173,49 @@ class ManagedTransformBuffer * @param[in] target_frame the target TF frame * @param[in] cloud_in the input point cloud * @param[out] cloud_out the resultant output point cloud - * @param[in] time the time at which the value of the transform is desired (0 will get the latest) + * @param[in] time the time at which the value of the transform is desired * @param[in] timeout how long to block before failing * @return true if the transformation is successful, false otherwise */ bool transformPointcloud( const std::string & target_frame, const sensor_msgs::msg::PointCloud2 & cloud_in, - sensor_msgs::msg::PointCloud2 & cloud_out, const rclcpp::Time & time = rclcpp::Time(0), - const rclcpp::Duration & timeout = default_timeout); + sensor_msgs::msg::PointCloud2 & cloud_out, const tf2::TimePoint & time, + const tf2::Duration & timeout); + + /** + * @brief Transforms a point cloud from one frame to another. + * + * @sa transformPointcloud(const std::string &, const sensor_msgs::msg::PointCloud2 &, + * sensor_msgs::msg::PointCloud2 &, const tf2::TimePoint &, const tf2::Duration) + */ + bool transformPointcloud( + const std::string & target_frame, const sensor_msgs::msg::PointCloud2 & cloud_in, + sensor_msgs::msg::PointCloud2 & cloud_out, const rclcpp::Time & time, + const rclcpp::Duration & timeout); + + /** @brief Check if all TFs requests have been for static TF so far. + * @return true if only static TFs have been requested + */ + bool isStatic() const; private: - /** @brief Initialize TF listener used for storing transforms */ + /** @brief Initialize TF listener */ void activateListener(); - /** @brief Initialize local TF listener used for building TF tree */ - void activateLocalListener(); - /** @brief Deactivate TF listener */ void deactivateListener(); - /** @brief Deactivate local TF listener */ - void deactivateLocalListener(); + /** @brief Register TF buffer as unknown. */ + void registerAsUnknown(); + + /** @brief Register TF buffer as dynamic. */ + void registerAsDynamic(); + + /** @brief Generate a unique node name + * + * @return a unique node name + */ + std::string generateUniqueNodeName(); /** @brief Callback for TF messages * @@ -189,9 +233,8 @@ class ManagedTransformBuffer * @return an optional containing the transform if successful, or empty if not */ std::optional lookupTransform( - const std::string & target_frame, const std::string & source_frame, - const rclcpp::Time & time = rclcpp::Time(0), - const rclcpp::Duration & timeout = default_timeout) const; + const std::string & target_frame, const std::string & source_frame, const tf2::TimePoint & time, + const tf2::Duration & timeout) const; /** @brief Traverse TF tree built by local TF listener. * @@ -202,7 +245,7 @@ class ManagedTransformBuffer */ TraverseResult traverseTree( const std::string & target_frame, const std::string & source_frame, - const rclcpp::Duration & timeout = default_timeout); + const tf2::Duration & timeout); /** @brief Get a dynamic transform from the TF buffer. * @@ -213,9 +256,8 @@ class ManagedTransformBuffer * @return an optional containing the transform if successful, or empty if not */ std::optional getDynamicTransform( - const std::string & target_frame, const std::string & source_frame, - const rclcpp::Time & time = rclcpp::Time(0), - const rclcpp::Duration & timeout = default_timeout); + const std::string & target_frame, const std::string & source_frame, const tf2::TimePoint & time, + const tf2::Duration & timeout); /** @brief Get a static transform from local TF buffer. * @@ -235,30 +277,30 @@ class ManagedTransformBuffer * @return an optional containing the transform if successful, or empty if not */ std::optional getUnknownTransform( - const std::string & target_frame, const std::string & source_frame, - const rclcpp::Time & time = rclcpp::Time(0), - const rclcpp::Duration & timeout = default_timeout); + const std::string & target_frame, const std::string & source_frame, const tf2::TimePoint & time, + const tf2::Duration & timeout); + rclcpp::Node::SharedPtr node_{nullptr}; + rclcpp::Clock::SharedPtr clock_{nullptr}; rclcpp::CallbackGroup::SharedPtr callback_group_{nullptr}; - rclcpp::Node * const node_; - rclcpp::Node::SharedPtr managed_listener_node_{nullptr}; rclcpp::NodeOptions options_; rclcpp::Subscription::SharedPtr tf_static_sub_{nullptr}; rclcpp::Subscription::SharedPtr tf_sub_{nullptr}; rclcpp::SubscriptionOptionsWithAllocator> tf_options_; rclcpp::SubscriptionOptionsWithAllocator> tf_static_options_; - rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_{nullptr}; std::function( - const std::string &, const std::string &, const rclcpp::Time &, const rclcpp::Duration &)> + const std::string &, const std::string &, const tf2::TimePoint &, const tf2::Duration &)> get_transform_; std::function cb_; std::function cb_static_; - std::unique_ptr dedicated_listener_thread_; + std::shared_ptr executor_{nullptr}; + std::shared_ptr executor_thread_{nullptr}; std::unique_ptr tf_buffer_; - std::unique_ptr tf_listener_; std::unique_ptr static_tf_buffer_; std::unique_ptr tf_tree_; - static std::chrono::milliseconds default_timeout; + std::mt19937 random_engine_; + std::uniform_int_distribution<> dis_; + bool is_static_{true}; }; } // namespace managed_transform_buffer diff --git a/managed_transform_buffer/src/managed_transform_buffer.cpp b/managed_transform_buffer/src/managed_transform_buffer.cpp index e204b63..5f8c3bc 100644 --- a/managed_transform_buffer/src/managed_transform_buffer.cpp +++ b/managed_transform_buffer/src/managed_transform_buffer.cpp @@ -26,9 +26,7 @@ #include #include -#include #include -#include #include #include #include @@ -36,55 +34,50 @@ namespace managed_transform_buffer { -std::chrono::milliseconds ManagedTransformBuffer::default_timeout = 10ms; - -ManagedTransformBuffer::ManagedTransformBuffer(rclcpp::Node * node, bool managed) : node_(node) +ManagedTransformBuffer::ManagedTransformBuffer( + rclcpp::Clock::SharedPtr clock, tf2::Duration cache_time) +: clock_(clock) { - get_transform_ = [this, managed]( - const std::string & target_frame, const std::string & source_frame, - const rclcpp::Time & time, - const rclcpp::Duration & timeout) -> std::optional { - if (managed) { - return getUnknownTransform(target_frame, source_frame, time, timeout); - } - return getDynamicTransform(target_frame, source_frame, time, timeout); - }; + executor_ = std::make_shared(); + executor_thread_ = std::make_shared( + std::bind(&rclcpp::executors::SingleThreadedExecutor::spin, executor_)); static_tf_buffer_ = std::make_unique(); tf_tree_ = std::make_unique(); - tf_buffer_ = std::make_unique(node_->get_clock()); - auto timer_interface = std::make_shared( - node_->get_node_base_interface(), node_->get_node_timers_interface()); - tf_buffer_->setCreateTimerInterface(timer_interface); + tf_buffer_ = std::make_unique(clock_, cache_time); + tf_buffer_->setUsingDedicatedThread(true); tf_options_ = tf2_ros::detail::get_default_transform_listener_sub_options(); tf_static_options_ = tf2_ros::detail::get_default_transform_listener_static_sub_options(); cb_ = std::bind(&ManagedTransformBuffer::tfCallback, this, std::placeholders::_1, false); cb_static_ = std::bind(&ManagedTransformBuffer::tfCallback, this, std::placeholders::_1, true); - std::stringstream sstream; - sstream << "managed_tf_listener_impl_" << std::hex << reinterpret_cast(this); - options_.arguments({"--ros-args", "-r", "__node:=" + std::string(sstream.str())}); options_.start_parameter_event_publisher(false); options_.start_parameter_services(false); + random_engine_ = std::mt19937(std::random_device{}()); + dis_ = std::uniform_int_distribution<>(0, 0xFFFFFF); + registerAsUnknown(); } ManagedTransformBuffer::~ManagedTransformBuffer() { deactivateListener(); - deactivateLocalListener(); + executor_->cancel(); + if (executor_thread_->joinable()) { + executor_thread_->join(); + } } template <> std::optional ManagedTransformBuffer::getTransform( - const std::string & target_frame, const std::string & source_frame, const rclcpp::Time & time, - const rclcpp::Duration & timeout) + const std::string & target_frame, const std::string & source_frame, const tf2::TimePoint & time, + const tf2::Duration & timeout) { return get_transform_(target_frame, source_frame, time, timeout); } template <> std::optional ManagedTransformBuffer::getTransform( - const std::string & target_frame, const std::string & source_frame, const rclcpp::Time & time, - const rclcpp::Duration & timeout) + const std::string & target_frame, const std::string & source_frame, const tf2::TimePoint & time, + const tf2::Duration & timeout) { auto tf = get_transform_(target_frame, source_frame, time, timeout); if (!tf.has_value()) { @@ -97,8 +90,8 @@ std::optional ManagedTransformBuffer::getTransform std::optional ManagedTransformBuffer::getTransform( - const std::string & target_frame, const std::string & source_frame, const rclcpp::Time & time, - const rclcpp::Duration & timeout) + const std::string & target_frame, const std::string & source_frame, const tf2::TimePoint & time, + const tf2::Duration & timeout) { auto tf = get_transform_(target_frame, source_frame, time, timeout); if (!tf.has_value()) { @@ -109,15 +102,41 @@ std::optional ManagedTransformBuffer::getTransform(tf2_transform); } +template <> +std::optional ManagedTransformBuffer::getTransform( + const std::string & target_frame, const std::string & source_frame, const rclcpp::Time & time, + const rclcpp::Duration & timeout) +{ + return getTransform( + target_frame, source_frame, tf2_ros::fromRclcpp(time), tf2_ros::fromRclcpp(timeout)); +} + +template <> +std::optional ManagedTransformBuffer::getTransform( + const std::string & target_frame, const std::string & source_frame, const rclcpp::Time & time, + const rclcpp::Duration & timeout) +{ + return getTransform( + target_frame, source_frame, tf2_ros::fromRclcpp(time), tf2_ros::fromRclcpp(timeout)); +} + +template <> +std::optional ManagedTransformBuffer::getTransform( + const std::string & target_frame, const std::string & source_frame, const rclcpp::Time & time, + const rclcpp::Duration & timeout) +{ + return getTransform( + target_frame, source_frame, tf2_ros::fromRclcpp(time), tf2_ros::fromRclcpp(timeout)); +} + bool ManagedTransformBuffer::transformPointcloud( const std::string & target_frame, const sensor_msgs::msg::PointCloud2 & cloud_in, - sensor_msgs::msg::PointCloud2 & cloud_out, const rclcpp::Time & time, - const rclcpp::Duration & timeout) + sensor_msgs::msg::PointCloud2 & cloud_out, const tf2::TimePoint & time, + const tf2::Duration & timeout) { if ( pcl::getFieldIndex(cloud_in, "x") == -1 || pcl::getFieldIndex(cloud_in, "y") == -1 || pcl::getFieldIndex(cloud_in, "z") == -1) { - RCLCPP_ERROR(node_->get_logger(), "Input pointcloud does not have xyz fields"); return false; } if (target_frame == cloud_in.header.frame_id) { @@ -134,74 +153,115 @@ bool ManagedTransformBuffer::transformPointcloud( return true; } -void ManagedTransformBuffer::activateListener() +bool ManagedTransformBuffer::transformPointcloud( + const std::string & target_frame, const sensor_msgs::msg::PointCloud2 & cloud_in, + sensor_msgs::msg::PointCloud2 & cloud_out, const rclcpp::Time & time, + const rclcpp::Duration & timeout) { - tf_listener_ = std::make_unique(*tf_buffer_); + return transformPointcloud( + target_frame, cloud_in, cloud_out, tf2_ros::fromRclcpp(time), tf2_ros::fromRclcpp(timeout)); } -void ManagedTransformBuffer::activateLocalListener() +bool ManagedTransformBuffer::isStatic() const { - managed_listener_node_ = rclcpp::Node::make_unique("_", options_); - callback_group_ = managed_listener_node_->create_callback_group( - rclcpp::CallbackGroupType::MutuallyExclusive, false); - tf_options_.callback_group = callback_group_; - tf_static_options_.callback_group = callback_group_; - tf_sub_ = rclcpp::create_subscription( - managed_listener_node_, "/tf", tf2_ros::DynamicListenerQoS(), cb_, tf_options_); - tf_static_sub_ = rclcpp::create_subscription( - managed_listener_node_, "/tf_static", tf2_ros::StaticListenerQoS(), cb_static_, - tf_static_options_); - executor_ = std::make_unique(); - executor_->add_callback_group(callback_group_, managed_listener_node_->get_node_base_interface()); - dedicated_listener_thread_ = std::make_unique([&]() { executor_->spin(); }); + return is_static_; } -void ManagedTransformBuffer::deactivateListener() +void ManagedTransformBuffer::activateListener() { - tf_listener_.reset(); + options_.arguments({"--ros-args", "-r", "__node:=" + generateUniqueNodeName()}); + node_ = rclcpp::Node::make_unique("_", options_); + callback_group_ = + node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); + tf_options_.callback_group = callback_group_; + tf_static_options_.callback_group = callback_group_; + tf_sub_ = node_->create_subscription( + "/tf", tf2_ros::DynamicListenerQoS(), cb_, tf_options_); + tf_static_sub_ = node_->create_subscription( + "/tf_static", tf2_ros::StaticListenerQoS(), cb_static_, tf_static_options_); + executor_->add_callback_group(callback_group_, node_->get_node_base_interface()); } -void ManagedTransformBuffer::deactivateLocalListener() +void ManagedTransformBuffer::deactivateListener() { - if (executor_) { - executor_->cancel(); - } - if (dedicated_listener_thread_ && dedicated_listener_thread_->joinable()) { - dedicated_listener_thread_->join(); + auto cb_grps = executor_->get_all_callback_groups(); + for (auto & cb_grp : cb_grps) { + executor_->remove_callback_group(cb_grp.lock()); } tf_static_sub_.reset(); tf_sub_.reset(); - managed_listener_node_.reset(); - executor_.reset(); - dedicated_listener_thread_.reset(); + node_.reset(); +} + +void ManagedTransformBuffer::registerAsUnknown() +{ + get_transform_ = [this]( + const std::string & target_frame, const std::string & source_frame, + const tf2::TimePoint & time, + const tf2::Duration & timeout) -> std::optional { + return getUnknownTransform(target_frame, source_frame, time, timeout); + }; +} + +void ManagedTransformBuffer::registerAsDynamic() +{ + is_static_ = false; + get_transform_ = [this]( + const std::string & target_frame, const std::string & source_frame, + const tf2::TimePoint & time, + const tf2::Duration & timeout) -> std::optional { + if (!node_) { + activateListener(); + } + return getDynamicTransform(target_frame, source_frame, time, timeout); + }; +} + +std::string ManagedTransformBuffer::generateUniqueNodeName() +{ + std::stringstream sstream; + sstream << "managed_tf_listener_impl_" << std::hex << dis_(random_engine_) + << dis_(random_engine_); + return sstream.str(); } void ManagedTransformBuffer::tfCallback( const tf2_msgs::msg::TFMessage::SharedPtr msg, const bool is_static) { - for (const auto & transform : msg->transforms) { - tf_tree_->emplace(transform.child_frame_id, TreeNode{transform.header.frame_id, is_static}); + std::string authority = "Authority undetectable"; + for (const auto & tf : msg->transforms) { + try { + tf_buffer_->setTransform(tf, authority, is_static); + tf_tree_->emplace(tf.child_frame_id, TreeNode{tf.header.frame_id, is_static}); + } catch (const tf2::TransformException & ex) { + if (node_) { + RCLCPP_ERROR( + node_->get_logger(), "Failure to set received transform from %s to %s with error: %s\n", + tf.child_frame_id.c_str(), tf.header.frame_id.c_str(), ex.what()); + } + } } } std::optional ManagedTransformBuffer::lookupTransform( - const std::string & target_frame, const std::string & source_frame, const rclcpp::Time & time, - const rclcpp::Duration & timeout) const + const std::string & target_frame, const std::string & source_frame, const tf2::TimePoint & time, + const tf2::Duration & timeout) const { try { auto tf = tf_buffer_->lookupTransform(target_frame, source_frame, time, timeout); return std::make_optional(tf); } catch (const tf2::TransformException & ex) { - RCLCPP_ERROR( - node_->get_logger(), "Failure to get transform from %s to %s with error: %s", - target_frame.c_str(), source_frame.c_str(), ex.what()); + if (node_) { + RCLCPP_ERROR( + node_->get_logger(), "Failure to get transform from %s to %s with error: %s", + target_frame.c_str(), source_frame.c_str(), ex.what()); + } return std::nullopt; } } TraverseResult ManagedTransformBuffer::traverseTree( - const std::string & target_frame, const std::string & source_frame, - const rclcpp::Duration & timeout) + const std::string & target_frame, const std::string & source_frame, const tf2::Duration & timeout) { std::atomic timeout_reached{false}; @@ -218,7 +278,9 @@ TraverseResult ManagedTransformBuffer::traverseTree( frame_it = last_tf_tree.find(current_frame); depth++; if (depth > tf2::BufferCore::MAX_GRAPH_DEPTH) { - RCLCPP_ERROR(node_->get_logger(), "Traverse depth exceeded for %s", start_frame.c_str()); + if (node_) { + RCLCPP_ERROR(node_->get_logger(), "Traverse depth exceeded for %s", start_frame.c_str()); + } return false; } } @@ -267,8 +329,7 @@ TraverseResult ManagedTransformBuffer::traverseTree( std::future future = std::async(std::launch::async, traverse, target_frame, source_frame); - if ( - future.wait_for(timeout.to_chrono()) == std::future_status::ready) { + if (future.wait_for(timeout) == std::future_status::ready) { return future.get(); } timeout_reached = true; @@ -276,10 +337,10 @@ TraverseResult ManagedTransformBuffer::traverseTree( } std::optional ManagedTransformBuffer::getDynamicTransform( - const std::string & target_frame, const std::string & source_frame, const rclcpp::Time & time, - const rclcpp::Duration & timeout) + const std::string & target_frame, const std::string & source_frame, const tf2::TimePoint & time, + const tf2::Duration & timeout) { - if (!tf_listener_) { + if (!node_) { activateListener(); } return lookupTransform(target_frame, source_frame, time, timeout); @@ -295,7 +356,7 @@ std::optional ManagedTransformBuffer::getStaticTransform( auto it = static_tf_buffer_->find(key); if (it != static_tf_buffer_->end()) { auto tf_msg = it->second; - tf_msg.header.stamp = node_->now(); + tf_msg.header.stamp = clock_->now(); return std::make_optional(tf_msg); } @@ -310,7 +371,7 @@ std::optional ManagedTransformBuffer::getStaticTransform( inv_tf_msg.transform = tf2::toMsg(inv_tf); inv_tf_msg.header.frame_id = tf_msg.child_frame_id; inv_tf_msg.child_frame_id = tf_msg.header.frame_id; - inv_tf_msg.header.stamp = node_->now(); + inv_tf_msg.header.stamp = clock_->now(); static_tf_buffer_->emplace(key, inv_tf_msg); return std::make_optional(inv_tf_msg); } @@ -322,7 +383,7 @@ std::optional ManagedTransformBuffer::getStaticTransform( tf_msg.transform = tf2::toMsg(tf_identity); tf_msg.header.frame_id = target_frame; tf_msg.child_frame_id = source_frame; - tf_msg.header.stamp = node_->now(); + tf_msg.header.stamp = clock_->now(); static_tf_buffer_->emplace(key, tf_msg); return std::make_optional(tf_msg); } @@ -331,8 +392,8 @@ std::optional ManagedTransformBuffer::getStaticTransform( } std::optional ManagedTransformBuffer::getUnknownTransform( - const std::string & target_frame, const std::string & source_frame, const rclcpp::Time & time, - const rclcpp::Duration & timeout) + const std::string & target_frame, const std::string & source_frame, const tf2::TimePoint & time, + const tf2::Duration & timeout) { // Try to get transform from local static buffer auto static_tf = getStaticTransform(target_frame, source_frame); @@ -341,7 +402,6 @@ std::optional ManagedTransformBuffer::getUnknownTransform( } // Initialize local TF listener and base TF listener - activateLocalListener(); activateListener(); // Check local TF tree and TF buffer asynchronously @@ -353,10 +413,10 @@ std::optional ManagedTransformBuffer::getUnknownTransform( time, timeout); auto traverse_result = traverse_future.get(); auto tf = tf_future.get(); - deactivateLocalListener(); // Mimic lookup transform error if TF not exists in tree or buffer if (!traverse_result.success || !tf.has_value()) { + deactivateListener(); return std::nullopt; } @@ -366,15 +426,12 @@ std::optional ManagedTransformBuffer::getUnknownTransform( static_tf_buffer_->emplace(key, tf.value()); deactivateListener(); } else { - get_transform_ = [this]( - const std::string & target_frame, const std::string & source_frame, - const rclcpp::Time & time, - const rclcpp::Duration & timeout) -> std::optional { - return getDynamicTransform(target_frame, source_frame, time, timeout); - }; - RCLCPP_DEBUG( - node_->get_logger(), "Transform %s -> %s is dynamic. Switching to dynamic listener.", - target_frame.c_str(), source_frame.c_str()); + registerAsDynamic(); + if (node_) { + RCLCPP_INFO( + node_->get_logger(), "Transform %s -> %s is dynamic. Switching to dynamic listener.", + target_frame.c_str(), source_frame.c_str()); + } } return tf; diff --git a/managed_transform_buffer/test/test_managed_transform_buffer.cpp b/managed_transform_buffer/test/test_managed_transform_buffer.cpp index 524a3a2..2d48969 100644 --- a/managed_transform_buffer/test/test_managed_transform_buffer.cpp +++ b/managed_transform_buffer/test/test_managed_transform_buffer.cpp @@ -27,34 +27,37 @@ #include #include #include +#include -#include #include #include #include -std::chrono::milliseconds managed_transform_buffer::ManagedTransformBuffer::default_timeout = - std::chrono::milliseconds(100); // Relax timeout for CI - class TestManagedTransformBuffer : public ::testing::Test { protected: - std::shared_ptr node_{nullptr}; - std::shared_ptr managed_tf_buffer_{nullptr}; - std::shared_ptr tf_broadcaster_; + rclcpp::Node::SharedPtr node_{nullptr}; + rclcpp::TimerBase::SharedPtr timer_{nullptr}; + std::unique_ptr managed_tf_buffer_{nullptr}; + std::unique_ptr static_tf_broadcaster_{nullptr}; + std::unique_ptr tf_broadcaster_{nullptr}; + geometry_msgs::msg::TransformStamped tf_map_to_base_; geometry_msgs::msg::TransformStamped tf_base_to_lidar_top_; geometry_msgs::msg::TransformStamped tf_base_to_lidar_right_; + Eigen::Matrix4f eigen_map_to_base_; Eigen::Matrix4f eigen_base_to_lidar_top_; Eigen::Matrix4f eigen_base_to_lidar_right_; - std::unique_ptr cloud_in_; + std::unique_ptr cloud_in_{nullptr}; double precision_; + rclcpp::Time time_; + rclcpp::Duration timeout_ = rclcpp::Duration::from_seconds(1); geometry_msgs::msg::TransformStamped generateTransformMsg( const int32_t seconds, const uint32_t nanoseconds, const std::string & parent_frame, const std::string & child_frame, double x, double y, double z, double qx, double qy, double qz, double qw) { - rclcpp::Time timestamp(seconds, nanoseconds, RCL_ROS_TIME); + rclcpp::Time timestamp(seconds, nanoseconds, node_->get_clock()->get_clock_type()); geometry_msgs::msg::TransformStamped tf_msg; tf_msg.header.stamp = timestamp; tf_msg.header.frame_id = parent_frame; @@ -69,23 +72,45 @@ class TestManagedTransformBuffer : public ::testing::Test return tf_msg; } + void broadcastDynamicTf(geometry_msgs::msg::TransformStamped transform, uint32_t seconds = 1) + { + timer_ = node_->create_wall_timer(std::chrono::milliseconds(100), [this, transform]() -> void { + tf_broadcaster_->sendTransform(transform); + }); + + rclcpp::Rate r(10); + rclcpp::spin_some(node_); + for (uint32_t i = 0; i < 10u * seconds; ++i) { + r.sleep(); + rclcpp::spin_some(node_); + } + + timer_->cancel(); + timer_->reset(); + } + void SetUp() override { - node_ = std::make_unique("test_managed_transform_buffer"); + node_ = std::make_shared("test_managed_transform_buffer"); managed_tf_buffer_ = - std::make_unique(node_.get(), true); - tf_broadcaster_ = std::make_unique(node_); + std::make_unique(node_->get_clock()); + static_tf_broadcaster_ = std::make_unique(node_); + tf_broadcaster_ = std::make_unique(node_); + tf_map_to_base_ = generateTransformMsg( + 10, 100'000'000, "map", "base_link", 120.0, 240.0, 1.0, 0.0, 0.0, 0.0, 1.0); tf_base_to_lidar_top_ = generateTransformMsg( 10, 100'000'000, "base_link", "lidar_top", 0.690, 0.000, 2.100, -0.007, -0.007, 0.692, 0.722); tf_base_to_lidar_right_ = generateTransformMsg( 10, 100'000'000, "base_link", "lidar_right", 0.0, -0.56362, -0.30555, 0.244, 0.248, 0.665, 0.661); + eigen_map_to_base_ = tf2::transformToEigen(tf_map_to_base_).matrix().cast(); eigen_base_to_lidar_top_ = tf2::transformToEigen(tf_base_to_lidar_top_).matrix().cast(); eigen_base_to_lidar_right_ = tf2::transformToEigen(tf_base_to_lidar_right_).matrix().cast(); cloud_in_ = std::make_unique(); precision_ = 0.01; + time_ = rclcpp::Time(10, 100'000'000); // Set up the fields for x, y, and z coordinates cloud_in_->fields.resize(3); @@ -109,158 +134,221 @@ class TestManagedTransformBuffer : public ::testing::Test // Set up cloud header cloud_in_->header.frame_id = "lidar_top"; - cloud_in_->header.stamp = rclcpp::Time(10, 100'000'000); + cloud_in_->header.stamp = time_; ASSERT_TRUE(rclcpp::ok()); } - void TearDown() override { managed_tf_buffer_.reset(); } + void TearDown() override {} }; TEST_F(TestManagedTransformBuffer, TestReturn) { - tf_broadcaster_->sendTransform(tf_base_to_lidar_top_); + static_tf_broadcaster_->sendTransform(tf_base_to_lidar_top_); auto eigen_transform = - managed_tf_buffer_->getTransform("base_link", "lidar_top"); + managed_tf_buffer_->getTransform("base_link", "lidar_top", time_, timeout_); EXPECT_TRUE(eigen_transform.has_value()); - auto tf2_transform = managed_tf_buffer_->getTransform("base_link", "lidar_top"); + auto tf2_transform = + managed_tf_buffer_->getTransform("base_link", "lidar_top", time_, timeout_); EXPECT_TRUE(tf2_transform.has_value()); auto tf_msg_transform = managed_tf_buffer_->getTransform( - "base_link", "lidar_top"); + "base_link", "lidar_top", time_, timeout_); EXPECT_TRUE(tf_msg_transform.has_value()); + EXPECT_TRUE(managed_tf_buffer_->isStatic()); } TEST_F(TestManagedTransformBuffer, TestTransformNoExist) { - tf_broadcaster_->sendTransform(tf_base_to_lidar_top_); + static_tf_broadcaster_->sendTransform(tf_base_to_lidar_top_); auto eigen_transform = - managed_tf_buffer_->getTransform("base_link", "fake_link"); + managed_tf_buffer_->getTransform("base_link", "fake_link", time_, timeout_); EXPECT_FALSE(eigen_transform.has_value()); + EXPECT_TRUE(managed_tf_buffer_->isStatic()); } TEST_F(TestManagedTransformBuffer, TestTransformBase) { - tf_broadcaster_->sendTransform(tf_base_to_lidar_top_); - + static_tf_broadcaster_->sendTransform(tf_base_to_lidar_top_); auto eigen_base_to_lidar_top = - managed_tf_buffer_->getTransform("base_link", "lidar_top"); + managed_tf_buffer_->getTransform("base_link", "lidar_top", time_, timeout_); ASSERT_TRUE(eigen_base_to_lidar_top.has_value()); EXPECT_TRUE(eigen_base_to_lidar_top.value().isApprox(eigen_base_to_lidar_top_, precision_)); + EXPECT_TRUE(managed_tf_buffer_->isStatic()); } TEST_F(TestManagedTransformBuffer, TestTransformSameFrame) { - tf_broadcaster_->sendTransform(tf_base_to_lidar_top_); + static_tf_broadcaster_->sendTransform(tf_base_to_lidar_top_); auto eigen_base_to_base = - managed_tf_buffer_->getTransform("base_link", "base_link"); + managed_tf_buffer_->getTransform("base_link", "base_link", time_, timeout_); ASSERT_TRUE(eigen_base_to_base.has_value()); EXPECT_TRUE(eigen_base_to_base.value().isApprox(Eigen::Matrix4f::Identity(), precision_)); + EXPECT_TRUE(managed_tf_buffer_->isStatic()); } TEST_F(TestManagedTransformBuffer, TestTransformInverse) { - tf_broadcaster_->sendTransform(tf_base_to_lidar_top_); + static_tf_broadcaster_->sendTransform(tf_base_to_lidar_top_); auto eigen_lidar_top_tobase = - managed_tf_buffer_->getTransform("lidar_top", "base_link"); + managed_tf_buffer_->getTransform("lidar_top", "base_link", time_, timeout_); ASSERT_TRUE(eigen_lidar_top_tobase.has_value()); EXPECT_TRUE( eigen_lidar_top_tobase.value().isApprox(eigen_base_to_lidar_top_.inverse(), precision_)); + EXPECT_TRUE(managed_tf_buffer_->isStatic()); } TEST_F(TestManagedTransformBuffer, TestTransformNonDirect) { - tf_broadcaster_->sendTransform(tf_base_to_lidar_top_); - tf_broadcaster_->sendTransform(tf_base_to_lidar_right_); + static_tf_broadcaster_->sendTransform(tf_base_to_lidar_top_); + static_tf_broadcaster_->sendTransform(tf_base_to_lidar_right_); auto eigen_lidar_top_to_lidar_right = - managed_tf_buffer_->getTransform("lidar_top", "lidar_right"); + managed_tf_buffer_->getTransform("lidar_top", "lidar_right", time_, timeout_); ASSERT_TRUE(eigen_lidar_top_to_lidar_right.has_value()); - // EXPECT_TRUE(eigen_lidar_top_to_lidar_right.value().isApprox( - // eigen_base_to_lidar_top_.inverse() * eigen_base_to_lidar_right_, precision_)); + EXPECT_TRUE(eigen_lidar_top_to_lidar_right.value().isApprox( + eigen_base_to_lidar_top_.inverse() * eigen_base_to_lidar_right_, precision_)); + EXPECT_TRUE(managed_tf_buffer_->isStatic()); +} + +TEST_F(TestManagedTransformBuffer, TestTransformDynamic) +{ + static_tf_broadcaster_->sendTransform(tf_base_to_lidar_top_); + static_tf_broadcaster_->sendTransform(tf_base_to_lidar_right_); + + std::future future = + std::async(std::launch::async, [this]() { broadcastDynamicTf(tf_map_to_base_); }); + auto eigen_map_to_base = + managed_tf_buffer_->getTransform("map", "base_link", time_, timeout_); + future.wait(); + + ASSERT_TRUE(eigen_map_to_base.has_value()); + EXPECT_TRUE(eigen_map_to_base.value().isApprox(eigen_map_to_base_, precision_)); + EXPECT_FALSE(managed_tf_buffer_->isStatic()); + + auto eigen_lidar_top_to_lidar_right = + managed_tf_buffer_->getTransform("lidar_top", "lidar_right", time_, timeout_); + ASSERT_TRUE(eigen_lidar_top_to_lidar_right.has_value()); + EXPECT_TRUE(eigen_lidar_top_to_lidar_right.value().isApprox( + eigen_base_to_lidar_top_.inverse() * eigen_base_to_lidar_right_, precision_)); + EXPECT_FALSE(managed_tf_buffer_->isStatic()); } TEST_F(TestManagedTransformBuffer, TestTransformMultipleCall) { - tf_broadcaster_->sendTransform(tf_base_to_lidar_top_); + static_tf_broadcaster_->sendTransform(tf_base_to_lidar_top_); std::optional eigen_transform; - eigen_transform = managed_tf_buffer_->getTransform("base_link", "fake_link"); + eigen_transform = + managed_tf_buffer_->getTransform("base_link", "fake_link", time_, timeout_); EXPECT_FALSE(eigen_transform.has_value()); - eigen_transform = managed_tf_buffer_->getTransform("lidar_top", "base_link"); + eigen_transform = + managed_tf_buffer_->getTransform("lidar_top", "base_link", time_, timeout_); ASSERT_TRUE(eigen_transform.has_value()); EXPECT_TRUE(eigen_transform.value().isApprox(eigen_base_to_lidar_top_.inverse(), precision_)); - eigen_transform = managed_tf_buffer_->getTransform("fake_link", "fake_link"); + eigen_transform = + managed_tf_buffer_->getTransform("fake_link", "fake_link", time_, timeout_); ASSERT_TRUE(eigen_transform.has_value()); EXPECT_TRUE(eigen_transform.value().isApprox(Eigen::Matrix4f::Identity(), precision_)); - eigen_transform = managed_tf_buffer_->getTransform("base_link", "lidar_top"); + eigen_transform = + managed_tf_buffer_->getTransform("base_link", "lidar_top", time_, timeout_); ASSERT_TRUE(eigen_transform.has_value()); EXPECT_TRUE(eigen_transform.value().isApprox(eigen_base_to_lidar_top_, precision_)); - eigen_transform = managed_tf_buffer_->getTransform("fake_link", "lidar_top"); + eigen_transform = + managed_tf_buffer_->getTransform("fake_link", "lidar_top", time_, timeout_); EXPECT_FALSE(eigen_transform.has_value()); - eigen_transform = managed_tf_buffer_->getTransform("base_link", "lidar_top"); + eigen_transform = + managed_tf_buffer_->getTransform("base_link", "lidar_top", time_, timeout_); ASSERT_TRUE(eigen_transform.has_value()); EXPECT_TRUE(eigen_transform.value().isApprox(eigen_base_to_lidar_top_, precision_)); + EXPECT_TRUE(managed_tf_buffer_->isStatic()); + + std::future future = + std::async(std::launch::async, [this]() { broadcastDynamicTf(tf_map_to_base_); }); + auto eigen_map_to_base = + managed_tf_buffer_->getTransform("map", "base_link", time_, timeout_); + future.wait(); + ASSERT_TRUE(eigen_map_to_base.has_value()); + EXPECT_TRUE(eigen_map_to_base.value().isApprox(eigen_map_to_base_, precision_)); + EXPECT_FALSE(managed_tf_buffer_->isStatic()); + + eigen_transform = + managed_tf_buffer_->getTransform("fake_link", "fake_link", time_, timeout_); + ASSERT_TRUE(eigen_transform.has_value()); + EXPECT_TRUE(eigen_transform.value().isApprox(Eigen::Matrix4f::Identity(), precision_)); + EXPECT_FALSE(managed_tf_buffer_->isStatic()); } TEST_F(TestManagedTransformBuffer, TestTransformEmptyPointCloud) { - tf_broadcaster_->sendTransform(tf_base_to_lidar_top_); + static_tf_broadcaster_->sendTransform(tf_base_to_lidar_top_); auto cloud_in = std::make_unique(); cloud_in->header.frame_id = "lidar_top"; cloud_in->header.stamp = rclcpp::Time(10, 100'000'000); auto cloud_out = std::make_unique(); - EXPECT_FALSE(managed_tf_buffer_->transformPointcloud("lidar_top", *cloud_in, *cloud_out)); - EXPECT_FALSE(managed_tf_buffer_->transformPointcloud("base_link", *cloud_in, *cloud_out)); - EXPECT_FALSE(managed_tf_buffer_->transformPointcloud("fake_link", *cloud_in, *cloud_out)); + EXPECT_FALSE( + managed_tf_buffer_->transformPointcloud("lidar_top", *cloud_in, *cloud_out, time_, timeout_)); + EXPECT_FALSE( + managed_tf_buffer_->transformPointcloud("base_link", *cloud_in, *cloud_out, time_, timeout_)); + EXPECT_FALSE( + managed_tf_buffer_->transformPointcloud("fake_link", *cloud_in, *cloud_out, time_, timeout_)); } TEST_F(TestManagedTransformBuffer, TestTransformEmptyPointCloudNoHeader) { - tf_broadcaster_->sendTransform(tf_base_to_lidar_top_); + static_tf_broadcaster_->sendTransform(tf_base_to_lidar_top_); auto cloud_in = std::make_unique(); auto cloud_out = std::make_unique(); - EXPECT_FALSE(managed_tf_buffer_->transformPointcloud("lidar_top", *cloud_in, *cloud_out)); - EXPECT_FALSE(managed_tf_buffer_->transformPointcloud("base_link", *cloud_in, *cloud_out)); - EXPECT_FALSE(managed_tf_buffer_->transformPointcloud("fake_link", *cloud_in, *cloud_out)); + EXPECT_FALSE( + managed_tf_buffer_->transformPointcloud("lidar_top", *cloud_in, *cloud_out, time_, timeout_)); + EXPECT_FALSE( + managed_tf_buffer_->transformPointcloud("base_link", *cloud_in, *cloud_out, time_, timeout_)); + EXPECT_FALSE( + managed_tf_buffer_->transformPointcloud("fake_link", *cloud_in, *cloud_out, time_, timeout_)); } TEST_F(TestManagedTransformBuffer, TestTransformPointCloud) { - tf_broadcaster_->sendTransform(tf_base_to_lidar_top_); + static_tf_broadcaster_->sendTransform(tf_base_to_lidar_top_); auto cloud_out = std::make_unique(); // Transform cloud with header - EXPECT_TRUE(managed_tf_buffer_->transformPointcloud("lidar_top", *cloud_in_, *cloud_out)); - EXPECT_TRUE(managed_tf_buffer_->transformPointcloud("base_link", *cloud_in_, *cloud_out)); - EXPECT_FALSE(managed_tf_buffer_->transformPointcloud("fake_link", *cloud_in_, *cloud_out)); + EXPECT_TRUE( + managed_tf_buffer_->transformPointcloud("lidar_top", *cloud_in_, *cloud_out, time_, timeout_)); + EXPECT_TRUE( + managed_tf_buffer_->transformPointcloud("base_link", *cloud_in_, *cloud_out, time_, timeout_)); + EXPECT_FALSE( + managed_tf_buffer_->transformPointcloud("fake_link", *cloud_in_, *cloud_out, time_, timeout_)); } TEST_F(TestManagedTransformBuffer, TestTransformPointCloudNoHeader) { - tf_broadcaster_->sendTransform(tf_base_to_lidar_top_); + static_tf_broadcaster_->sendTransform(tf_base_to_lidar_top_); auto cloud_out = std::make_unique(); // Transform cloud without header auto cloud_in = std::make_unique(*cloud_in_); cloud_in->header.frame_id = ""; cloud_in->header.stamp = rclcpp::Time(0, 0); - EXPECT_FALSE(managed_tf_buffer_->transformPointcloud("lidar_top", *cloud_in, *cloud_out)); - EXPECT_FALSE(managed_tf_buffer_->transformPointcloud("base_link", *cloud_in, *cloud_out)); - EXPECT_FALSE(managed_tf_buffer_->transformPointcloud("fake_link", *cloud_in, *cloud_out)); + EXPECT_FALSE( + managed_tf_buffer_->transformPointcloud("lidar_top", *cloud_in, *cloud_out, time_, timeout_)); + EXPECT_FALSE( + managed_tf_buffer_->transformPointcloud("base_link", *cloud_in, *cloud_out, time_, timeout_)); + EXPECT_FALSE( + managed_tf_buffer_->transformPointcloud("fake_link", *cloud_in, *cloud_out, time_, timeout_)); } int main(int argc, char ** argv) From 759d5ec3184bdffadeaba7f71090cca2bb0e8e0e Mon Sep 17 00:00:00 2001 From: Amadeusz Szymko Date: Tue, 25 Feb 2025 19:50:34 +0900 Subject: [PATCH 2/3] feat: use singleton pattern Signed-off-by: Amadeusz Szymko --- README.md | 19 +- managed_transform_buffer/CMakeLists.txt | 1 + .../managed_transform_buffer.hpp | 162 +------- .../managed_transform_buffer_provider.hpp | 235 ++++++++++++ .../src/managed_transform_buffer.cpp | 315 +-------------- .../src/managed_transform_buffer_provider.cpp | 361 ++++++++++++++++++ .../test/test_managed_transform_buffer.cpp | 4 +- 7 files changed, 621 insertions(+), 476 deletions(-) create mode 100644 managed_transform_buffer/include/managed_transform_buffer/managed_transform_buffer_provider.hpp create mode 100644 managed_transform_buffer/src/managed_transform_buffer_provider.cpp diff --git a/README.md b/README.md index 4cb09d7..2a518d3 100644 --- a/README.md +++ b/README.md @@ -2,11 +2,12 @@ ## Purpose -This package contains a wrapper of ROS 2 TF buffer & listener. It offers better performance -in large systems with multiple TF listeners to static transformations. +This package contains a wrapper of ROS 2 TF buffer & listener. It offers better performance in large systems with multiple TF listeners. ![Managed Transform Buffer](https://github.com/user-attachments/assets/b8c29b6a-fc77-4941-a50b-8aa30fdc2e36) +Apart from the obvious benefits in cases of static-only transformations, it also boosts performance for scenarios with Composable Node Containers thanks to the use of the Singleton pattern. + ## Installation ```bash @@ -22,12 +23,12 @@ Library exposes a few handy function for handling TFs and PointCloud2 transforma ```cpp // Create a managed TF buffer -auto managed_tf_buffer = std::make_unique(node); +auto managed_tf_buffer = std::make_unique(this->get_clock()); // Get a transform from source_frame to target_frame -auto tf_msg_transform = managed_tf_buffer->getTransform("my_target_frame", "my_source_frame"); -auto tf2_transform = managed_tf_buffer->getTransform("my_target_frame", "my_source_frame"); -auto eigen_transform = managed_tf_buffer->getTransform("my_target_frame", "my_source_frame"); +auto tf_msg_transform = managed_tf_buffer->getTransform("my_target_frame", "my_source_frame", this->now(), rclcpp::Duration::from_seconds(1)); +auto tf2_transform = managed_tf_buffer->getTransform("my_target_frame", "my_source_frame", this->now(), rclcpp::Duration::from_seconds(1)); +auto eigen_transform = managed_tf_buffer->getTransform("my_target_frame", "my_source_frame", this->now(), rclcpp::Duration::from_seconds(1)); if (tf_msg_transform.has_value()) { @@ -36,7 +37,7 @@ if (tf_msg_transform.has_value()) // Transform a PointCloud2 message sensor_msgs::msg::PointCloud2 transformed_cloud; -auto success = managed_tf_buffer->transformPointcloud("my_target_frame", *in_cloud_msg, transformed_cloud); +auto success = managed_tf_buffer->transformPointcloud("my_target_frame", *in_cloud_msg, transformed_cloud, this->now(), rclcpp::Duration::from_seconds(1)); ``` For full example, see [example_managed_transform_buffer.cpp](managed_transform_buffer/examples/example_managed_transform_buffer.cpp). @@ -46,3 +47,7 @@ You can also build this example and run it: colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -DBUILD_EXAMPLES=On --packages-select managed_transform_buffer ros2 run managed_transform_buffer example_managed_transform_buffer --ros-args -p target_frame:=my_target_frame -p source_frame:=my_source_frame -r input/cloud:=/my_input_cloud -r output/cloud:=/my_output_cloud ``` + +## Limitations + +- Requests for dynamic transforms with zero timeout might never succeed. This limitation is due to the fact that the listener is initialized for each transform request (till first occurence of dynamic transform). If timeout is zero, the listener might not have enought time to fill the buffer. diff --git a/managed_transform_buffer/CMakeLists.txt b/managed_transform_buffer/CMakeLists.txt index 88187f7..ad6f947 100644 --- a/managed_transform_buffer/CMakeLists.txt +++ b/managed_transform_buffer/CMakeLists.txt @@ -16,6 +16,7 @@ ament_auto_find_build_dependencies() ament_auto_add_library(${PROJECT_NAME} SHARED src/managed_transform_buffer.cpp + src/managed_transform_buffer_provider.cpp ) if(BUILD_TESTING) diff --git a/managed_transform_buffer/include/managed_transform_buffer/managed_transform_buffer.hpp b/managed_transform_buffer/include/managed_transform_buffer/managed_transform_buffer.hpp index 8483e06..28e709f 100644 --- a/managed_transform_buffer/include/managed_transform_buffer/managed_transform_buffer.hpp +++ b/managed_transform_buffer/include/managed_transform_buffer/managed_transform_buffer.hpp @@ -17,76 +17,23 @@ #ifndef MANAGED_TRANSFORM_BUFFER__MANAGED_TRANSFORM_BUFFER_HPP_ #define MANAGED_TRANSFORM_BUFFER__MANAGED_TRANSFORM_BUFFER_HPP_ +#include "managed_transform_buffer/managed_transform_buffer_provider.hpp" + #include #include +#include #include #include #include -#include -#include - -#include -#include -#include #include -#include #include -#include #include -#include -#include - -namespace std -{ -template <> -struct hash> -{ - size_t operator()(const std::pair & p) const - { - size_t h1 = std::hash{}(p.first); - size_t h2 = std::hash{}(p.second); - return h1 ^ (h2 << 1u); - } -}; -} // namespace std namespace managed_transform_buffer { -using Key = std::pair; -struct PairEqual -{ - bool operator()(const Key & p1, const Key & p2) const - { - return p1.first == p2.first && p1.second == p2.second; - } -}; -struct TreeNode -{ - TreeNode() : is_static(false) {} - TreeNode(std::string p_parent, const bool p_is_static) - : parent(std::move(p_parent)), is_static(p_is_static) - { - } - std::string parent; - bool is_static; -}; - -struct TraverseResult -{ - TraverseResult() : success(false), is_static(false) {} - TraverseResult(const bool p_success, const bool p_is_static) - : success(p_success), is_static(p_is_static) - { - } - bool success; - bool is_static; -}; -using std::chrono_literals::operator""ms; using geometry_msgs::msg::TransformStamped; -using TFMap = std::unordered_map, PairEqual>; -using TreeMap = std::unordered_map; /** * @brief A managed TF buffer that handles listener node lifetime. This buffer triggers listener @@ -199,108 +146,7 @@ class ManagedTransformBuffer bool isStatic() const; private: - /** @brief Initialize TF listener */ - void activateListener(); - - /** @brief Deactivate TF listener */ - void deactivateListener(); - - /** @brief Register TF buffer as unknown. */ - void registerAsUnknown(); - - /** @brief Register TF buffer as dynamic. */ - void registerAsDynamic(); - - /** @brief Generate a unique node name - * - * @return a unique node name - */ - std::string generateUniqueNodeName(); - - /** @brief Callback for TF messages - * - * @param[in] msg the TF message - * @param[in] is_static whether the TF topic refers to static transforms - */ - void tfCallback(const tf2_msgs::msg::TFMessage::SharedPtr msg, const bool is_static); - - /** @brief Default ROS-ish lookupTransform trigger. - * - * @param[in] target_frame the frame to which data should be transformed - * @param[in] source_frame the frame where the data originated - * @param[in] time the time at which the value of the transform is desired (0 will get the latest) - * @param[in] timeout how long to block before failing - * @return an optional containing the transform if successful, or empty if not - */ - std::optional lookupTransform( - const std::string & target_frame, const std::string & source_frame, const tf2::TimePoint & time, - const tf2::Duration & timeout) const; - - /** @brief Traverse TF tree built by local TF listener. - * - * @param[in] target_frame the frame to which data should be transformed - * @param[in] source_frame the frame where the data originated - * @param[in] timeout how long to block before failing - * @return a traverse result indicating if the transform is possible and if it is static - */ - TraverseResult traverseTree( - const std::string & target_frame, const std::string & source_frame, - const tf2::Duration & timeout); - - /** @brief Get a dynamic transform from the TF buffer. - * - * @param[in] target_frame the frame to which data should be transformed - * @param[in] source_frame the frame where the data originated - * @param[in] time the time at which the value of the transform is desired (0 will get the latest) - * @param[in] timeout how long to block before failing - * @return an optional containing the transform if successful, or empty if not - */ - std::optional getDynamicTransform( - const std::string & target_frame, const std::string & source_frame, const tf2::TimePoint & time, - const tf2::Duration & timeout); - - /** @brief Get a static transform from local TF buffer. - * - * @param[in] target_frame the frame to which data should be transformed - * @param[in] source_frame the frame where the data originated - * @return an optional containing the transform if successful, or empty if not - */ - std::optional getStaticTransform( - const std::string & target_frame, const std::string & source_frame); - - /** @brief Get an unknown (static or dynamic) transform. - * - * @param[in] target_frame the frame to which data should be transformed - * @param[in] source_frame the frame where the data originated - * @param[in] time the time at which the value of the transform is desired (0 will get the latest) - * @param[in] timeout how long to block before failing - * @return an optional containing the transform if successful, or empty if not - */ - std::optional getUnknownTransform( - const std::string & target_frame, const std::string & source_frame, const tf2::TimePoint & time, - const tf2::Duration & timeout); - - rclcpp::Node::SharedPtr node_{nullptr}; - rclcpp::Clock::SharedPtr clock_{nullptr}; - rclcpp::CallbackGroup::SharedPtr callback_group_{nullptr}; - rclcpp::NodeOptions options_; - rclcpp::Subscription::SharedPtr tf_static_sub_{nullptr}; - rclcpp::Subscription::SharedPtr tf_sub_{nullptr}; - rclcpp::SubscriptionOptionsWithAllocator> tf_options_; - rclcpp::SubscriptionOptionsWithAllocator> tf_static_options_; - std::function( - const std::string &, const std::string &, const tf2::TimePoint &, const tf2::Duration &)> - get_transform_; - std::function cb_; - std::function cb_static_; - std::shared_ptr executor_{nullptr}; - std::shared_ptr executor_thread_{nullptr}; - std::unique_ptr tf_buffer_; - std::unique_ptr static_tf_buffer_; - std::unique_ptr tf_tree_; - std::mt19937 random_engine_; - std::uniform_int_distribution<> dis_; - bool is_static_{true}; + ManagedTransformBufferProvider * provider_; }; } // namespace managed_transform_buffer diff --git a/managed_transform_buffer/include/managed_transform_buffer/managed_transform_buffer_provider.hpp b/managed_transform_buffer/include/managed_transform_buffer/managed_transform_buffer_provider.hpp new file mode 100644 index 0000000..f8622af --- /dev/null +++ b/managed_transform_buffer/include/managed_transform_buffer/managed_transform_buffer_provider.hpp @@ -0,0 +1,235 @@ +// Copyright 2024 The Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Co-developed by Tier IV, Inc. + +#ifndef MANAGED_TRANSFORM_BUFFER__MANAGED_TRANSFORM_BUFFER_PROVIDER_HPP_ +#define MANAGED_TRANSFORM_BUFFER__MANAGED_TRANSFORM_BUFFER_PROVIDER_HPP_ + +#include + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace std +{ +template <> +struct hash> +{ + size_t operator()(const std::pair & p) const + { + size_t h1 = std::hash{}(p.first); + size_t h2 = std::hash{}(p.second); + return h1 ^ (h2 << 1u); + } +}; +} // namespace std + +namespace managed_transform_buffer +{ +using Key = std::pair; +struct PairEqual +{ + bool operator()(const Key & p1, const Key & p2) const + { + return p1.first == p2.first && p1.second == p2.second; + } +}; + +struct TreeNode +{ + TreeNode() : is_static(false) {} + TreeNode(std::string p_parent, const bool p_is_static) + : parent(std::move(p_parent)), is_static(p_is_static) + { + } + std::string parent; + bool is_static; +}; + +struct TraverseResult +{ + TraverseResult() : success(false), is_static(false) {} + TraverseResult(const bool p_success, const bool p_is_static) + : success(p_success), is_static(p_is_static) + { + } + bool success; + bool is_static; +}; + +using geometry_msgs::msg::TransformStamped; +using TFMap = std::unordered_map, PairEqual>; +using TreeMap = std::unordered_map; + +/** + * @brief A managed TF buffer provider with use of singleton pattern. + */ +class ManagedTransformBufferProvider +{ +public: + /** @brief Get the instance of the ManagedTransformBufferProvider + * + * @return the instance of the ManagedTransformBufferProvider + */ + static ManagedTransformBufferProvider & getInstance( + rclcpp::Clock::SharedPtr clock, + tf2::Duration cache_time = tf2::Duration(tf2::BUFFER_CORE_DEFAULT_CACHE_TIME)); + + ManagedTransformBufferProvider(const ManagedTransformBufferProvider &) = delete; + ManagedTransformBufferProvider & operator=(const ManagedTransformBufferProvider &) = delete; + + /** @brief Destroy the Managed Transform Buffer object */ + ~ManagedTransformBufferProvider(); + + std::optional getTransform( + const std::string & target_frame, const std::string & source_frame, const tf2::TimePoint & time, + const tf2::Duration & timeout); + + /** @brief Check if all TFs requests have been for static TF so far. + * @return true if only static TFs have been requested + */ + bool isStatic() const; + +private: + /** + * @brief Construct a new Managed Transform Buffer object + * + * @param[in] clock A clock to use for time and sleeping + * @param[in] cache_time How long to keep a history of transforms + */ + explicit ManagedTransformBufferProvider( + rclcpp::Clock::SharedPtr clock, + tf2::Duration cache_time = tf2::Duration(tf2::BUFFER_CORE_DEFAULT_CACHE_TIME)); + + /** @brief Initialize TF listener */ + void activateListener(); + + /** @brief Deactivate TF listener */ + void deactivateListener(); + + /** @brief Register TF buffer as unknown. */ + void registerAsUnknown(); + + /** @brief Register TF buffer as dynamic. */ + void registerAsDynamic(); + + /** @brief Generate a unique node name + * + * @return a unique node name + */ + std::string generateUniqueNodeName(); + + /** @brief Callback for TF messages + * + * @param[in] msg the TF message + * @param[in] is_static whether the TF topic refers to static transforms + */ + void tfCallback(const tf2_msgs::msg::TFMessage::SharedPtr msg, const bool is_static); + + /** @brief Default ROS-ish lookupTransform trigger. + * + * @param[in] target_frame the frame to which data should be transformed + * @param[in] source_frame the frame where the data originated + * @param[in] time the time at which the value of the transform is desired (0 will get the latest) + * @param[in] timeout how long to block before failing + * @return an optional containing the transform if successful, or empty if not + */ + std::optional lookupTransform( + const std::string & target_frame, const std::string & source_frame, const tf2::TimePoint & time, + const tf2::Duration & timeout) const; + + /** @brief Traverse TF tree built by local TF listener. + * + * @param[in] target_frame the frame to which data should be transformed + * @param[in] source_frame the frame where the data originated + * @param[in] timeout how long to block before failing + * @return a traverse result indicating if the transform is possible and if it is static + */ + TraverseResult traverseTree( + const std::string & target_frame, const std::string & source_frame, + const tf2::Duration & timeout); + + /** @brief Get a dynamic transform from the TF buffer. + * + * @param[in] target_frame the frame to which data should be transformed + * @param[in] source_frame the frame where the data originated + * @param[in] time the time at which the value of the transform is desired (0 will get the latest) + * @param[in] timeout how long to block before failing + * @return an optional containing the transform if successful, or empty if not + */ + std::optional getDynamicTransform( + const std::string & target_frame, const std::string & source_frame, const tf2::TimePoint & time, + const tf2::Duration & timeout); + + /** @brief Get a static transform from local TF buffer. + * + * @param[in] target_frame the frame to which data should be transformed + * @param[in] source_frame the frame where the data originated + * @return an optional containing the transform if successful, or empty if not + */ + std::optional getStaticTransform( + const std::string & target_frame, const std::string & source_frame); + + /** @brief Get an unknown (static or dynamic) transform. + * + * @param[in] target_frame the frame to which data should be transformed + * @param[in] source_frame the frame where the data originated + * @param[in] time the time at which the value of the transform is desired (0 will get the latest) + * @param[in] timeout how long to block before failing + * @return an optional containing the transform if successful, or empty if not + */ + std::optional getUnknownTransform( + const std::string & target_frame, const std::string & source_frame, const tf2::TimePoint & time, + const tf2::Duration & timeout); + + static std::unique_ptr instance; + rclcpp::Node::SharedPtr node_{nullptr}; + rclcpp::Clock::SharedPtr clock_{nullptr}; + rclcpp::CallbackGroup::SharedPtr callback_group_{nullptr}; + rclcpp::NodeOptions options_; + rclcpp::Subscription::SharedPtr tf_static_sub_{nullptr}; + rclcpp::Subscription::SharedPtr tf_sub_{nullptr}; + rclcpp::SubscriptionOptionsWithAllocator> tf_options_; + rclcpp::SubscriptionOptionsWithAllocator> tf_static_options_; + std::function( + const std::string &, const std::string &, const tf2::TimePoint &, const tf2::Duration &)> + get_transform_; + std::function cb_; + std::function cb_static_; + std::shared_ptr executor_{nullptr}; + std::shared_ptr executor_thread_{nullptr}; + std::unique_ptr tf_buffer_; + std::unique_ptr static_tf_buffer_; + std::unique_ptr tf_tree_; + std::mt19937 random_engine_; + std::uniform_int_distribution<> dis_; + std::mutex mutex_; + bool is_static_{true}; +}; + +} // namespace managed_transform_buffer + +#endif // MANAGED_TRANSFORM_BUFFER__MANAGED_TRANSFORM_BUFFER_PROVIDER_HPP_ diff --git a/managed_transform_buffer/src/managed_transform_buffer.cpp b/managed_transform_buffer/src/managed_transform_buffer.cpp index 5f8c3bc..b75f821 100644 --- a/managed_transform_buffer/src/managed_transform_buffer.cpp +++ b/managed_transform_buffer/src/managed_transform_buffer.cpp @@ -17,61 +17,28 @@ #include "managed_transform_buffer/managed_transform_buffer.hpp" #include -#include #include #include -#include -#include -#include - -#include -#include -#include -#include namespace managed_transform_buffer { ManagedTransformBuffer::ManagedTransformBuffer( rclcpp::Clock::SharedPtr clock, tf2::Duration cache_time) -: clock_(clock) { - executor_ = std::make_shared(); - executor_thread_ = std::make_shared( - std::bind(&rclcpp::executors::SingleThreadedExecutor::spin, executor_)); - - static_tf_buffer_ = std::make_unique(); - tf_tree_ = std::make_unique(); - tf_buffer_ = std::make_unique(clock_, cache_time); - tf_buffer_->setUsingDedicatedThread(true); - tf_options_ = tf2_ros::detail::get_default_transform_listener_sub_options(); - tf_static_options_ = tf2_ros::detail::get_default_transform_listener_static_sub_options(); - cb_ = std::bind(&ManagedTransformBuffer::tfCallback, this, std::placeholders::_1, false); - cb_static_ = std::bind(&ManagedTransformBuffer::tfCallback, this, std::placeholders::_1, true); - options_.start_parameter_event_publisher(false); - options_.start_parameter_services(false); - random_engine_ = std::mt19937(std::random_device{}()); - dis_ = std::uniform_int_distribution<>(0, 0xFFFFFF); - registerAsUnknown(); + provider_ = &ManagedTransformBufferProvider::getInstance(clock, cache_time); } -ManagedTransformBuffer::~ManagedTransformBuffer() -{ - deactivateListener(); - executor_->cancel(); - if (executor_thread_->joinable()) { - executor_thread_->join(); - } -} +ManagedTransformBuffer::~ManagedTransformBuffer() = default; template <> std::optional ManagedTransformBuffer::getTransform( const std::string & target_frame, const std::string & source_frame, const tf2::TimePoint & time, const tf2::Duration & timeout) { - return get_transform_(target_frame, source_frame, time, timeout); + return provider_->getTransform(target_frame, source_frame, time, timeout); } template <> @@ -79,7 +46,7 @@ std::optional ManagedTransformBuffer::getTransformgetTransform(target_frame, source_frame, time, timeout); if (!tf.has_value()) { return std::nullopt; } @@ -93,7 +60,7 @@ std::optional ManagedTransformBuffer::getTransformgetTransform(target_frame, source_frame, time, timeout); if (!tf.has_value()) { return std::nullopt; } @@ -164,277 +131,7 @@ bool ManagedTransformBuffer::transformPointcloud( bool ManagedTransformBuffer::isStatic() const { - return is_static_; -} - -void ManagedTransformBuffer::activateListener() -{ - options_.arguments({"--ros-args", "-r", "__node:=" + generateUniqueNodeName()}); - node_ = rclcpp::Node::make_unique("_", options_); - callback_group_ = - node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); - tf_options_.callback_group = callback_group_; - tf_static_options_.callback_group = callback_group_; - tf_sub_ = node_->create_subscription( - "/tf", tf2_ros::DynamicListenerQoS(), cb_, tf_options_); - tf_static_sub_ = node_->create_subscription( - "/tf_static", tf2_ros::StaticListenerQoS(), cb_static_, tf_static_options_); - executor_->add_callback_group(callback_group_, node_->get_node_base_interface()); -} - -void ManagedTransformBuffer::deactivateListener() -{ - auto cb_grps = executor_->get_all_callback_groups(); - for (auto & cb_grp : cb_grps) { - executor_->remove_callback_group(cb_grp.lock()); - } - tf_static_sub_.reset(); - tf_sub_.reset(); - node_.reset(); -} - -void ManagedTransformBuffer::registerAsUnknown() -{ - get_transform_ = [this]( - const std::string & target_frame, const std::string & source_frame, - const tf2::TimePoint & time, - const tf2::Duration & timeout) -> std::optional { - return getUnknownTransform(target_frame, source_frame, time, timeout); - }; -} - -void ManagedTransformBuffer::registerAsDynamic() -{ - is_static_ = false; - get_transform_ = [this]( - const std::string & target_frame, const std::string & source_frame, - const tf2::TimePoint & time, - const tf2::Duration & timeout) -> std::optional { - if (!node_) { - activateListener(); - } - return getDynamicTransform(target_frame, source_frame, time, timeout); - }; -} - -std::string ManagedTransformBuffer::generateUniqueNodeName() -{ - std::stringstream sstream; - sstream << "managed_tf_listener_impl_" << std::hex << dis_(random_engine_) - << dis_(random_engine_); - return sstream.str(); -} - -void ManagedTransformBuffer::tfCallback( - const tf2_msgs::msg::TFMessage::SharedPtr msg, const bool is_static) -{ - std::string authority = "Authority undetectable"; - for (const auto & tf : msg->transforms) { - try { - tf_buffer_->setTransform(tf, authority, is_static); - tf_tree_->emplace(tf.child_frame_id, TreeNode{tf.header.frame_id, is_static}); - } catch (const tf2::TransformException & ex) { - if (node_) { - RCLCPP_ERROR( - node_->get_logger(), "Failure to set received transform from %s to %s with error: %s\n", - tf.child_frame_id.c_str(), tf.header.frame_id.c_str(), ex.what()); - } - } - } -} - -std::optional ManagedTransformBuffer::lookupTransform( - const std::string & target_frame, const std::string & source_frame, const tf2::TimePoint & time, - const tf2::Duration & timeout) const -{ - try { - auto tf = tf_buffer_->lookupTransform(target_frame, source_frame, time, timeout); - return std::make_optional(tf); - } catch (const tf2::TransformException & ex) { - if (node_) { - RCLCPP_ERROR( - node_->get_logger(), "Failure to get transform from %s to %s with error: %s", - target_frame.c_str(), source_frame.c_str(), ex.what()); - } - return std::nullopt; - } -} - -TraverseResult ManagedTransformBuffer::traverseTree( - const std::string & target_frame, const std::string & source_frame, const tf2::Duration & timeout) -{ - std::atomic timeout_reached{false}; - - auto framesToRoot = [this]( - const std::string & start_frame, TreeMap & last_tf_tree, - std::vector & frames_out) -> bool { - frames_out.push_back(start_frame); - uint32_t depth = 0; - auto current_frame = start_frame; - auto frame_it = last_tf_tree.find(current_frame); - while (frame_it != last_tf_tree.end()) { - current_frame = frame_it->second.parent; - frames_out.push_back(current_frame); - frame_it = last_tf_tree.find(current_frame); - depth++; - if (depth > tf2::BufferCore::MAX_GRAPH_DEPTH) { - if (node_) { - RCLCPP_ERROR(node_->get_logger(), "Traverse depth exceeded for %s", start_frame.c_str()); - } - return false; - } - } - return true; - }; - - auto traverse = [this, &timeout_reached, &framesToRoot]( - const std::string & t1, const std::string & t2) -> TraverseResult { - while (!timeout_reached) { - std::vector frames_from_t1; - std::vector frames_from_t2; - auto last_tf_tree = *tf_tree_; - - // Collect all frames from bottom to the top of TF tree for both frames - if ( - !framesToRoot(t1, last_tf_tree, frames_from_t1) || - !framesToRoot(t2, last_tf_tree, frames_from_t2)) { - // Possibly TF loop occurred - return {false, false}; - } - - // Find common ancestor - bool only_static_requested_from_t1 = true; - bool only_static_requested_from_t2 = true; - for (auto & frame_from_t1 : frames_from_t1) { - auto frame_from_t1_it = last_tf_tree.find(frame_from_t1); - if (frame_from_t1_it != last_tf_tree.end()) { // Otherwise frame is TF root (no parent) - only_static_requested_from_t1 &= last_tf_tree.at(frame_from_t1).is_static; - } - for (auto & frame_from_t2 : frames_from_t2) { - auto frame_from_t2_it = last_tf_tree.find(frame_from_t2); - if (frame_from_t2_it != last_tf_tree.end()) { // Otherwise frame is TF root (no parent) - only_static_requested_from_t2 &= last_tf_tree.at(frame_from_t2).is_static; - } - if (frame_from_t1 == frame_from_t2) { - return {true, only_static_requested_from_t1 && only_static_requested_from_t2}; - } - } - only_static_requested_from_t1 = true; - only_static_requested_from_t2 = true; - } - } - // Timeout reached - return {false, false}; - }; - - std::future future = - std::async(std::launch::async, traverse, target_frame, source_frame); - if (future.wait_for(timeout) == std::future_status::ready) { - return future.get(); - } - timeout_reached = true; - return {false, false}; -} - -std::optional ManagedTransformBuffer::getDynamicTransform( - const std::string & target_frame, const std::string & source_frame, const tf2::TimePoint & time, - const tf2::Duration & timeout) -{ - if (!node_) { - activateListener(); - } - return lookupTransform(target_frame, source_frame, time, timeout); -} - -std::optional ManagedTransformBuffer::getStaticTransform( - const std::string & target_frame, const std::string & source_frame) -{ - auto key = std::make_pair(target_frame, source_frame); - auto key_inv = std::make_pair(source_frame, target_frame); - - // Check if the transform is already in the buffer - auto it = static_tf_buffer_->find(key); - if (it != static_tf_buffer_->end()) { - auto tf_msg = it->second; - tf_msg.header.stamp = clock_->now(); - return std::make_optional(tf_msg); - } - - // Check if the inverse transform is already in the buffer - auto it_inv = static_tf_buffer_->find(key_inv); - if (it_inv != static_tf_buffer_->end()) { - auto tf_msg = it_inv->second; - tf2::Transform tf; - tf2::fromMsg(tf_msg.transform, tf); - tf2::Transform inv_tf = tf.inverse(); - TransformStamped inv_tf_msg; - inv_tf_msg.transform = tf2::toMsg(inv_tf); - inv_tf_msg.header.frame_id = tf_msg.child_frame_id; - inv_tf_msg.child_frame_id = tf_msg.header.frame_id; - inv_tf_msg.header.stamp = clock_->now(); - static_tf_buffer_->emplace(key, inv_tf_msg); - return std::make_optional(inv_tf_msg); - } - - // Check if transform is needed - if (target_frame == source_frame) { - auto tf_identity = tf2::Transform::getIdentity(); - TransformStamped tf_msg; - tf_msg.transform = tf2::toMsg(tf_identity); - tf_msg.header.frame_id = target_frame; - tf_msg.child_frame_id = source_frame; - tf_msg.header.stamp = clock_->now(); - static_tf_buffer_->emplace(key, tf_msg); - return std::make_optional(tf_msg); - } - - return std::nullopt; -} - -std::optional ManagedTransformBuffer::getUnknownTransform( - const std::string & target_frame, const std::string & source_frame, const tf2::TimePoint & time, - const tf2::Duration & timeout) -{ - // Try to get transform from local static buffer - auto static_tf = getStaticTransform(target_frame, source_frame); - if (static_tf.has_value()) { - return static_tf; - } - - // Initialize local TF listener and base TF listener - activateListener(); - - // Check local TF tree and TF buffer asynchronously - std::future traverse_future = std::async( - std::launch::async, &ManagedTransformBuffer::traverseTree, this, target_frame, source_frame, - timeout); - std::future> tf_future = std::async( - std::launch::async, &ManagedTransformBuffer::lookupTransform, this, target_frame, source_frame, - time, timeout); - auto traverse_result = traverse_future.get(); - auto tf = tf_future.get(); - - // Mimic lookup transform error if TF not exists in tree or buffer - if (!traverse_result.success || !tf.has_value()) { - deactivateListener(); - return std::nullopt; - } - - // If TF is static, add it to the static buffer. Otherwise, switch to dynamic listener - if (traverse_result.is_static) { - auto key = std::make_pair(target_frame, source_frame); - static_tf_buffer_->emplace(key, tf.value()); - deactivateListener(); - } else { - registerAsDynamic(); - if (node_) { - RCLCPP_INFO( - node_->get_logger(), "Transform %s -> %s is dynamic. Switching to dynamic listener.", - target_frame.c_str(), source_frame.c_str()); - } - } - - return tf; + return provider_->isStatic(); } } // namespace managed_transform_buffer diff --git a/managed_transform_buffer/src/managed_transform_buffer_provider.cpp b/managed_transform_buffer/src/managed_transform_buffer_provider.cpp new file mode 100644 index 0000000..e283b53 --- /dev/null +++ b/managed_transform_buffer/src/managed_transform_buffer_provider.cpp @@ -0,0 +1,361 @@ +// Copyright 2024 The Autoware Foundation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Co-developed by Tier IV, Inc. + +#include "managed_transform_buffer/managed_transform_buffer_provider.hpp" + +#include +#include +#include + +#include + +#include + +#include +#include +#include + +namespace managed_transform_buffer +{ + +std::unique_ptr ManagedTransformBufferProvider::instance = nullptr; + +ManagedTransformBufferProvider & ManagedTransformBufferProvider::getInstance( + rclcpp::Clock::SharedPtr clock, tf2::Duration cache_time) +{ + static ManagedTransformBufferProvider instance(clock, cache_time); + return instance; +} + +std::optional ManagedTransformBufferProvider::getTransform( + const std::string & target_frame, const std::string & source_frame, const tf2::TimePoint & time, + const tf2::Duration & timeout) +{ + return get_transform_(target_frame, source_frame, time, timeout); +} + +bool ManagedTransformBufferProvider::isStatic() const +{ + return is_static_; +} + +ManagedTransformBufferProvider::ManagedTransformBufferProvider( + rclcpp::Clock::SharedPtr clock, tf2::Duration cache_time) +: clock_(clock) +{ + executor_ = std::make_shared(); + executor_thread_ = std::make_shared( + std::bind(&rclcpp::executors::SingleThreadedExecutor::spin, executor_)); + + static_tf_buffer_ = std::make_unique(); + tf_tree_ = std::make_unique(); + tf_buffer_ = std::make_unique(clock_, cache_time); + tf_buffer_->setUsingDedicatedThread(true); + tf_options_ = tf2_ros::detail::get_default_transform_listener_sub_options(); + tf_static_options_ = tf2_ros::detail::get_default_transform_listener_static_sub_options(); + cb_ = std::bind(&ManagedTransformBufferProvider::tfCallback, this, std::placeholders::_1, false); + cb_static_ = + std::bind(&ManagedTransformBufferProvider::tfCallback, this, std::placeholders::_1, true); + options_.start_parameter_event_publisher(false); + options_.start_parameter_services(false); + random_engine_ = std::mt19937(std::random_device{}()); + dis_ = std::uniform_int_distribution<>(0, 0xFFFFFF); + registerAsUnknown(); +} + +ManagedTransformBufferProvider::~ManagedTransformBufferProvider() +{ + deactivateListener(); + executor_->cancel(); + if (executor_thread_->joinable()) { + executor_thread_->join(); + } +} + +void ManagedTransformBufferProvider::activateListener() +{ + std::lock_guard lock(mutex_); + options_.arguments({"--ros-args", "-r", "__node:=" + generateUniqueNodeName()}); + node_ = rclcpp::Node::make_unique("_", options_); + callback_group_ = + node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); + tf_options_.callback_group = callback_group_; + tf_options_.use_intra_process_comm = rclcpp::IntraProcessSetting::Enable; + tf_static_options_.callback_group = callback_group_; + tf_sub_ = node_->create_subscription( + "/tf", tf2_ros::DynamicListenerQoS(), cb_, tf_options_); + tf_static_sub_ = node_->create_subscription( + "/tf_static", tf2_ros::StaticListenerQoS(), cb_static_, tf_static_options_); + executor_->add_callback_group(callback_group_, node_->get_node_base_interface()); +} + +void ManagedTransformBufferProvider::deactivateListener() +{ + std::lock_guard lock(mutex_); + auto cb_grps = executor_->get_all_callback_groups(); + for (auto & cb_grp : cb_grps) { + executor_->remove_callback_group(cb_grp.lock()); + } + tf_static_sub_.reset(); + tf_sub_.reset(); + node_.reset(); +} + +void ManagedTransformBufferProvider::registerAsUnknown() +{ + get_transform_ = [this]( + const std::string & target_frame, const std::string & source_frame, + const tf2::TimePoint & time, + const tf2::Duration & timeout) -> std::optional { + return getUnknownTransform(target_frame, source_frame, time, timeout); + }; +} + +void ManagedTransformBufferProvider::registerAsDynamic() +{ + is_static_ = false; + get_transform_ = [this]( + const std::string & target_frame, const std::string & source_frame, + const tf2::TimePoint & time, + const tf2::Duration & timeout) -> std::optional { + if (!node_) { + activateListener(); + } + return getDynamicTransform(target_frame, source_frame, time, timeout); + }; +} + +std::string ManagedTransformBufferProvider::generateUniqueNodeName() +{ + std::stringstream sstream; + sstream << "managed_tf_listener_impl_" << std::hex << dis_(random_engine_) + << dis_(random_engine_); + return sstream.str(); +} + +void ManagedTransformBufferProvider::tfCallback( + const tf2_msgs::msg::TFMessage::SharedPtr msg, const bool is_static) +{ + std::string authority = "Authority undetectable"; + for (const auto & tf : msg->transforms) { + try { + tf_buffer_->setTransform(tf, authority, is_static); + tf_tree_->emplace(tf.child_frame_id, TreeNode{tf.header.frame_id, is_static}); + } catch (const tf2::TransformException & ex) { + if (node_) { + RCLCPP_ERROR( + node_->get_logger(), "Failure to set received transform from %s to %s with error: %s\n", + tf.child_frame_id.c_str(), tf.header.frame_id.c_str(), ex.what()); + } + } + } +} + +std::optional ManagedTransformBufferProvider::lookupTransform( + const std::string & target_frame, const std::string & source_frame, const tf2::TimePoint & time, + const tf2::Duration & timeout) const +{ + try { + auto tf = tf_buffer_->lookupTransform(target_frame, source_frame, time, timeout); + return std::make_optional(tf); + } catch (const tf2::TransformException & ex) { + if (node_) { + RCLCPP_ERROR( + node_->get_logger(), "Failure to get transform from %s to %s with error: %s", + target_frame.c_str(), source_frame.c_str(), ex.what()); + } + return std::nullopt; + } +} + +TraverseResult ManagedTransformBufferProvider::traverseTree( + const std::string & target_frame, const std::string & source_frame, const tf2::Duration & timeout) +{ + std::atomic timeout_reached{false}; + + auto framesToRoot = [this]( + const std::string & start_frame, TreeMap & last_tf_tree, + std::vector & frames_out) -> bool { + frames_out.push_back(start_frame); + uint32_t depth = 0; + auto current_frame = start_frame; + auto frame_it = last_tf_tree.find(current_frame); + while (frame_it != last_tf_tree.end()) { + current_frame = frame_it->second.parent; + frames_out.push_back(current_frame); + frame_it = last_tf_tree.find(current_frame); + depth++; + if (depth > tf2::BufferCore::MAX_GRAPH_DEPTH) { + if (node_) { + RCLCPP_ERROR(node_->get_logger(), "Traverse depth exceeded for %s", start_frame.c_str()); + } + return false; + } + } + return true; + }; + + auto traverse = [this, &timeout_reached, &framesToRoot]( + const std::string & t1, const std::string & t2) -> TraverseResult { + while (!timeout_reached) { + std::vector frames_from_t1; + std::vector frames_from_t2; + auto last_tf_tree = *tf_tree_; + + // Collect all frames from bottom to the top of TF tree for both frames + if ( + !framesToRoot(t1, last_tf_tree, frames_from_t1) || + !framesToRoot(t2, last_tf_tree, frames_from_t2)) { + // Possibly TF loop occurred + return {false, false}; + } + + // Find common ancestor + bool only_static_requested_from_t1 = true; + bool only_static_requested_from_t2 = true; + for (auto & frame_from_t1 : frames_from_t1) { + auto frame_from_t1_it = last_tf_tree.find(frame_from_t1); + if (frame_from_t1_it != last_tf_tree.end()) { // Otherwise frame is TF root (no parent) + only_static_requested_from_t1 &= last_tf_tree.at(frame_from_t1).is_static; + } + for (auto & frame_from_t2 : frames_from_t2) { + auto frame_from_t2_it = last_tf_tree.find(frame_from_t2); + if (frame_from_t2_it != last_tf_tree.end()) { // Otherwise frame is TF root (no parent) + only_static_requested_from_t2 &= last_tf_tree.at(frame_from_t2).is_static; + } + if (frame_from_t1 == frame_from_t2) { + return {true, only_static_requested_from_t1 && only_static_requested_from_t2}; + } + } + only_static_requested_from_t1 = true; + only_static_requested_from_t2 = true; + } + } + // Timeout reached + return {false, false}; + }; + + std::future future = + std::async(std::launch::async, traverse, target_frame, source_frame); + if (future.wait_for(timeout) == std::future_status::ready) { + return future.get(); + } + timeout_reached = true; + return {false, false}; +} + +std::optional ManagedTransformBufferProvider::getDynamicTransform( + const std::string & target_frame, const std::string & source_frame, const tf2::TimePoint & time, + const tf2::Duration & timeout) +{ + if (!node_) { + activateListener(); + } + return lookupTransform(target_frame, source_frame, time, timeout); +} + +std::optional ManagedTransformBufferProvider::getStaticTransform( + const std::string & target_frame, const std::string & source_frame) +{ + auto key = std::make_pair(target_frame, source_frame); + auto key_inv = std::make_pair(source_frame, target_frame); + + // Check if the transform is already in the buffer + auto it = static_tf_buffer_->find(key); + if (it != static_tf_buffer_->end()) { + auto tf_msg = it->second; + tf_msg.header.stamp = clock_->now(); + return std::make_optional(tf_msg); + } + + // Check if the inverse transform is already in the buffer + auto it_inv = static_tf_buffer_->find(key_inv); + if (it_inv != static_tf_buffer_->end()) { + auto tf_msg = it_inv->second; + tf2::Transform tf; + tf2::fromMsg(tf_msg.transform, tf); + tf2::Transform inv_tf = tf.inverse(); + TransformStamped inv_tf_msg; + inv_tf_msg.transform = tf2::toMsg(inv_tf); + inv_tf_msg.header.frame_id = tf_msg.child_frame_id; + inv_tf_msg.child_frame_id = tf_msg.header.frame_id; + inv_tf_msg.header.stamp = clock_->now(); + static_tf_buffer_->emplace(key, inv_tf_msg); + return std::make_optional(inv_tf_msg); + } + + // Check if transform is needed + if (target_frame == source_frame) { + auto tf_identity = tf2::Transform::getIdentity(); + TransformStamped tf_msg; + tf_msg.transform = tf2::toMsg(tf_identity); + tf_msg.header.frame_id = target_frame; + tf_msg.child_frame_id = source_frame; + tf_msg.header.stamp = clock_->now(); + static_tf_buffer_->emplace(key, tf_msg); + return std::make_optional(tf_msg); + } + + return std::nullopt; +} + +std::optional ManagedTransformBufferProvider::getUnknownTransform( + const std::string & target_frame, const std::string & source_frame, const tf2::TimePoint & time, + const tf2::Duration & timeout) +{ + // Try to get transform from local static buffer + auto static_tf = getStaticTransform(target_frame, source_frame); + if (static_tf.has_value()) { + return static_tf; + } + + // Initialize local TF listener and base TF listener + activateListener(); + + // Check local TF tree and TF buffer asynchronously + std::future traverse_future = std::async( + std::launch::async, &ManagedTransformBufferProvider::traverseTree, this, target_frame, + source_frame, timeout); + std::future> tf_future = std::async( + std::launch::async, &ManagedTransformBufferProvider::lookupTransform, this, target_frame, + source_frame, time, timeout); + auto traverse_result = traverse_future.get(); + auto tf = tf_future.get(); + + // Mimic lookup transform error if TF not exists in tree or buffer + if (!traverse_result.success || !tf.has_value()) { + deactivateListener(); + return std::nullopt; + } + + // If TF is static, add it to the static buffer. Otherwise, switch to dynamic listener + if (traverse_result.is_static) { + auto key = std::make_pair(target_frame, source_frame); + static_tf_buffer_->emplace(key, tf.value()); + deactivateListener(); + } else { + registerAsDynamic(); + if (node_) { + RCLCPP_INFO( + node_->get_logger(), "Transform %s -> %s is dynamic. Switching to dynamic listener.", + target_frame.c_str(), source_frame.c_str()); + } + } + + return tf; +} + +} // namespace managed_transform_buffer diff --git a/managed_transform_buffer/test/test_managed_transform_buffer.cpp b/managed_transform_buffer/test/test_managed_transform_buffer.cpp index 2d48969..1d16d16 100644 --- a/managed_transform_buffer/test/test_managed_transform_buffer.cpp +++ b/managed_transform_buffer/test/test_managed_transform_buffer.cpp @@ -241,6 +241,8 @@ TEST_F(TestManagedTransformBuffer, TestTransformDynamic) TEST_F(TestManagedTransformBuffer, TestTransformMultipleCall) { + EXPECT_FALSE(managed_tf_buffer_->isStatic()); + static_tf_broadcaster_->sendTransform(tf_base_to_lidar_top_); std::optional eigen_transform; @@ -271,7 +273,6 @@ TEST_F(TestManagedTransformBuffer, TestTransformMultipleCall) managed_tf_buffer_->getTransform("base_link", "lidar_top", time_, timeout_); ASSERT_TRUE(eigen_transform.has_value()); EXPECT_TRUE(eigen_transform.value().isApprox(eigen_base_to_lidar_top_, precision_)); - EXPECT_TRUE(managed_tf_buffer_->isStatic()); std::future future = std::async(std::launch::async, [this]() { broadcastDynamicTf(tf_map_to_base_); }); @@ -280,7 +281,6 @@ TEST_F(TestManagedTransformBuffer, TestTransformMultipleCall) future.wait(); ASSERT_TRUE(eigen_map_to_base.has_value()); EXPECT_TRUE(eigen_map_to_base.value().isApprox(eigen_map_to_base_, precision_)); - EXPECT_FALSE(managed_tf_buffer_->isStatic()); eigen_transform = managed_tf_buffer_->getTransform("fake_link", "fake_link", time_, timeout_); From 831bbe6c14f8cbf2f4dc5d45fb488c2495802c03 Mon Sep 17 00:00:00 2001 From: Amadeusz Szymko Date: Tue, 25 Feb 2025 19:57:11 +0900 Subject: [PATCH 3/3] fix: typo Signed-off-by: Amadeusz Szymko --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 2a518d3..86f9f7f 100644 --- a/README.md +++ b/README.md @@ -50,4 +50,4 @@ ros2 run managed_transform_buffer example_managed_transform_buffer --ros-args -p ## Limitations -- Requests for dynamic transforms with zero timeout might never succeed. This limitation is due to the fact that the listener is initialized for each transform request (till first occurence of dynamic transform). If timeout is zero, the listener might not have enought time to fill the buffer. +- Requests for dynamic transforms with zero timeout might never succeed. This limitation is due to the fact that the listener is initialized for each transform request (till first occurrence of dynamic transform). If timeout is zero, the listener might not have enough time to fill the buffer.