diff --git a/README.md b/README.md index 991fb16..5240418 100644 --- a/README.md +++ b/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(this->get_clock()); +auto managed_tf_buffer = std::make_unique(); // Get a transform from source_frame to target_frame auto tf_msg_transform = managed_tf_buffer->getTransform("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(RCL_ROS_TIME, false, tf2::durationFromSec(0.3)); +``` diff --git a/managed_transform_buffer/examples/example_managed_transform_buffer.cpp b/managed_transform_buffer/examples/example_managed_transform_buffer.cpp index 38e8b6d..ca898c8 100644 --- a/managed_transform_buffer/examples/example_managed_transform_buffer.cpp +++ b/managed_transform_buffer/examples/example_managed_transform_buffer.cpp @@ -30,8 +30,7 @@ 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->get_clock()); + managed_tf_buffer_ = std::make_unique(); cloud_sub_ = create_subscription( "input/cloud", rclcpp::SensorDataQoS(), std::bind(&ExampleNode::cloud_cb, this, std::placeholders::_1)); 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 28e709f..1a0d99d 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 @@ -27,6 +27,7 @@ #include #include +#include #include #include #include @@ -34,6 +35,7 @@ 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 @@ -80,17 +86,17 @@ class ManagedTransformBuffer std::enable_if_t, std::optional> 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 std::enable_if_t, std::optional> 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 std::enable_if_t, std::optional> 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::optional> 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 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); + const rclcpp::Duration & timeout, const rclcpp::Logger & logger = defaultLogger()); 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); + 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 + * @return An optional containing the TransformStamped if successful, or empty if not + * + * @overload getTransform + * @return An optional containing the Eigen::Matrix4f if successful, or empty if not + * + * @overload getTransform + * @return An optional containing the tf2::Transform if successful, or empty if not + */ + template + std::enable_if_t, std::optional> + getLatestTransform( + const std::string & target_frame, const std::string & source_frame, + const rclcpp::Logger & logger = defaultLogger()); + + template + std::enable_if_t, std::optional> + getLatestTransform( + const std::string & target_frame, const std::string & source_frame, + const rclcpp::Logger & logger = defaultLogger()); + + template + std::enable_if_t, std::optional> + 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_; }; 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 index f8622af..9b6c903 100644 --- 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 @@ -24,8 +24,10 @@ #include +#include #include #include +#include #include #include #include @@ -95,8 +97,8 @@ 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; @@ -104,11 +106,22 @@ class ManagedTransformBufferProvider /** @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 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 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 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 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 instance; rclcpp::Node::SharedPtr node_{nullptr}; @@ -215,7 +232,8 @@ class ManagedTransformBufferProvider rclcpp::SubscriptionOptionsWithAllocator> tf_options_; rclcpp::SubscriptionOptionsWithAllocator> tf_static_options_; std::function( - 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 cb_; std::function 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 is_static_{true}; + tf2::Duration discovery_timeout_; + rclcpp::Logger logger_; }; } // 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 b75f821..def5a1b 100644 --- a/managed_transform_buffer/src/managed_transform_buffer.cpp +++ b/managed_transform_buffer/src/managed_transform_buffer.cpp @@ -26,9 +26,11 @@ namespace managed_transform_buffer { ManagedTransformBuffer::ManagedTransformBuffer( - rclcpp::Clock::SharedPtr clock, tf2::Duration cache_time) + rcl_clock_type_t clock_type, const bool force_dynamic, tf2::Duration discovery_timeout, + tf2::Duration cache_time) { - provider_ = &ManagedTransformBufferProvider::getInstance(clock, cache_time); + provider_ = &ManagedTransformBufferProvider::getInstance( + clock_type, force_dynamic, discovery_timeout, cache_time); } ManagedTransformBuffer::~ManagedTransformBuffer() = default; @@ -36,17 +38,18 @@ 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) + const tf2::Duration & timeout, const rclcpp::Logger & logger) { - return provider_->getTransform(target_frame, source_frame, time, timeout); + provider_->getTransform(target_frame, source_frame, time, timeout, logger); + return provider_->getTransform(target_frame, source_frame, time, timeout, logger); } template <> std::optional ManagedTransformBuffer::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) { - auto tf = provider_->getTransform(target_frame, source_frame, time, timeout); + auto tf = provider_->getTransform(target_frame, source_frame, time, timeout, logger); if (!tf.has_value()) { return std::nullopt; } @@ -58,9 +61,9 @@ std::optional ManagedTransformBuffer::getTransform std::optional ManagedTransformBuffer::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) { - auto tf = provider_->getTransform(target_frame, source_frame, time, timeout); + auto tf = provider_->getTransform(target_frame, source_frame, time, timeout, logger); if (!tf.has_value()) { return std::nullopt; } @@ -72,34 +75,58 @@ 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 rclcpp::Duration & timeout, const rclcpp::Logger & logger) { return getTransform( - target_frame, source_frame, tf2_ros::fromRclcpp(time), tf2_ros::fromRclcpp(timeout)); + target_frame, source_frame, tf2_ros::fromRclcpp(time), tf2_ros::fromRclcpp(timeout), logger); } template <> std::optional ManagedTransformBuffer::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) { return getTransform( - target_frame, source_frame, tf2_ros::fromRclcpp(time), tf2_ros::fromRclcpp(timeout)); + target_frame, source_frame, tf2_ros::fromRclcpp(time), tf2_ros::fromRclcpp(timeout), logger); } template <> std::optional ManagedTransformBuffer::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) { return getTransform( - target_frame, source_frame, tf2_ros::fromRclcpp(time), tf2_ros::fromRclcpp(timeout)); + target_frame, source_frame, tf2_ros::fromRclcpp(time), tf2_ros::fromRclcpp(timeout), logger); +} + +template <> +std::optional ManagedTransformBuffer::getLatestTransform( + const std::string & target_frame, const std::string & source_frame, const rclcpp::Logger & logger) +{ + return getTransform( + target_frame, source_frame, tf2::TimePointZero, tf2::Duration::zero(), logger); +} + +template <> +std::optional ManagedTransformBuffer::getLatestTransform( + const std::string & target_frame, const std::string & source_frame, const rclcpp::Logger & logger) +{ + return getTransform( + target_frame, source_frame, tf2::TimePointZero, tf2::Duration::zero(), logger); +} + +template <> +std::optional ManagedTransformBuffer::getLatestTransform( + const std::string & target_frame, const std::string & source_frame, const rclcpp::Logger & logger) +{ + return getTransform( + target_frame, source_frame, tf2::TimePointZero, tf2::Duration::zero(), logger); } bool ManagedTransformBuffer::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) { if ( pcl::getFieldIndex(cloud_in, "x") == -1 || pcl::getFieldIndex(cloud_in, "y") == -1 || @@ -111,7 +138,7 @@ bool ManagedTransformBuffer::transformPointcloud( return true; } auto eigen_transform = - getTransform(target_frame, cloud_in.header.frame_id, time, timeout); + getTransform(target_frame, cloud_in.header.frame_id, time, timeout, logger); if (!eigen_transform.has_value()) { return false; } @@ -123,10 +150,11 @@ bool ManagedTransformBuffer::transformPointcloud( 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) + const rclcpp::Duration & timeout, const rclcpp::Logger & logger) { return transformPointcloud( - target_frame, cloud_in, cloud_out, tf2_ros::fromRclcpp(time), tf2_ros::fromRclcpp(timeout)); + target_frame, cloud_in, cloud_out, tf2_ros::fromRclcpp(time), tf2_ros::fromRclcpp(timeout), + logger); } bool ManagedTransformBuffer::isStatic() const @@ -134,4 +162,9 @@ bool ManagedTransformBuffer::isStatic() const return provider_->isStatic(); } +rclcpp::Logger ManagedTransformBuffer::defaultLogger() +{ + return rclcpp::get_logger("ManagedTransformBuffer"); +} + } // 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 index e283b53..7a343fd 100644 --- a/managed_transform_buffer/src/managed_transform_buffer_provider.cpp +++ b/managed_transform_buffer/src/managed_transform_buffer_provider.cpp @@ -22,9 +22,9 @@ #include +#include #include -#include #include #include @@ -34,27 +34,43 @@ namespace managed_transform_buffer std::unique_ptr ManagedTransformBufferProvider::instance = nullptr; ManagedTransformBufferProvider & ManagedTransformBufferProvider::getInstance( - rclcpp::Clock::SharedPtr clock, tf2::Duration cache_time) + rcl_clock_type_t clock_type, const bool force_dynamic, tf2::Duration discovery_timeout, + tf2::Duration cache_time) { - static ManagedTransformBufferProvider instance(clock, cache_time); + static ManagedTransformBufferProvider instance(clock_type, discovery_timeout, cache_time); + if (force_dynamic) { + std::lock_guard lock(instance.mutex_); + if (instance.isStatic()) { + instance.registerAsDynamic(); + } + if (clock_type != instance.clock_->get_clock_type()) { + RCLCPP_WARN_THROTTLE( + instance.logger_, *instance.clock_, 3000, + "Input clock type does not match (%d vs. %d). Input clock type will be ignored.", + clock_type, instance.clock_->get_clock_type()); + } + } return instance; } std::optional ManagedTransformBufferProvider::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) { - return get_transform_(target_frame, source_frame, time, timeout); + std::lock_guard lock(mutex_); + return get_transform_(target_frame, source_frame, time, timeout, logger); } bool ManagedTransformBufferProvider::isStatic() const { - return is_static_; + return is_static_.load(); } ManagedTransformBufferProvider::ManagedTransformBufferProvider( - rclcpp::Clock::SharedPtr clock, tf2::Duration cache_time) -: clock_(clock) + rcl_clock_type_t clock_type, tf2::Duration discovery_timeout, tf2::Duration cache_time) +: clock_(std::make_shared(clock_type)), + discovery_timeout_(discovery_timeout), + logger_(rclcpp::get_logger("ManagedTransformBuffer")) { executor_ = std::make_shared(); executor_thread_ = std::make_shared( @@ -64,6 +80,7 @@ ManagedTransformBufferProvider::ManagedTransformBufferProvider( 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); @@ -78,7 +95,6 @@ ManagedTransformBufferProvider::ManagedTransformBufferProvider( ManagedTransformBufferProvider::~ManagedTransformBufferProvider() { - deactivateListener(); executor_->cancel(); if (executor_thread_->joinable()) { executor_thread_->join(); @@ -87,9 +103,11 @@ ManagedTransformBufferProvider::~ManagedTransformBufferProvider() void ManagedTransformBufferProvider::activateListener() { - std::lock_guard lock(mutex_); options_.arguments({"--ros-args", "-r", "__node:=" + generateUniqueNodeName()}); node_ = rclcpp::Node::make_unique("_", options_); + auto timer_interface = std::make_shared( + node_->get_node_base_interface(), node_->get_node_timers_interface()); + tf_buffer_->setCreateTimerInterface(timer_interface); callback_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); tf_options_.callback_group = callback_group_; @@ -104,7 +122,6 @@ void ManagedTransformBufferProvider::activateListener() 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()); @@ -118,23 +135,28 @@ 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); + const tf2::TimePoint & time, const tf2::Duration & timeout, + const rclcpp::Logger & logger) -> std::optional { + // Try to get transform from local static buffer + auto static_tf = getStaticTransform(target_frame, source_frame); + if (static_tf) return static_tf; + + // Try to discover transform + if (!isStatic()) { // Another thread could switch to dynamic listener + return getDynamicTransform(target_frame, source_frame, time, timeout, logger); + } + return getUnknownTransform(target_frame, source_frame, time, timeout, logger); }; } void ManagedTransformBufferProvider::registerAsDynamic() { - is_static_ = false; + is_static_.store(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); + const tf2::TimePoint & time, const tf2::Duration & timeout, + const rclcpp::Logger & logger) -> std::optional { + return getDynamicTransform(target_frame, source_frame, time, timeout, logger); }; } @@ -155,53 +177,51 @@ void ManagedTransformBufferProvider::tfCallback( 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()); - } + RCLCPP_ERROR_THROTTLE( + logger_, *clock_, 3000, "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 + const tf2::Duration & timeout, const rclcpp::Logger & logger) { try { + auto x = node_->get_logger(); 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()); - } + RCLCPP_ERROR_THROTTLE( + logger, *clock_, 3000, "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) + const std::string & target_frame, const std::string & source_frame, const tf2::Duration & timeout, + const rclcpp::Logger & logger) { std::atomic timeout_reached{false}; auto framesToRoot = [this]( - const std::string & start_frame, TreeMap & last_tf_tree, - std::vector & frames_out) -> bool { + const std::string & start_frame, const TreeMap & tf_tree, + std::vector & frames_out, + const rclcpp::Logger & logger) -> 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()) { + auto frame_it = tf_tree.find(current_frame); + while (frame_it != tf_tree.end()) { current_frame = frame_it->second.parent; frames_out.push_back(current_frame); - frame_it = last_tf_tree.find(current_frame); + frame_it = 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()); - } + RCLCPP_ERROR_THROTTLE( + logger, *clock_, 3000, "Traverse depth exceeded for %s", start_frame.c_str()); return false; } } @@ -209,16 +229,17 @@ TraverseResult ManagedTransformBufferProvider::traverseTree( }; auto traverse = [this, &timeout_reached, &framesToRoot]( - const std::string & t1, const std::string & t2) -> TraverseResult { - while (!timeout_reached) { + const std::string & t1, const std::string & t2, + const rclcpp::Logger & logger) -> TraverseResult { + while (!timeout_reached.load()) { 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)) { + !framesToRoot(t1, last_tf_tree, frames_from_t1, logger) || + !framesToRoot(t2, last_tf_tree, frames_from_t2, logger)) { // Possibly TF loop occurred return {false, false}; } @@ -249,22 +270,19 @@ TraverseResult ManagedTransformBufferProvider::traverseTree( }; std::future future = - std::async(std::launch::async, traverse, target_frame, source_frame); + std::async(std::launch::async, traverse, target_frame, source_frame, logger); if (future.wait_for(timeout) == std::future_status::ready) { return future.get(); } - timeout_reached = true; + timeout_reached.store(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) + const tf2::Duration & timeout, const rclcpp::Logger & logger) { - if (!node_) { - activateListener(); - } - return lookupTransform(target_frame, source_frame, time, timeout); + return lookupTransform(target_frame, source_frame, time, timeout, logger); } std::optional ManagedTransformBufferProvider::getStaticTransform( @@ -314,24 +332,19 @@ std::optional ManagedTransformBufferProvider::getStaticTransfo std::optional ManagedTransformBufferProvider::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) { - // 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 + auto discovery_timeout = std::max(timeout, discovery_timeout_); std::future traverse_future = std::async( std::launch::async, &ManagedTransformBufferProvider::traverseTree, this, target_frame, - source_frame, timeout); + source_frame, discovery_timeout, logger); std::future> tf_future = std::async( std::launch::async, &ManagedTransformBufferProvider::lookupTransform, this, target_frame, - source_frame, time, timeout); + source_frame, time, discovery_timeout, logger); auto traverse_result = traverse_future.get(); auto tf = tf_future.get(); @@ -348,11 +361,9 @@ std::optional ManagedTransformBufferProvider::getUnknownTransf 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()); - } + RCLCPP_INFO_THROTTLE( + logger, *clock_, 3000, "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 1d16d16..ba1e3bd 100644 --- a/managed_transform_buffer/test/test_managed_transform_buffer.cpp +++ b/managed_transform_buffer/test/test_managed_transform_buffer.cpp @@ -92,8 +92,8 @@ class TestManagedTransformBuffer : public ::testing::Test void SetUp() override { node_ = std::make_shared("test_managed_transform_buffer"); - managed_tf_buffer_ = - std::make_unique(node_->get_clock()); + managed_tf_buffer_ = std::make_unique( + node_->get_clock()->get_clock_type()); static_tf_broadcaster_ = std::make_unique(node_); tf_broadcaster_ = std::make_unique(node_);