diff --git a/tf2_ros/include/tf2_ros/transform_listener.hpp b/tf2_ros/include/tf2_ros/transform_listener.hpp index 528242bad..03c172337 100644 --- a/tf2_ros/include/tf2_ros/transform_listener.hpp +++ b/tf2_ros/include/tf2_ros/transform_listener.hpp @@ -93,6 +93,18 @@ class TransformListener TF2_ROS_PUBLIC explicit TransformListener(tf2::BufferCore & buffer, bool spin_thread = true); + /** \brief Simplified constructor for transform listener with static_only option. + * + * This constructor will create a new ROS 2 node under the hood. + * If you already have access to a ROS 2 node and you want to associate the 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, + bool static_only); + /** \brief Node constructor */ template> TransformListener( @@ -118,6 +130,31 @@ class TransformListener static_options) {} + /** \brief Node constructor with static_only option */ + template> + TransformListener( + tf2::BufferCore & buffer, + NodeT && node, + bool spin_thread, + const rclcpp::QoS & qos, + const rclcpp::QoS & static_qos, + const rclcpp::SubscriptionOptionsWithAllocator & options, + const rclcpp::SubscriptionOptionsWithAllocator & static_options, + bool static_only) + : TransformListener( + buffer, + node->get_node_base_interface(), + node->get_node_logging_interface(), + node->get_node_parameters_interface(), + node->get_node_topics_interface(), + spin_thread, + qos, + static_qos, + options, + static_options, + static_only) + {} + /** \brief Node interface constructor */ template> TransformListener( @@ -147,6 +184,35 @@ class TransformListener static_options); } + /** \brief Node interface constructor with static_only option */ + template> + TransformListener( + 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, + const rclcpp::QoS & qos, + const rclcpp::QoS & static_qos, + const rclcpp::SubscriptionOptionsWithAllocator & options, + const rclcpp::SubscriptionOptionsWithAllocator & static_options, + bool static_only) + : buffer_(buffer) + { + init( + node_base, + node_logging, + node_parameters, + node_topics, + spin_thread, + qos, + static_qos, + options, + static_options, + static_only); + } + TF2_ROS_PUBLIC virtual ~TransformListener(); @@ -216,6 +282,71 @@ class TransformListener } } + // Overload of init() with the static_only flag + template> + void init( + 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, + const rclcpp::QoS & qos, + const rclcpp::QoS & static_qos, + const rclcpp::SubscriptionOptionsWithAllocator & options, + const rclcpp::SubscriptionOptionsWithAllocator & static_options, + bool static_only) + { + if (!static_only) { + init( + node_base, + node_logging, + node_parameters, + node_topics, + spin_thread, + qos, + static_qos, + options, + static_options); + return; + } + + spin_thread_ = spin_thread; + node_base_interface_ = node_base; + node_logging_interface_ = node_logging; + + using callback_t = std::function; + callback_t static_cb = std::bind( + &TransformListener::subscription_callback, this, std::placeholders::_1, true); + + if (spin_thread_) { + callback_group_ = node_base_interface_->create_callback_group( + rclcpp::CallbackGroupType::MutuallyExclusive, false); + rclcpp::SubscriptionOptionsWithAllocator tf_static_options = static_options; + tf_static_options.callback_group = callback_group_; + + message_subscription_tf_static_ = rclcpp::create_subscription( + node_parameters, + node_topics, + "/tf_static", + static_qos, + std::move(static_cb), + tf_static_options); + + executor_ = std::make_shared(); + executor_->add_callback_group(callback_group_, node_base_interface_); + dedicated_listener_thread_ = std::make_unique([&]() {executor_->spin();}); + buffer_.setUsingDedicatedThread(true); + } else { + message_subscription_tf_static_ = rclcpp::create_subscription( + node_parameters, + node_topics, + "/tf_static", + static_qos, + std::move(static_cb), + static_options); + } + } + bool spin_thread_{false}; std::unique_ptr dedicated_listener_thread_ {nullptr}; rclcpp::Executor::SharedPtr executor_ {nullptr}; @@ -231,6 +362,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_HPP_ diff --git a/tf2_ros/src/transform_listener.cpp b/tf2_ros/src/transform_listener.cpp index 52467ffb2..af705a98f 100644 --- a/tf2_ros/src/transform_listener.cpp +++ b/tf2_ros/src/transform_listener.cpp @@ -67,6 +67,34 @@ TransformListener::TransformListener(tf2::BufferCore & buffer, bool spin_thread) detail::get_default_transform_listener_static_sub_options()); } +TransformListener::TransformListener(tf2::BufferCore & buffer, bool spin_thread, bool static_only) +: buffer_(buffer) +{ + rclcpp::NodeOptions options; + // create a unique name for the node + // but specify its name in .arguments to override any __node passed on the command line. + // avoiding sstream because it's behavior can be overridden by external libraries. + // See this issue: https://github.com/ros2/geometry2/issues/540 + char node_name[42]; + snprintf( + node_name, sizeof(node_name), "transform_listener_impl_%zx", + reinterpret_cast(this) + ); + options.arguments({"--ros-args", "-r", "__node:=" + std::string(node_name)}); + options.start_parameter_event_publisher(false); + options.start_parameter_services(false); + optional_default_node_ = rclcpp::Node::make_shared("_", options); + init( + optional_default_node_->get_node_base_interface(), + optional_default_node_->get_node_logging_interface(), + optional_default_node_->get_node_parameters_interface(), + 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(), + static_only); +} + TransformListener::~TransformListener() { if (spin_thread_) { diff --git a/tf2_ros/test/test_transform_listener.cpp b/tf2_ros/test/test_transform_listener.cpp index 66b34bba2..c82287638 100644 --- a/tf2_ros/test/test_transform_listener.cpp +++ b/tf2_ros/test/test_transform_listener.cpp @@ -29,6 +29,7 @@ #include +#include #include #include @@ -36,7 +37,6 @@ #include #include - #include "node_wrapper.hpp" class CustomNode : public rclcpp::Node @@ -53,6 +53,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_; }; @@ -71,6 +79,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_; }; @@ -108,6 +124,85 @@ 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); + tf2_ros::StaticTransformListener stfl(buffer, node, false); +} + +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 stfl(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", tf2::TimePointZero)); + + // 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", tf2::TimePointZero)); +} + int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv);