From d85102217f692746abea8546c8e41f0abc95c8b8 Mon Sep 17 00:00:00 2001 From: Tom Moore Date: Wed, 28 Aug 2024 07:57:27 +0100 Subject: [PATCH] Adding static transform listener (#673) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Alejandro Hernández Cordero --- tf2_ros/include/tf2_ros/transform_listener.h | 113 ++++++++++++++++--- tf2_ros/src/transform_listener.cpp | 5 +- tf2_ros/test/test_transform_listener.cpp | 95 ++++++++++++++++ 3 files changed, 198 insertions(+), 15 deletions(-) diff --git a/tf2_ros/include/tf2_ros/transform_listener.h b/tf2_ros/include/tf2_ros/transform_listener.h index 59eb841e0..640bf7667 100644 --- a/tf2_ros/include/tf2_ros/transform_listener.h +++ b/tf2_ros/include/tf2_ros/transform_listener.h @@ -89,7 +89,10 @@ class TransformListener * to it, then it's recommended to use one of the other constructors. */ TF2_ROS_PUBLIC - explicit TransformListener(tf2::BufferCore & buffer, bool spin_thread = true); + explicit TransformListener( + tf2::BufferCore & buffer, + bool spin_thread = true, + bool static_only = false); /** \brief Node constructor */ template> @@ -102,7 +105,8 @@ class TransformListener const rclcpp::SubscriptionOptionsWithAllocator & options = detail::get_default_transform_listener_sub_options(), const rclcpp::SubscriptionOptionsWithAllocator & static_options = - detail::get_default_transform_listener_static_sub_options()) + detail::get_default_transform_listener_static_sub_options(), + bool static_only = false) : TransformListener( buffer, node->get_node_base_interface(), @@ -113,7 +117,8 @@ class TransformListener qos, static_qos, options, - static_options) + static_options, + static_only) {} /** \brief Node interface constructor */ @@ -130,7 +135,8 @@ class TransformListener const rclcpp::SubscriptionOptionsWithAllocator & options = detail::get_default_transform_listener_sub_options(), const rclcpp::SubscriptionOptionsWithAllocator & static_options = - detail::get_default_transform_listener_static_sub_options()) + detail::get_default_transform_listener_static_sub_options(), + bool static_only = false) : buffer_(buffer) { init( @@ -142,7 +148,8 @@ class TransformListener qos, static_qos, options, - static_options); + static_options, + static_only); } TF2_ROS_PUBLIC @@ -163,7 +170,8 @@ class TransformListener const rclcpp::QoS & qos, const rclcpp::QoS & static_qos, const rclcpp::SubscriptionOptionsWithAllocator & options, - const rclcpp::SubscriptionOptionsWithAllocator & static_options) + const rclcpp::SubscriptionOptionsWithAllocator & static_options, + bool static_only = false) { spin_thread_ = spin_thread; node_base_interface_ = node_base; @@ -179,14 +187,19 @@ class TransformListener // Create new callback group for message_subscription of tf and tf_static callback_group_ = node_base_interface_->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive, false); - // Duplicate to modify option of subscription - rclcpp::SubscriptionOptionsWithAllocator tf_options = options; + + if (!static_only) { + // Duplicate to modify subscription options + rclcpp::SubscriptionOptionsWithAllocator tf_options = options; + tf_options.callback_group = callback_group_; + + message_subscription_tf_ = rclcpp::create_subscription( + node_parameters, node_topics, "/tf", qos, std::move(cb), tf_options); + } + rclcpp::SubscriptionOptionsWithAllocator tf_static_options = static_options; - tf_options.callback_group = callback_group_; tf_static_options.callback_group = callback_group_; - message_subscription_tf_ = rclcpp::create_subscription( - node_parameters, node_topics, "/tf", qos, std::move(cb), tf_options); message_subscription_tf_static_ = rclcpp::create_subscription( node_parameters, node_topics, @@ -202,8 +215,10 @@ class TransformListener // Tell the buffer we have a dedicated thread to enable timeouts buffer_.setUsingDedicatedThread(true); } else { - message_subscription_tf_ = rclcpp::create_subscription( - node_parameters, node_topics, "/tf", qos, std::move(cb), options); + if (!static_only) { + message_subscription_tf_ = rclcpp::create_subscription( + node_parameters, node_topics, "/tf", qos, std::move(cb), options); + } message_subscription_tf_static_ = rclcpp::create_subscription( node_parameters, node_topics, @@ -229,6 +244,78 @@ class TransformListener rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface_ {nullptr}; rclcpp::CallbackGroup::SharedPtr callback_group_{nullptr}; }; + +/** \brief Convenience class for subscribing to static-only transforms + * + * While users can achieve the same thing with a normal TransformListener, the number of parameters that must be + * provided is unwieldy. + */ +class StaticTransformListener : public TransformListener +{ +public: + /** \brief Simplified constructor for a static transform listener + * \see the simplified TransformListener documentation + */ + TF2_ROS_PUBLIC + explicit StaticTransformListener(tf2::BufferCore & buffer, bool spin_thread = true) + : TransformListener(buffer, spin_thread, true) + { + } + + /** \brief Node constructor */ + template> + StaticTransformListener( + tf2::BufferCore & buffer, + NodeT && node, + bool spin_thread = true, + const rclcpp::QoS & static_qos = StaticListenerQoS(), + const rclcpp::SubscriptionOptionsWithAllocator & static_options = + detail::get_default_transform_listener_static_sub_options()) + : TransformListener( + buffer, + node, + spin_thread, + rclcpp::QoS(1), + static_qos, + rclcpp::SubscriptionOptionsWithAllocator(), + static_options, + true) + { + } + + /** \brief Node interface constructor */ + template> + StaticTransformListener( + tf2::BufferCore & buffer, + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, + rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters, + rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics, + bool spin_thread = true, + const rclcpp::QoS & static_qos = StaticListenerQoS(), + const rclcpp::SubscriptionOptionsWithAllocator & static_options = + detail::get_default_transform_listener_static_sub_options()) + : TransformListener( + buffer, + node_base, + node_logging, + node_parameters, + node_topics, + spin_thread, + rclcpp::QoS(1), + static_qos, + rclcpp::SubscriptionOptionsWithAllocator(), + static_options, + true) + { + } + + TF2_ROS_PUBLIC + virtual ~StaticTransformListener() + { + } +}; + } // namespace tf2_ros #endif // TF2_ROS__TRANSFORM_LISTENER_H_ diff --git a/tf2_ros/src/transform_listener.cpp b/tf2_ros/src/transform_listener.cpp index 5e31b6a8d..55e946496 100644 --- a/tf2_ros/src/transform_listener.cpp +++ b/tf2_ros/src/transform_listener.cpp @@ -40,7 +40,7 @@ namespace tf2_ros { -TransformListener::TransformListener(tf2::BufferCore & buffer, bool spin_thread) +TransformListener::TransformListener(tf2::BufferCore & buffer, bool spin_thread, bool static_only) : buffer_(buffer) { rclcpp::NodeOptions options; @@ -64,7 +64,8 @@ TransformListener::TransformListener(tf2::BufferCore & buffer, bool spin_thread) optional_default_node_->get_node_topics_interface(), spin_thread, DynamicListenerQoS(), StaticListenerQoS(), detail::get_default_transform_listener_sub_options(), - detail::get_default_transform_listener_static_sub_options()); + detail::get_default_transform_listener_static_sub_options(), + static_only); } TransformListener::~TransformListener() diff --git a/tf2_ros/test/test_transform_listener.cpp b/tf2_ros/test/test_transform_listener.cpp index d6e28f6ea..313520839 100644 --- a/tf2_ros/test/test_transform_listener.cpp +++ b/tf2_ros/test/test_transform_listener.cpp @@ -30,7 +30,10 @@ #include #include #include +#include +#include +#include #include #include "node_wrapper.hpp" @@ -49,6 +52,14 @@ class CustomNode : public rclcpp::Node tf_listener_ = std::make_shared(buffer, shared_from_this(), false); } + void init_static_tf_listener() + { + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); + tf2_ros::Buffer buffer(clock); + tf_listener_ = + std::make_shared(buffer, shared_from_this(), false); + } + private: std::shared_ptr tf_listener_; }; @@ -67,6 +78,14 @@ class CustomComposableNode : public rclcpp::Node tf_listener_ = std::make_shared(buffer, shared_from_this(), false); } + void init_static_tf_listener() + { + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); + tf2_ros::Buffer buffer(clock); + tf_listener_ = + std::make_shared(buffer, shared_from_this(), false); + } + private: std::shared_ptr tf_listener_; }; @@ -104,6 +123,82 @@ TEST(tf2_test_transform_listener, transform_listener_with_intraprocess) custom_node->init_tf_listener(); } +TEST(tf2_test_static_transform_listener, static_transform_listener_rclcpp_node) +{ + auto node = rclcpp::Node::make_shared("tf2_ros_static_transform_listener"); + + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); + tf2_ros::Buffer buffer(clock); +} + +TEST(tf2_test_static_transform_listener, static_transform_listener_custom_rclcpp_node) +{ + auto node = std::make_shared("tf2_ros_static_transform_listener"); + + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); + tf2_ros::Buffer buffer(clock); + tf2_ros::StaticTransformListener tfl(buffer, node, false); +} + +TEST(tf2_test_static_transform_listener, static_transform_listener_as_member) +{ + auto custom_node = std::make_shared(); + custom_node->init_static_tf_listener(); +} + +TEST(tf2_test_static_transform_listener, static_transform_listener_with_intraprocess) +{ + rclcpp::executors::SingleThreadedExecutor exec; + rclcpp::NodeOptions options; + options = options.use_intra_process_comms(true); + auto custom_node = std::make_shared(options); + custom_node->init_static_tf_listener(); +} + +TEST(tf2_test_listeners, static_vs_dynamic) +{ + auto node = rclcpp::Node::make_shared("tf2_ros_static_transform_listener"); + + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); + tf2_ros::Buffer dynamic_buffer(clock); + tf2_ros::Buffer static_buffer(clock); + tf2_ros::TransformListener tfl(dynamic_buffer, node, true); + tf2_ros::StaticTransformListener stfl(static_buffer, node, true); + tf2_ros::TransformBroadcaster broadcaster(node); + tf2_ros::StaticTransformBroadcaster static_broadcaster(node); + + geometry_msgs::msg::TransformStamped static_trans; + static_trans.header.stamp = clock->now(); + static_trans.header.frame_id = "parent_static"; + static_trans.child_frame_id = "child_static"; + static_trans.transform.rotation.w = 1.0; + static_broadcaster.sendTransform(static_trans); + + geometry_msgs::msg::TransformStamped dynamic_trans; + dynamic_trans.header.frame_id = "parent_dynamic"; + dynamic_trans.child_frame_id = "child_dynamic"; + dynamic_trans.transform.rotation.w = 1.0; + + for (int i = 0; i < 10; ++i) { + dynamic_trans.header.stamp = clock->now(); + broadcaster.sendTransform(dynamic_trans); + + rclcpp::spin_some(node); + rclcpp::sleep_for(std::chrono::milliseconds(10)); + } + + // Dynamic buffer should have both dynamic and static transforms available + EXPECT_NO_THROW( + dynamic_buffer.lookupTransform("parent_dynamic", "child_dynamic", tf2::TimePointZero)); + EXPECT_NO_THROW(dynamic_buffer.lookupTransform("parent_static", "child_static", clock->now())); + + // Static buffer should have only static transforms available + EXPECT_THROW( + static_buffer.lookupTransform("parent_dynamic", "child_dynamic", tf2::TimePointZero), + tf2::LookupException); + EXPECT_NO_THROW(static_buffer.lookupTransform("parent_static", "child_static", clock->now())); +} + int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv);