Skip to content

Commit

Permalink
feat: use singleton pattern
Browse files Browse the repository at this point in the history 
Signed-off-by: Amadeusz Szymko <[email protected]>
  • Loading branch information
@amadeuszsz
amadeuszsz committed Feb 25, 2025
1 parent 9cb4145 commit 759d5ec
Show file tree
Hide file tree
Showing 7 changed files with 621 additions and 476 deletions.
19 changes: 12 additions & 7 deletions README.md
View file
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 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
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 occurence of dynamic transform). If timeout is zero, the listener might not have enought time to fill the buffer.

Check warning on line 53 in README.md

View workflow job for this annotation

GitHub Actions / spell-check-differential 

Misspelled word (occurence) Suggestions: (occurrence*)

Check warning on line 53 in README.md

View workflow job for this annotation

GitHub Actions / spell-check-differential 

Unknown word (enought)
1 change: 1 addition & 0 deletions managed_transform_buffer/CMakeLists.txt
View file
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
162 changes: 4 additions & 158 deletions managed_transform_buffer/include/managed_transform_buffer/managed_transform_buffer.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 <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 <chrono>
#include <functional>
#include <memory>
#include <optional>
#include <random>
#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 Down Expand Up @@ -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<TransformStamped> 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<TransformStamped> 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<TransformStamped> 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<TransformStamped> 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<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_;
std::function<std::optional<TransformStamped>(
const std::string &, const std::string &, const tf2::TimePoint &, const tf2::Duration &)>
get_transform_;
std::function<void(tf2_msgs::msg::TFMessage::SharedPtr)> cb_;
std::function<void(tf2_msgs::msg::TFMessage::SharedPtr)> cb_static_;
std::shared_ptr<rclcpp::executors::SingleThreadedExecutor> executor_{nullptr};
std::shared_ptr<std::thread> executor_thread_{nullptr};
std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
std::unique_ptr<TFMap> static_tf_buffer_;
std::unique_ptr<TreeMap> tf_tree_;
std::mt19937 random_engine_;
std::uniform_int_distribution<> dis_;
bool is_static_{true};
ManagedTransformBufferProvider * provider_;
};

} // namespace managed_transform_buffer
Expand Down
Loading

0 comments on commit 759d5ec

Please sign in to comment.