Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

perf: use single TF listener & add singleton #5

Merged
merged 3 commits into from
Feb 25, 2025
Merged

perf: use single TF listener & add singleton #5

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
9cb4145
feat: reduce impl to single temp listener
amadeuszsz Feb 25, 2025
759d5ec
feat: use singleton pattern
amadeuszsz Feb 25, 2025
831bbe6
fix: typo
amadeuszsz Feb 25, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
19 changes: 12 additions & 7 deletions README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,11 +2,12 @@

## Purpose

This package contains a wrapper of ROS 2 TF buffer & listener. It offers better pefromance
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
Expand All @@ -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<ManagedTFBuffer>(node);
auto managed_tf_buffer = std::make_unique<ManagedTFBuffer>(this->get_clock());

// Get a transform from source_frame to target_frame
auto tf_msg_transform = managed_tf_buffer->getTransform<geometry_msgs::msg::TransformStamped>("my_target_frame", "my_source_frame");
auto tf2_transform = managed_tf_buffer->getTransform<tf2::Transform>("my_target_frame", "my_source_frame");
auto eigen_transform = managed_tf_buffer->getTransform<Eigen::Matrix4f>("my_target_frame", "my_source_frame");
auto tf_msg_transform = managed_tf_buffer->getTransform<geometry_msgs::msg::TransformStamped>("my_target_frame", "my_source_frame", this->now(), rclcpp::Duration::from_seconds(1));
auto tf2_transform = managed_tf_buffer->getTransform<tf2::Transform>("my_target_frame", "my_source_frame", this->now(), rclcpp::Duration::from_seconds(1));
auto eigen_transform = managed_tf_buffer->getTransform<Eigen::Matrix4f>("my_target_frame", "my_source_frame", this->now(), rclcpp::Duration::from_seconds(1));

if (tf_msg_transform.has_value())
{
Expand All @@ -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).
Expand All @@ -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 occurrence of dynamic transform). If timeout is zero, the listener might not have enough time to fill the buffer.
1 change: 1 addition & 0 deletions managed_transform_buffer/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
9 changes: 6 additions & 3 deletions managed_transform_buffer/examples/example_managed_transform_buffer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,8 @@ class ExampleNode : public rclcpp::Node
{
target_frame_ = declare_parameter<std::string>("target_frame", "dummy_target_frame");
source_frame_ = declare_parameter<std::string>("source_frame", "dummy_source_frame");
managed_tf_buffer_ = std::make_unique<managed_transform_buffer::ManagedTransformBuffer>(this);
managed_tf_buffer_ =
std::make_unique<managed_transform_buffer::ManagedTransformBuffer>(this->get_clock());
cloud_sub_ = create_subscription<sensor_msgs::msg::PointCloud2>(
"input/cloud", rclcpp::SensorDataQoS(),
std::bind(&ExampleNode::cloud_cb, this, std::placeholders::_1));
Expand All @@ -50,15 +51,17 @@ 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);
}
}

void timer_cb()
{
auto tf = managed_tf_buffer_->getTransform<Eigen::Matrix4f>(target_frame_, source_frame_);
auto tf = managed_tf_buffer_->getTransform<Eigen::Matrix4f>(
target_frame_, source_frame_, this->now(), rclcpp::Duration::from_seconds(1));

if (tf.has_value()) {
RCLCPP_INFO(
Expand Down
218 changes: 53 additions & 165 deletions managed_transform_buffer/include/managed_transform_buffer/managed_transform_buffer.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,75 +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 <eigen3/Eigen/Core>
#include <rclcpp/rclcpp.hpp>
#include <tf2/LinearMath/Transform.hpp>

#include <geometry_msgs/msg/transform_stamped.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <tf2_msgs/msg/tf_message.hpp>

#include <tf2/LinearMath/Transform.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>

#include <chrono>
#include <functional>
#include <memory>
#include <optional>
#include <string>
#include <thread>
#include <type_traits>
#include <unordered_map>
#include <utility>

namespace std
{
template <>
struct hash<std::pair<std::string, std::string>>
{
size_t operator()(const std::pair<std::string, std::string> & p) const
{
size_t h1 = std::hash<std::string>{}(p.first);
size_t h2 = std::hash<std::string>{}(p.second);
return h1 ^ (h2 << 1u);
}
};
} // namespace std

namespace managed_transform_buffer
{
using Key = std::pair<std::string, std::string>;
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<Key, TransformStamped, std::hash<Key>, PairEqual>;
using TreeMap = std::unordered_map<std::string, TreeNode>;

/**
* @brief A managed TF buffer that handles listener node lifetime. This buffer triggers listener
Expand All @@ -99,10 +47,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();
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

⚠️ performance-trivially-destructible ⚠️
class ManagedTransformBuffer can be made trivially destructible by defaulting the destructor on its first declaration

Suggested change
~ManagedTransformBuffer();
~ManagedTransformBuffer() = default;

Expand All @@ -129,136 +79,74 @@ class ManagedTransformBuffer
template <typename T = TransformStamped>
std::enable_if_t<std::is_same_v<T, TransformStamped>, std::optional<TransformStamped>>
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 <typename T = Eigen::Matrix4f>
std::enable_if_t<std::is_same_v<T, Eigen::Matrix4f>, std::optional<Eigen::Matrix4f>> getTransform(
const std::string & target_frame, const std::string & source_frame, const tf2::TimePoint & time,
const tf2::Duration & timeout);

template <typename T = tf2::Transform>
std::enable_if_t<std::is_same_v<T, tf2::Transform>, std::optional<tf2::Transform>> getTransform(
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 <typename T = TransformStamped>
std::enable_if_t<std::is_same_v<T, TransformStamped>, std::optional<TransformStamped>>
getTransform(
const std::string & target_frame, const std::string & source_frame, const rclcpp::Time & time,
const rclcpp::Duration & timeout);

template <typename T = Eigen::Matrix4f>
std::enable_if_t<std::is_same_v<T, Eigen::Matrix4f>, std::optional<Eigen::Matrix4f>> 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 rclcpp::Time & time,
const rclcpp::Duration & timeout);

template <typename T = tf2::Transform>
std::enable_if_t<std::is_same_v<T, tf2::Transform>, std::optional<tf2::Transform>> 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 rclcpp::Time & time,
const rclcpp::Duration & timeout);

/**
* @brief Transforms a point cloud from one frame to another.
*
* @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);

private:
/** @brief Initialize TF listener used for storing transforms */
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 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);
sensor_msgs::msg::PointCloud2 & cloud_out, const tf2::TimePoint & time,
const tf2::Duration & timeout);

/** @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<TransformStamped> 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;

/** @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 rclcpp::Duration & timeout = default_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<TransformStamped> getDynamicTransform(
const std::string & target_frame, const std::string & source_frame,
const rclcpp::Time & time = rclcpp::Time(0),
const rclcpp::Duration & timeout = default_timeout);

/** @brief Get a static transform from local TF buffer.
/**
* @brief Transforms a point cloud from one frame to another.
*
* @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
* @sa transformPointcloud(const std::string &, const sensor_msgs::msg::PointCloud2 &,
* sensor_msgs::msg::PointCloud2 &, const tf2::TimePoint &, const tf2::Duration)
*/
std::optional<TransformStamped> getStaticTransform(
const std::string & target_frame, const std::string & source_frame);
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 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
/** @brief Check if all TFs requests have been for static TF so far.
* @return true if only static TFs have been requested
*/
std::optional<TransformStamped> getUnknownTransform(
const std::string & target_frame, const std::string & source_frame,
const rclcpp::Time & time = rclcpp::Time(0),
const rclcpp::Duration & timeout = default_timeout);
bool isStatic() const;

rclcpp::CallbackGroup::SharedPtr callback_group_{nullptr};
rclcpp::Node * const node_;
rclcpp::Node::SharedPtr managed_listener_node_{nullptr};
rclcpp::NodeOptions options_;
rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr tf_static_sub_{nullptr};
rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr tf_sub_{nullptr};
rclcpp::SubscriptionOptionsWithAllocator<std::allocator<void>> tf_options_;
rclcpp::SubscriptionOptionsWithAllocator<std::allocator<void>> tf_static_options_;
rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_{nullptr};
std::function<std::optional<TransformStamped>(
const std::string &, const std::string &, const rclcpp::Time &, const rclcpp::Duration &)>
get_transform_;
std::function<void(tf2_msgs::msg::TFMessage::SharedPtr)> cb_;
std::function<void(tf2_msgs::msg::TFMessage::SharedPtr)> cb_static_;
std::unique_ptr<std::thread> dedicated_listener_thread_;
std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
std::unique_ptr<tf2_ros::TransformListener> tf_listener_;
std::unique_ptr<TFMap> static_tf_buffer_;
std::unique_ptr<TreeMap> tf_tree_;
static std::chrono::milliseconds default_timeout;
private:
ManagedTransformBufferProvider * provider_;
};

} // namespace managed_transform_buffer
Expand Down
Loading
Loading