feat: rework constructor, expand API, improve logging, fix sync (#7)

Signed-off-by: Amadeusz Szymko <amadeusz.szymko.2@tier4.jp>

README.md

@@ -23,7 +23,7 @@ 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>(this->get_clock());
+auto managed_tf_buffer = std::make_unique<managed_transform_buffer::ManagedTransformBuffer>();
 
 // 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", this->now(), rclcpp::Duration::from_seconds(1));
@@ -50,4 +50,8 @@ 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 occurrence of dynamic transform). If timeout is zero, the listener might not have enough 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 will not have enough time to fill the buffer. This can be controlled with `discovery_timeout` parameter for `ManagedTransformBuffer` class constructor. `discovery_timeout` (default: 20ms) is used to set timeout for the first occurrence of any transform:
+
+```cpp
+auto managed_tf_buffer = std::make_unique<managed_transform_buffer::ManagedTransformBuffer>(RCL_ROS_TIME, false, tf2::durationFromSec(0.3));
+```

managed_transform_buffer/examples/example_managed_transform_buffer.cpp

@@ -30,8 +30,7 @@ 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->get_clock());
+    managed_tf_buffer_ = std::make_unique<managed_transform_buffer::ManagedTransformBuffer>();
     cloud_sub_ = create_subscription<sensor_msgs::msg::PointCloud2>(
       "input/cloud", rclcpp::SensorDataQoS(),
       std::bind(&ExampleNode::cloud_cb, this, std::placeholders::_1));

managed_transform_buffer/include/managed_transform_buffer/managed_transform_buffer.hpp

@@ -27,13 +27,15 @@
 #include <sensor_msgs/msg/point_cloud2.hpp>
 #include <tf2_msgs/msg/tf_message.hpp>
 
+#include <chrono>
 #include <optional>
 #include <string>
 #include <type_traits>
 
 namespace managed_transform_buffer
 {
 using geometry_msgs::msg::TransformStamped;
+constexpr tf2::Duration DISCOVERY_TIMEOUT = tf2::Duration(std::chrono::milliseconds(20));
 
 /**
  * @brief A managed TF buffer that handles listener node lifetime. This buffer triggers listener
@@ -47,11 +49,14 @@ class ManagedTransformBuffer
   /**
    * @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
+   * @param[in] clock_type type of the clock
+   * @param[in] force_dynamic if true, TF listener will be activated during initialization
+   * @param[in] discovery_timeout how long to wait for first TF discovery
+   * @param[in] cache_time how long to keep a history of transforms
    */
   explicit ManagedTransformBuffer(
-    rclcpp::Clock::SharedPtr clock,
+    rcl_clock_type_t clock_type = RCL_ROS_TIME, const bool force_dynamic = false,
+    tf2::Duration discovery_timeout = tf2::Duration(managed_transform_buffer::DISCOVERY_TIMEOUT),
     tf2::Duration cache_time = tf2::Duration(tf2::BUFFER_CORE_DEFAULT_CACHE_TIME));
 
   /** @brief Destroy the Managed Transform Buffer object */
@@ -65,6 +70,7 @@ class ManagedTransformBuffer
    * @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
+   * @param[in] logger logger, if not specified, default logger will be used
    * @return an optional containing the transform if successful, or empty if not
    *
    * @overload getTransform<geometry_msgs::msg::TransformStamped>
@@ -80,17 +86,17 @@ class ManagedTransformBuffer
   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 tf2::TimePoint & time,
-    const tf2::Duration & timeout);
+    const tf2::Duration & timeout, const rclcpp::Logger & logger = defaultLogger());
 
   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);
+    const tf2::Duration & timeout, const rclcpp::Logger & logger = defaultLogger());
 
   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);
+    const tf2::Duration & timeout, const rclcpp::Logger & logger = defaultLogger());
 
   /**
    * @brief Get the transform between two frames by frame ID.
@@ -102,17 +108,53 @@ class ManagedTransformBuffer
   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);
+    const rclcpp::Duration & timeout, const rclcpp::Logger & logger = defaultLogger());
 
   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,
-    const rclcpp::Duration & timeout);
+    const rclcpp::Duration & timeout, const rclcpp::Logger & logger = defaultLogger());
 
   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,
-    const rclcpp::Duration & timeout);
+    const rclcpp::Duration & timeout, const rclcpp::Logger & logger = defaultLogger());
+
+  /**
+   * @brief Get the latest transform between two frames by frame ID.
+   *
+   * @tparam T the type of the transformation to retrieve
+   * @param[in] target_frame the frame to which data should be transformed
+   * @param[in] source_frame the frame where the data originated
+   * @param[in] logger logger, if not specified, default logger will be used
+   * @return an optional containing the transform if successful, or empty if not
+   *
+   * @overload getTransform<geometry_msgs::msg::TransformStamped>
+   * @return An optional containing the TransformStamped if successful, or empty if not
+   *
+   * @overload getTransform<Eigen::Matrix4f>
+   * @return An optional containing the Eigen::Matrix4f if successful, or empty if not
+   *
+   * @overload getTransform<tf2::Transform>
+   * @return An optional containing the tf2::Transform if successful, or empty if not
+   */
+  template <typename T = TransformStamped>
+  std::enable_if_t<std::is_same_v<T, TransformStamped>, std::optional<TransformStamped>>
+  getLatestTransform(
+    const std::string & target_frame, const std::string & source_frame,
+    const rclcpp::Logger & logger = defaultLogger());
+
+  template <typename T = Eigen::Matrix4f>
+  std::enable_if_t<std::is_same_v<T, Eigen::Matrix4f>, std::optional<Eigen::Matrix4f>>
+  getLatestTransform(
+    const std::string & target_frame, const std::string & source_frame,
+    const rclcpp::Logger & logger = defaultLogger());
+
+  template <typename T = tf2::Transform>
+  std::enable_if_t<std::is_same_v<T, tf2::Transform>, std::optional<tf2::Transform>>
+  getLatestTransform(
+    const std::string & target_frame, const std::string & source_frame,
+    const rclcpp::Logger & logger = defaultLogger());
 
   /**
    * @brief Transforms a point cloud from one frame to another.
@@ -122,12 +164,13 @@ class ManagedTransformBuffer
    * @param[out] cloud_out the resultant output point cloud
    * @param[in] time the time at which the value of the transform is desired
    * @param[in] timeout how long to block before failing
+   * @param[in] logger logger, if not specified, default logger will be used
    * @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 tf2::TimePoint & time,
-    const tf2::Duration & timeout);
+    const tf2::Duration & timeout, const rclcpp::Logger & logger = defaultLogger());
 
   /**
    * @brief Transforms a point cloud from one frame to another.
@@ -138,14 +181,21 @@ class ManagedTransformBuffer
   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);
+    const rclcpp::Duration & timeout, const rclcpp::Logger & logger = defaultLogger());
 
   /** @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 Get the default logger.
+   *
+   * @return the default logger
+   */
+  static rclcpp::Logger defaultLogger();
+
   ManagedTransformBufferProvider * provider_;
 };
 

managed_transform_buffer/include/managed_transform_buffer/managed_transform_buffer_provider.hpp

@@ -24,8 +24,10 @@
 
 #include <tf2_ros/buffer.h>
 
+#include <atomic>
 #include <functional>
 #include <memory>
+#include <mutex>
 #include <optional>
 #include <random>
 #include <string>
@@ -95,20 +97,31 @@ class 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));
+    rcl_clock_type_t clock_type, const bool force_dynamic, tf2::Duration discovery_timeout,
+    tf2::Duration cache_time);
 
   ManagedTransformBufferProvider(const ManagedTransformBufferProvider &) = delete;
   ManagedTransformBufferProvider & operator=(const ManagedTransformBufferProvider &) = delete;
 
   /** @brief Destroy the Managed Transform Buffer object */
   ~ManagedTransformBufferProvider();
 
+  /**
+   * @brief Get the transform between two frames by frame ID.
+   *
+   * @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
+   * @param[in] timeout how long to block before failing
+   * @param[in] logger logger, if not specified, default logger will be used
+   * @return an optional containing the transform if successful, or empty if not
+   */
   std::optional<TransformStamped> getTransform(
     const std::string & target_frame, const std::string & source_frame, const tf2::TimePoint & time,
-    const tf2::Duration & timeout);
+    const tf2::Duration & timeout, const rclcpp::Logger & logger);
 
   /** @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;
@@ -117,12 +130,12 @@ class ManagedTransformBufferProvider
   /**
    * @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
+   * @param[in] clock_type type of the clock
+   * @param[in] discovery_timeout how long to wait for first TF discovery
+   * @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));
+    rcl_clock_type_t clock_type, tf2::Duration discovery_timeout, tf2::Duration cache_time);
 
   /** @brief Initialize TF listener */
   void activateListener();
@@ -155,34 +168,37 @@ class ManagedTransformBufferProvider
    * @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
+   * @param[in] logger logger, if not specified, default logger will be used
    * @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;
+    const tf2::Duration & timeout, const rclcpp::Logger & logger);
 
   /** @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
+   * @param[in] logger logger, if not specified, default logger will be used
    * @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);
+    const tf2::Duration & timeout, const rclcpp::Logger & logger);
 
   /** @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
+   * @param[in] timeout how long to block before
+   * @param[in] logger logger, if not specified, default logger will be used
    * @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);
+    const tf2::Duration & timeout, const rclcpp::Logger & logger);
 
   /** @brief Get a static transform from local TF buffer.
    *
@@ -199,11 +215,12 @@ class ManagedTransformBufferProvider
    * @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
+   * @param[in] logger logger, if not specified, default logger will be used
    * @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);
+    const tf2::Duration & timeout, const rclcpp::Logger & logger);
 
   static std::unique_ptr<ManagedTransformBufferProvider> instance;
   rclcpp::Node::SharedPtr node_{nullptr};
@@ -215,7 +232,8 @@ class ManagedTransformBufferProvider
   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 &)>
+    const std::string &, const std::string &, const tf2::TimePoint &, const tf2::Duration &,
+    const rclcpp::Logger &)>
     get_transform_;
   std::function<void(tf2_msgs::msg::TFMessage::SharedPtr)> cb_;
   std::function<void(tf2_msgs::msg::TFMessage::SharedPtr)> cb_static_;
@@ -227,7 +245,9 @@ class ManagedTransformBufferProvider
   std::mt19937 random_engine_;
   std::uniform_int_distribution<> dis_;
   std::mutex mutex_;
-  bool is_static_{true};
+  std::atomic<bool> is_static_{true};
+  tf2::Duration discovery_timeout_;
+  rclcpp::Logger logger_;
 };
 
 }  // namespace managed_transform_buffer