From 6e5f0433119b19b71c57fab06906cdf92e8c49c8 Mon Sep 17 00:00:00 2001 From: CursedRock17 Date: Fri, 11 Oct 2024 23:33:47 -0400 Subject: [PATCH 1/2] Adding NodeInterfaces API Design Signed-off-by: CursedRock17 --- test_tf2/test/test_buffer_server.cpp | 2 +- test_tf2/test/test_message_filter.cpp | 54 +++------ test_tf2/test/test_static_publisher.cpp | 8 +- tf2_ros/include/tf2_ros/buffer.h | 57 +++++----- tf2_ros/include/tf2_ros/create_timer_ros.h | 14 ++- tf2_ros/include/tf2_ros/message_filter.h | 104 ++++++++++++++---- .../tf2_ros/static_transform_broadcaster.h | 44 ++++++-- .../include/tf2_ros/transform_broadcaster.h | 49 +++++++-- tf2_ros/include/tf2_ros/transform_listener.h | 86 ++++++++++----- tf2_ros/src/buffer.cpp | 39 +++++++ tf2_ros/src/buffer_server_main.cpp | 2 +- tf2_ros/src/create_timer_ros.cpp | 19 +++- tf2_ros/src/tf2_echo.cpp | 2 +- tf2_ros/src/tf2_monitor.cpp | 2 +- tf2_ros/src/transform_listener.cpp | 7 +- tf2_ros/test/listener_unittest.cpp | 4 +- tf2_ros/test/message_filter_test.cpp | 51 ++++----- tf2_ros/test/node_wrapper.hpp | 3 + tf2_ros/test/test_buffer.cpp | 45 +++++--- .../test_static_transform_broadcaster.cpp | 10 +- tf2_ros/test/test_transform_broadcaster.cpp | 10 +- tf2_ros/test/test_transform_listener.cpp | 16 +-- tf2_ros/test/time_reset_test.cpp | 6 +- 23 files changed, 422 insertions(+), 212 deletions(-) diff --git a/test_tf2/test/test_buffer_server.cpp b/test_tf2/test/test_buffer_server.cpp index 10d77ab6b..88014dee9 100644 --- a/test_tf2/test/test_buffer_server.cpp +++ b/test_tf2/test/test_buffer_server.cpp @@ -47,7 +47,7 @@ int main(int argc, char ** argv) auto node = rclcpp::Node::make_shared("tf2_ros_message_filter"); rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); - tf2_ros::Buffer buffer(clock); + tf2_ros::Buffer buffer(clock, *node); tf2_ros::TransformListener tfl(buffer, node, false); std::unique_ptr server = std::make_unique( buffer, diff --git a/test_tf2/test/test_message_filter.cpp b/test_tf2/test/test_message_filter.cpp index 29c23ce6b..57c4dc5bd 100644 --- a/test_tf2/test/test_message_filter.cpp +++ b/test_tf2/test/test_message_filter.cpp @@ -79,12 +79,10 @@ class Notification TEST(MessageFilter, noTransforms) { auto node = rclcpp::Node::make_shared("tf2_ros_message_filter"); - auto create_timer_interface = std::make_shared( - node->get_node_base_interface(), - node->get_node_timers_interface()); + auto create_timer_interface = std::make_shared(*node); rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); - tf2_ros::Buffer buffer(clock); + tf2_ros::Buffer buffer(clock, *node); buffer.setCreateTimerInterface(create_timer_interface); tf2_ros::MessageFilter filter(buffer, "frame1", 10, node); Notification n(1); @@ -102,12 +100,10 @@ TEST(MessageFilter, noTransforms) TEST(MessageFilter, noTransformsSameFrame) { auto node = rclcpp::Node::make_shared("tf2_ros_message_filter"); - auto create_timer_interface = std::make_shared( - node->get_node_base_interface(), - node->get_node_timers_interface()); + auto create_timer_interface = std::make_shared(*node); rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); - tf2_ros::Buffer buffer(clock); + tf2_ros::Buffer buffer(clock, *node); buffer.setCreateTimerInterface(create_timer_interface); tf2_ros::MessageFilter filter(buffer, "frame1", 10, node); Notification n(1); @@ -145,12 +141,10 @@ geometry_msgs::msg::TransformStamped createTransform( TEST(MessageFilter, preexistingTransforms) { auto node = rclcpp::Node::make_shared("tf2_ros_message_filter"); - auto create_timer_interface = std::make_shared( - node->get_node_base_interface(), - node->get_node_timers_interface()); + auto create_timer_interface = std::make_shared(*node); rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); - tf2_ros::Buffer buffer(clock); + tf2_ros::Buffer buffer(clock, *node); buffer.setCreateTimerInterface(create_timer_interface); tf2_ros::MessageFilter filter(buffer, "frame1", 10, node); Notification n(1); @@ -176,12 +170,10 @@ TEST(MessageFilter, preexistingTransforms) TEST(MessageFilter, postTransforms) { auto node = rclcpp::Node::make_shared("tf2_ros_message_filter"); - auto create_timer_interface = std::make_shared( - node->get_node_base_interface(), - node->get_node_timers_interface()); + auto create_timer_interface = std::make_shared(*node); rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); - tf2_ros::Buffer buffer(clock); + tf2_ros::Buffer buffer(clock, *node); buffer.setCreateTimerInterface(create_timer_interface); tf2_ros::MessageFilter filter(buffer, "frame1", 10, node); Notification n(1); @@ -212,9 +204,7 @@ TEST(MessageFilter, concurrentTransforms) const int messages = 30; const int buffer_size = messages; auto node = rclcpp::Node::make_shared("tf2_ros_message_filter"); - auto create_timer_interface = std::make_shared( - node->get_node_base_interface(), - node->get_node_timers_interface()); + auto create_timer_interface = std::make_shared(*node); rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); @@ -225,7 +215,7 @@ TEST(MessageFilter, concurrentTransforms) msg->header.stamp = stamp; msg->header.frame_id = "frame2"; - tf2_ros::Buffer buffer(clock); + tf2_ros::Buffer buffer(clock, *node); for (int i = 0; i < 50; i++) { buffer.setCreateTimerInterface(create_timer_interface); tf2_ros::MessageFilter filter(buffer, "frame1", buffer_size, @@ -294,12 +284,10 @@ TEST(MessageFilter, concurrentTransforms) TEST(MessageFilter, setTargetFrame) { auto node = rclcpp::Node::make_shared("tf2_ros_message_filter"); - auto create_timer_interface = std::make_shared( - node->get_node_base_interface(), - node->get_node_timers_interface()); + auto create_timer_interface = std::make_shared(*node); rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); - tf2_ros::Buffer buffer(clock); + tf2_ros::Buffer buffer(clock, *node); buffer.setCreateTimerInterface(create_timer_interface); tf2_ros::MessageFilter filter(buffer, "frame1", 10, node); Notification n(1); @@ -325,12 +313,10 @@ TEST(MessageFilter, setTargetFrame) TEST(MessageFilter, multipleTargetFrames) { auto node = rclcpp::Node::make_shared("tf2_ros_message_filter"); - auto create_timer_interface = std::make_shared( - node->get_node_base_interface(), - node->get_node_timers_interface()); + auto create_timer_interface = std::make_shared(*node); rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); - tf2_ros::Buffer buffer(clock); + tf2_ros::Buffer buffer(clock, *node); buffer.setCreateTimerInterface(create_timer_interface); tf2_ros::MessageFilter filter(buffer, "", 10, node); Notification n(1); @@ -366,12 +352,10 @@ TEST(MessageFilter, multipleTargetFrames) TEST(MessageFilter, tolerance) { auto node = rclcpp::Node::make_shared("tf2_ros_message_filter"); - auto create_timer_interface = std::make_shared( - node->get_node_base_interface(), - node->get_node_timers_interface()); + auto create_timer_interface = std::make_shared(*node); rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); - tf2_ros::Buffer buffer(clock); + tf2_ros::Buffer buffer(clock, *node); buffer.setCreateTimerInterface(create_timer_interface); tf2_ros::MessageFilter filter(buffer, "frame1", 10, node); Notification n(1); @@ -497,12 +481,10 @@ TEST(MessageFilter, tolerance) TEST(MessageFilter, checkStampPrecisionLoss) { auto node = rclcpp::Node::make_shared("tf2_ros_message_filter"); - auto create_timer_interface = std::make_shared( - node->get_node_base_interface(), - node->get_node_timers_interface()); + auto create_timer_interface = std::make_shared(*node); rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); - tf2_ros::Buffer buffer(clock); + tf2_ros::Buffer buffer(clock, *node); buffer.setCreateTimerInterface(create_timer_interface); tf2_ros::MessageFilter filter(buffer, "frame1", 10, node); Notification n(1); diff --git a/test_tf2/test/test_static_publisher.cpp b/test_tf2/test/test_static_publisher.cpp index bd1274667..7d78517d6 100644 --- a/test_tf2/test/test_static_publisher.cpp +++ b/test_tf2/test/test_static_publisher.cpp @@ -53,7 +53,7 @@ TEST(StaticTransformPublisher, a_b_different_times) auto node = rclcpp::Node::make_shared("StaticTransformPublisher_a_b_different_times_test"); rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); - tf2_ros::Buffer mB(clock); + tf2_ros::Buffer mB(clock, *node); tf2_ros::TransformListener tfl(mB, node, false); rclcpp::executors::SingleThreadedExecutor executor; @@ -88,7 +88,7 @@ TEST(StaticTransformPublisher, a_c_different_times) auto node = rclcpp::Node::make_shared("StaticTransformPublisher_a_c_different_times_test"); rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); - tf2_ros::Buffer mB(clock); + tf2_ros::Buffer mB(clock, *node); tf2_ros::TransformListener tfl(mB, node, false); rclcpp::executors::SingleThreadedExecutor executor; @@ -122,7 +122,7 @@ TEST(StaticTransformPublisher, a_d_different_times) auto node = rclcpp::Node::make_shared("StaticTransformPublisher_a_d_different_times_test"); rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); - tf2_ros::Buffer mB(clock); + tf2_ros::Buffer mB(clock, *node); tf2_ros::TransformListener tfl(mB, node, false); rclcpp::executors::SingleThreadedExecutor executor; @@ -173,7 +173,7 @@ TEST(StaticTransformPublisher, multiple_parent_test) auto node = rclcpp::Node::make_shared("StaticTransformPublisher_a_d_different_times_test"); rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); - tf2_ros::Buffer mB(clock); + tf2_ros::Buffer mB(clock, *node); tf2_ros::TransformListener tfl(mB, node, false); rclcpp::executors::SingleThreadedExecutor executor; diff --git a/tf2_ros/include/tf2_ros/buffer.h b/tf2_ros/include/tf2_ros/buffer.h index 13571fa9b..8686da343 100644 --- a/tf2_ros/include/tf2_ros/buffer.h +++ b/tf2_ros/include/tf2_ros/buffer.h @@ -49,9 +49,12 @@ #include "geometry_msgs/msg/transform_stamped.hpp" #include "tf2_msgs/srv/frame_graph.hpp" #include "rclcpp/node_interfaces/get_node_base_interface.hpp" -#include "rclcpp/node_interfaces/get_node_services_interface.hpp" #include "rclcpp/node_interfaces/get_node_logging_interface.hpp" +#include "rclcpp/node_interfaces/get_node_services_interface.hpp" +#include "rclcpp/node_interfaces/node_base_interface.hpp" #include "rclcpp/node_interfaces/node_logging_interface.hpp" +#include "rclcpp/node_interfaces/node_services_interface.hpp" +#include "rclcpp/node_interfaces/node_interfaces.hpp" #include "rclcpp/rclcpp.hpp" namespace tf2_ros @@ -70,6 +73,22 @@ class Buffer : public BufferInterface, public AsyncBufferInterface, public tf2:: using tf2::BufferCore::canTransform; using SharedPtr = std::shared_ptr; + /** \brief Constructor for a Buffer object + * \param clock A clock to use for time and sleeping + * \param cache_time How long to keep a history of transforms + * \param interfaces Advertise the view_frames service that exposes debugging information from the buffer, based on a set of node interfaces + * \param qos If passed change the quality of service of the frames_server_ service + */ + Buffer( + rclcpp::Clock::SharedPtr clock, + rclcpp::node_interfaces::NodeInterfaces< + rclcpp::node_interfaces::NodeBaseInterface, + rclcpp::node_interfaces::NodeLoggingInterface, + rclcpp::node_interfaces::NodeServicesInterface + > node_interfaces, + tf2::Duration cache_time = tf2::Duration(tf2::BUFFER_CORE_DEFAULT_CACHE_TIME), + const rclcpp::QoS & qos = rclcpp::ServicesQoS()); + /** \brief Constructor for a Buffer object * \param clock A clock to use for time and sleeping * \param cache_time How long to keep a history of transforms @@ -77,6 +96,7 @@ class Buffer : public BufferInterface, public AsyncBufferInterface, public tf2:: * \param qos If passed change the quality of service of the frames_server_ service */ template + [[deprecated("Use rclcpp::node_interfaces::NodeInterfaces instead of NoteT")]] Buffer( rclcpp::Clock::SharedPtr clock, tf2::Duration cache_time = tf2::Duration(tf2::BUFFER_CORE_DEFAULT_CACHE_TIME), @@ -84,32 +104,7 @@ class Buffer : public BufferInterface, public AsyncBufferInterface, public tf2:: const rclcpp::QoS & qos = rclcpp::ServicesQoS()) : BufferCore(cache_time), clock_(clock), timer_interface_(nullptr) { - if (nullptr == clock_) { - throw std::invalid_argument("clock must be a valid instance"); - } - - auto post_jump_cb = [this](const rcl_time_jump_t & jump_info) {onTimeJump(jump_info);}; - - rcl_jump_threshold_t jump_threshold; - // Disable forward jump callbacks - jump_threshold.min_forward.nanoseconds = 0; - // Anything backwards is a jump - jump_threshold.min_backward.nanoseconds = -1; - // Callback if the clock changes too - jump_threshold.on_clock_change = true; - - jump_handler_ = clock_->create_jump_callback(nullptr, post_jump_cb, jump_threshold); - - if (node) { - node_logging_interface_ = rclcpp::node_interfaces::get_node_logging_interface(node); - - frames_server_ = rclcpp::create_service( - rclcpp::node_interfaces::get_node_base_interface(node), - rclcpp::node_interfaces::get_node_services_interface(node), - "tf2_frames", std::bind( - &Buffer::getFrames, this, std::placeholders::_1, - std::placeholders::_2), qos, nullptr); - } + Buffer(clock, *node, cache_time, qos); } /** \brief Get the transform between two frames by frame ID. @@ -332,7 +327,13 @@ class Buffer : public BufferInterface, public AsyncBufferInterface, public tf2:: /// \brief A clock to use for time and sleeping rclcpp::Clock::SharedPtr clock_; - /// \brief A node logging interface to access the buffer node's logger + /// \brief A set of interface to access the buffer's node + rclcpp::node_interfaces::NodeInterfaces< + rclcpp::node_interfaces::NodeBaseInterface, + rclcpp::node_interfaces::NodeLoggingInterface, + rclcpp::node_interfaces::NodeServicesInterface + > node_interfaces_; + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface_; /// \brief Interface for creating timers diff --git a/tf2_ros/include/tf2_ros/create_timer_ros.h b/tf2_ros/include/tf2_ros/create_timer_ros.h index 549d98484..0333480b0 100644 --- a/tf2_ros/include/tf2_ros/create_timer_ros.h +++ b/tf2_ros/include/tf2_ros/create_timer_ros.h @@ -38,6 +38,7 @@ #include "tf2/time.h" #include "rclcpp/rclcpp.hpp" +#include "rclcpp/node_interfaces/node_interfaces.hpp" namespace tf2_ros { @@ -50,6 +51,13 @@ namespace tf2_ros class CreateTimerROS : public CreateTimerInterface { public: + CreateTimerROS( + rclcpp::node_interfaces::NodeInterfaces< + rclcpp::node_interfaces::NodeBaseInterface, + rclcpp::node_interfaces::NodeTimersInterface> node_interfaces, + rclcpp::CallbackGroup::SharedPtr callback_group = nullptr); + + [[deprecated("Use rclcpp::node_interfaces::NodeInterfaces instead of multiple interfaces")]] TF2_ROS_PUBLIC CreateTimerROS( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, @@ -119,8 +127,10 @@ class CreateTimerROS : public CreateTimerInterface const TimerHandle & timer_handle, TimerCallbackType callback); - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; - rclcpp::node_interfaces::NodeTimersInterface::SharedPtr node_timers_; + rclcpp::node_interfaces::NodeInterfaces< + rclcpp::node_interfaces::NodeBaseInterface, + rclcpp::node_interfaces::NodeTimersInterface> node_interfaces_; + TimerHandle next_timer_handle_index_; std::unordered_map timers_map_; std::mutex timers_map_mutex_; diff --git a/tf2_ros/include/tf2_ros/message_filter.h b/tf2_ros/include/tf2_ros/message_filter.h index fd461a8bc..717640f3d 100644 --- a/tf2_ros/include/tf2_ros/message_filter.h +++ b/tf2_ros/include/tf2_ros/message_filter.h @@ -57,6 +57,7 @@ #include "builtin_interfaces/msg/time.hpp" #include "rclcpp/rclcpp.hpp" +#include "rclcpp/node_interfaces/node_interfaces.hpp" #define TF2_ROS_MESSAGEFILTER_DEBUG(fmt, ...) \ RCUTILS_LOG_DEBUG_NAMED( \ @@ -152,6 +153,32 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi using MConstPtr = std::shared_ptr; typedef message_filters::MessageEvent MEvent; + /** + * \brief Constructor + * + * \param buffer The buffer this filter should use. + * \param target_frame The frame this filter should attempt to transform to. To use multiple frames, pass an empty string here and use the setTargetFrames() function. + * \param queue_size The number of messages to queue up before throwing away old ones. 0 means infinite (dangerous). + * \param node_interfaces The ros2 node interfaces (logging and clock) used for various operations. + * \param buffer_timeout The timeout duration after requesting transforms from the buffer. + */ + template + MessageFilter( + BufferT & buffer, const std::string & target_frame, uint32_t queue_size, + rclcpp::node_interfaces::NodeInterfaces< + rclcpp::node_interfaces::NodeLoggingInterface, + rclcpp::node_interfaces::NodeClockInterface> node_interfaces, + std::chrono::duration buffer_timeout = + std::chrono::duration::max()) + : node_interfaces_(std::move(node_interfaces)), + buffer_(buffer), + queue_size_(queue_size), + buffer_timeout_(buffer_timeout) + { + init(); + setTargetFrame(target_frame); + } + /** * \brief Constructor * @@ -162,14 +189,14 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi * \param buffer_timeout The timeout duration after requesting transforms from the buffer. */ template + [[deprecated("Use rclcpp::node_interfaces::NodeInterfaces instead of rclcpp::Node")]] MessageFilter( BufferT & buffer, const std::string & target_frame, uint32_t queue_size, const rclcpp::Node::SharedPtr & node, std::chrono::duration buffer_timeout = std::chrono::duration::max()) : MessageFilter( - buffer, target_frame, queue_size, node->get_node_logging_interface(), - node->get_node_clock_interface(), buffer_timeout) + buffer, target_frame, queue_size, *node, buffer_timeout) { static_assert( std::is_base_of::value, @@ -190,20 +217,48 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi * \param buffer_timeout The timeout duration after requesting transforms from the buffer. */ template + [[deprecated("Use rclcpp::node_interfaces::NodeInterfaces instead of multiple interfaces")]] MessageFilter( BufferT & buffer, const std::string & target_frame, uint32_t queue_size, const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & node_logging, const rclcpp::node_interfaces::NodeClockInterface::SharedPtr & node_clock, std::chrono::duration buffer_timeout = std::chrono::duration::max()) - : node_logging_(node_logging), - node_clock_(node_clock), + : MessageFilter( + buffer, target_frame, queue_size, + rclcpp::node_interfaces::NodeInterfaces< + rclcpp::node_interfaces::NodeLoggingInterface, + rclcpp::node_interfaces::NodeClockInterface>(node_logging, node_clock), + buffer_timeout) + { + } + + /** + * \brief Constructor + * + * \param f The filter to connect this filter's input to. Often will be a message_filters::Subscriber. + * \param buffer The buffer this filter should use. + * \param target_frame The frame this filter should attempt to transform to. To use multiple frames, pass an empty string here and use the setTargetFrames() function. + * \param queue_size The number of messages to queue up before throwing away old ones. 0 means infinite (dangerous). + * \param node_interfaces The ros2 node interfaces (logging and clock) used for various operations. + * \param buffer_timeout The timeout duration after requesting transforms from the buffer. + */ + template + MessageFilter( + F & f, BufferT & buffer, const std::string & target_frame, uint32_t queue_size, + rclcpp::node_interfaces::NodeInterfaces< + rclcpp::node_interfaces::NodeLoggingInterface, + rclcpp::node_interfaces::NodeClockInterface> node_interfaces, + std::chrono::duration buffer_timeout = + std::chrono::duration::max()) + : node_interfaces_(std::move(node_interfaces)), buffer_(buffer), queue_size_(queue_size), buffer_timeout_(buffer_timeout) { init(); setTargetFrame(target_frame); + connectInput(f); } /** @@ -217,14 +272,13 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi * \param buffer_timeout The timeout duration after requesting transforms from the buffer. */ template + [[deprecated("Use rclcpp::node_interfaces::NodeInterfaces instead of rclcpp::Node")]] MessageFilter( F & f, BufferT & buffer, const std::string & target_frame, uint32_t queue_size, const rclcpp::Node::SharedPtr & node, std::chrono::duration buffer_timeout = std::chrono::duration::max()) - : MessageFilter( - f, buffer, target_frame, queue_size, node->get_node_logging_interface(), - node->get_node_clock_interface(), buffer_timeout) + : MessageFilter(f, buffer, target_frame, queue_size, *node, buffer_timeout) { } @@ -240,21 +294,20 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi * \param buffer_timeout The timeout duration after requesting transforms from the buffer. */ template + [[deprecated("Use rclcpp::node_interfaces::NodeInterfaces instead of multiple interfaces")]] MessageFilter( F & f, BufferT & buffer, const std::string & target_frame, uint32_t queue_size, const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & node_logging, const rclcpp::node_interfaces::NodeClockInterface::SharedPtr & node_clock, std::chrono::duration buffer_timeout = std::chrono::duration::max()) - : node_logging_(node_logging), - node_clock_(node_clock), - buffer_(buffer), - queue_size_(queue_size), - buffer_timeout_(buffer_timeout) + : MessageFilter( + f, buffer, target_frame, queue_size, + rclcpp::node_interfaces::NodeInterfaces< + rclcpp::node_interfaces::NodeLoggingInterface, + rclcpp::node_interfaces::NodeClockInterface>(node_logging, node_clock), + buffer_timeout) { - init(); - setTargetFrame(target_frame); - connectInput(f); } /** @@ -458,7 +511,7 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi */ void add(const MConstPtr & message) { - auto t = node_clock_->get_clock()->now(); + auto t = node_interfaces_.get_node_clock_interface()->get_clock()->now(); add(MEvent(message, t)); } @@ -608,10 +661,11 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi void checkFailures() { if (!next_failure_warning_.nanoseconds()) { - next_failure_warning_ = node_clock_->get_clock()->now() + rclcpp::Duration(15, 0); + next_failure_warning_ = node_interfaces_.get_node_clock_interface()->get_clock()->now() + + rclcpp::Duration(15, 0); } - if (node_clock_->get_clock()->now() >= next_failure_warning_) { + if (node_interfaces_.get_node_clock_interface()->get_clock()->now() >= next_failure_warning_) { if (incoming_message_count_ - messages_.size() == 0) { return; } @@ -624,7 +678,8 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi "[tf2_ros_message_filter.message_notifier] rosconsole logger to DEBUG for more " "information.", dropped_pct * 100); - next_failure_warning_ = node_clock_->get_clock()->now() + rclcpp::Duration(60, 0); + next_failure_warning_ = node_interfaces_.get_node_clock_interface()->get_clock()->now() + + rclcpp::Duration(60, 0); if (static_cast(failed_out_the_back_count_) / static_cast(dropped_message_count_) > 0.5) @@ -706,7 +761,7 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi std::string frame_id = stripSlash(mt::FrameId::value(*message)); rclcpp::Time stamp = mt::TimeStamp::value(*message); RCLCPP_INFO( - node_logging_->get_logger(), + node_interfaces_.get_node_logging_interface()->get_logger(), "Message Filter dropping message: frame '%s' at time %.3f for reason '%s'", frame_id.c_str(), stamp.seconds(), get_filter_failure_reason_string(reason).c_str()); } @@ -722,10 +777,11 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi return in; } - ///< The node logging interface to use for any log messages - const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_; - ///< The node clock interface to use to get the clock to use - const rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_; + ///< The interfaces (logging and clock) to use to get the log messages and clock. + rclcpp::node_interfaces::NodeInterfaces< + rclcpp::node_interfaces::NodeLoggingInterface, + rclcpp::node_interfaces::NodeClockInterface> node_interfaces_; + ///< The Transformer used to determine if transformation data is available BufferT & buffer_; ///< The frames we need to be able to transform to before a message is ready diff --git a/tf2_ros/include/tf2_ros/static_transform_broadcaster.h b/tf2_ros/include/tf2_ros/static_transform_broadcaster.h index a0d4f15b7..82470fd67 100644 --- a/tf2_ros/include/tf2_ros/static_transform_broadcaster.h +++ b/tf2_ros/include/tf2_ros/static_transform_broadcaster.h @@ -40,6 +40,7 @@ #include "rclcpp/node_interfaces/get_node_parameters_interface.hpp" #include "rclcpp/node_interfaces/get_node_topics_interface.hpp" +#include "rclcpp/node_interfaces/node_interfaces.hpp" #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" #include "tf2_msgs/msg/tf_message.hpp" @@ -55,6 +56,30 @@ namespace tf2_ros class StaticTransformBroadcaster { public: + /** \brief NodeInterfaces constructor */ + template> + StaticTransformBroadcaster( + rclcpp::node_interfaces::NodeInterfaces< + rclcpp::node_interfaces::NodeParametersInterface, + rclcpp::node_interfaces::NodeTopicsInterface> node_interfaces, + const rclcpp::QoS & qos = DynamicBroadcasterQoS(), + const rclcpp::PublisherOptionsWithAllocator & options = [] () { + rclcpp::PublisherOptionsWithAllocator options; + options.qos_overriding_options = rclcpp::QosOverridingOptions{ + rclcpp::QosPolicyKind::Depth, + rclcpp::QosPolicyKind::Durability, + rclcpp::QosPolicyKind::History, + rclcpp::QosPolicyKind::Reliability}; + return options; + } ()) + { + auto node_parameters = node_interfaces.get_node_parameters_interface(); + auto node_topics = node_interfaces.get_node_topics_interface(); + + publisher_ = rclcpp::create_publisher( + node_parameters, node_topics, "/tf", qos, options); + } + /** \brief Node constructor */ template> StaticTransformBroadcaster( @@ -68,15 +93,13 @@ class StaticTransformBroadcaster rclcpp::QosPolicyKind::Reliability}; return options; } ()) - : StaticTransformBroadcaster( - rclcpp::node_interfaces::get_node_parameters_interface(node), - rclcpp::node_interfaces::get_node_topics_interface(node), - qos, - options) - {} + { + StaticTransformBroadcaster(node, qos, options); + } /** \brief Node interfaces constructor */ template> + [[deprecated("Use rclcpp::node_interfaces::NodeInterfaces instead of multiple interfaces")]] StaticTransformBroadcaster( rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters, rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics, @@ -89,10 +112,11 @@ class StaticTransformBroadcaster rclcpp::QosPolicyKind::Reliability}; return options; } ()) - { - publisher_ = rclcpp::create_publisher( - node_parameters, node_topics, "/tf_static", qos, options); - } + : StaticTransformBroadcaster( + rclcpp::node_interfaces::NodeInterfaces(node_parameters, node_topics), + qos, + options) + {} /** \brief Send a TransformStamped message * The stamped data structure includes frame_id, and time, and parent_id already. */ diff --git a/tf2_ros/include/tf2_ros/transform_broadcaster.h b/tf2_ros/include/tf2_ros/transform_broadcaster.h index e2cab966f..32eb43ad1 100644 --- a/tf2_ros/include/tf2_ros/transform_broadcaster.h +++ b/tf2_ros/include/tf2_ros/transform_broadcaster.h @@ -40,6 +40,7 @@ #include "rclcpp/node_interfaces/get_node_parameters_interface.hpp" #include "rclcpp/node_interfaces/get_node_topics_interface.hpp" +#include "rclcpp/node_interfaces/node_interfaces.hpp" #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" #include "tf2_msgs/msg/tf_message.hpp" @@ -55,10 +56,34 @@ namespace tf2_ros class TransformBroadcaster { public: + /** \brief NodeInterfaces constructor */ + template> + TransformBroadcaster( + rclcpp::node_interfaces::NodeInterfaces< + rclcpp::node_interfaces::NodeParametersInterface, + rclcpp::node_interfaces::NodeTopicsInterface> node_interfaces, + const rclcpp::QoS & qos = DynamicBroadcasterQoS(), + const rclcpp::PublisherOptionsWithAllocator & options = [] () { + rclcpp::PublisherOptionsWithAllocator options; + options.qos_overriding_options = rclcpp::QosOverridingOptions{ + rclcpp::QosPolicyKind::Depth, + rclcpp::QosPolicyKind::Durability, + rclcpp::QosPolicyKind::History, + rclcpp::QosPolicyKind::Reliability}; + return options; + } ()) + { + auto node_parameters = node_interfaces.get_node_parameters_interface(); + auto node_topics = node_interfaces.get_node_topics_interface(); + + publisher_ = rclcpp::create_publisher( + node_parameters, node_topics, "/tf", qos, options); + } + /** \brief Node constructor */ - template> + template> TransformBroadcaster( - NodeT && node, + NodeT && node = NodeT(), const rclcpp::QoS & qos = DynamicBroadcasterQoS(), const rclcpp::PublisherOptionsWithAllocator & options = [] () { rclcpp::PublisherOptionsWithAllocator options; @@ -69,15 +94,13 @@ class TransformBroadcaster rclcpp::QosPolicyKind::Reliability}; return options; } ()) - : TransformBroadcaster( - rclcpp::node_interfaces::get_node_parameters_interface(node), - rclcpp::node_interfaces::get_node_topics_interface(node), - qos, - options) - {} + { + TransformBroadcaster(node, qos, options); + } /** \brief Node interfaces constructor */ template> + [[deprecated("Use rclcpp::node_interfaces::NodeInterfaces instead of multiple interfaces")]] TransformBroadcaster( rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters, rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics, @@ -91,10 +114,12 @@ class TransformBroadcaster rclcpp::QosPolicyKind::Reliability}; return options; } ()) - { - publisher_ = rclcpp::create_publisher( - node_parameters, node_topics, "/tf", qos, options); - } + : TransformBroadcaster( + rclcpp::node_interfaces::NodeInterfaces(node_parameters, node_topics), + qos, + options) + {} + /** \brief Send a TransformStamped message * diff --git a/tf2_ros/include/tf2_ros/transform_listener.h b/tf2_ros/include/tf2_ros/transform_listener.h index 59eb841e0..fb6593a48 100644 --- a/tf2_ros/include/tf2_ros/transform_listener.h +++ b/tf2_ros/include/tf2_ros/transform_listener.h @@ -43,6 +43,7 @@ #include "tf2_msgs/msg/tf_message.hpp" #include "rclcpp/rclcpp.hpp" +#include "rclcpp/node_interfaces/node_interfaces.hpp" #include "tf2_ros/qos.hpp" @@ -91,33 +92,54 @@ class TransformListener TF2_ROS_PUBLIC explicit TransformListener(tf2::BufferCore & buffer, bool spin_thread = true); - /** \brief Node constructor */ - template> + /** \brief NodeInterfaces constructor */ + template> TransformListener( tf2::BufferCore & buffer, - NodeT && node, + rclcpp::node_interfaces::NodeInterfaces< + rclcpp::node_interfaces::NodeBaseInterface, + rclcpp::node_interfaces::NodeLoggingInterface, + rclcpp::node_interfaces::NodeParametersInterface, + rclcpp::node_interfaces::NodeTopicsInterface> node_interfaces, bool spin_thread = true, const rclcpp::QoS & qos = DynamicListenerQoS(), const rclcpp::QoS & static_qos = StaticListenerQoS(), const rclcpp::SubscriptionOptionsWithAllocator & options = detail::get_default_transform_listener_sub_options(), const rclcpp::SubscriptionOptionsWithAllocator & static_options = - detail::get_default_transform_listener_static_sub_options()) - : TransformListener( - buffer, - node->get_node_base_interface(), - node->get_node_logging_interface(), - node->get_node_parameters_interface(), - node->get_node_topics_interface(), + detail::get_default_transform_listener_static_sub_options() + ) + : buffer_(buffer), node_interfaces_(std::move(node_interfaces)) + { + init( + node_interfaces, spin_thread, qos, static_qos, options, - static_options) - {} + static_options); + } + + /** \brief Node constructor */ + template> + TransformListener( + tf2::BufferCore & buffer, + NodeT && node, + bool spin_thread = true, + const rclcpp::QoS & qos = DynamicListenerQoS(), + const rclcpp::QoS & static_qos = StaticListenerQoS(), + const rclcpp::SubscriptionOptionsWithAllocator & options = + detail::get_default_transform_listener_sub_options(), + const rclcpp::SubscriptionOptionsWithAllocator & static_options = + detail::get_default_transform_listener_static_sub_options()) + : buffer_(buffer) + { + TransformListener(buffer, node, spin_thread, qos, static_qos, options, static_options); + } /** \brief Node interface constructor */ template> + [[deprecated("Use rclcpp::node_interfaces::NodeInterfaces instead of multiple interfaces")]] TransformListener( tf2::BufferCore & buffer, rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, @@ -134,10 +156,15 @@ class TransformListener : buffer_(buffer) { init( - node_base, - node_logging, - node_parameters, - node_topics, + rclcpp::node_interfaces::NodeInterfaces< + rclcpp::node_interfaces::NodeBaseInterface, + rclcpp::node_interfaces::NodeLoggingInterface, + rclcpp::node_interfaces::NodeParametersInterface, + rclcpp::node_interfaces::NodeTopicsInterface>( + node_base, + node_logging, + node_parameters, + node_topics), spin_thread, qos, static_qos, @@ -155,19 +182,22 @@ class TransformListener private: 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, + rclcpp::node_interfaces::NodeInterfaces< + rclcpp::node_interfaces::NodeBaseInterface, + rclcpp::node_interfaces::NodeLoggingInterface, + rclcpp::node_interfaces::NodeParametersInterface, + rclcpp::node_interfaces::NodeTopicsInterface> node_interfaces, bool spin_thread, const rclcpp::QoS & qos, const rclcpp::QoS & static_qos, const rclcpp::SubscriptionOptionsWithAllocator & options, const rclcpp::SubscriptionOptionsWithAllocator & static_options) { + auto node_base = node_interfaces.get_node_base_interface(); + auto node_parameters = node_interfaces.get_node_parameters_interface(); + auto node_topics = node_interfaces.get_node_topics_interface(); + spin_thread_ = spin_thread; - node_base_interface_ = node_base; - node_logging_interface_ = node_logging; using callback_t = std::function; callback_t cb = std::bind( @@ -177,7 +207,7 @@ class TransformListener if (spin_thread_) { // Create new callback group for message_subscription of tf and tf_static - callback_group_ = node_base_interface_->create_callback_group( + callback_group_ = node_base->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive, false); // Duplicate to modify option of subscription rclcpp::SubscriptionOptionsWithAllocator tf_options = options; @@ -197,7 +227,7 @@ class TransformListener // Create executor with dedicated thread to spin. executor_ = std::make_shared(); - executor_->add_callback_group(callback_group_, node_base_interface_); + executor_->add_callback_group(callback_group_, node_base); dedicated_listener_thread_ = std::make_unique([&]() {executor_->spin();}); // Tell the buffer we have a dedicated thread to enable timeouts buffer_.setUsingDedicatedThread(true); @@ -225,9 +255,13 @@ class TransformListener message_subscription_tf_static_ {nullptr}; tf2::BufferCore & buffer_; tf2::TimePoint last_update_; - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface_ {nullptr}; - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface_ {nullptr}; rclcpp::CallbackGroup::SharedPtr callback_group_{nullptr}; + + rclcpp::node_interfaces::NodeInterfaces< + rclcpp::node_interfaces::NodeBaseInterface, + rclcpp::node_interfaces::NodeLoggingInterface, + rclcpp::node_interfaces::NodeParametersInterface, + rclcpp::node_interfaces::NodeTopicsInterface> node_interfaces_; }; } // namespace tf2_ros diff --git a/tf2_ros/src/buffer.cpp b/tf2_ros/src/buffer.cpp index 10ee57cc2..29d0d4336 100644 --- a/tf2_ros/src/buffer.cpp +++ b/tf2_ros/src/buffer.cpp @@ -56,6 +56,45 @@ to_rclcpp(const tf2::Duration & duration) return rclcpp::Duration(std::chrono::nanoseconds(duration)); } +Buffer::Buffer( + rclcpp::Clock::SharedPtr clock, + rclcpp::node_interfaces::NodeInterfaces< + rclcpp::node_interfaces::NodeBaseInterface, + rclcpp::node_interfaces::NodeLoggingInterface, + rclcpp::node_interfaces::NodeServicesInterface + > node_interfaces, + tf2::Duration cache_time, + const rclcpp::QoS & qos) +: BufferCore(cache_time), clock_(clock), node_interfaces_(std::move(node_interfaces)), + timer_interface_(nullptr) +{ + if (nullptr == clock_) { + throw std::invalid_argument("clock must be a valid instance"); + } + + auto post_jump_cb = [this](const rcl_time_jump_t & jump_info) {onTimeJump(jump_info);}; + + rcl_jump_threshold_t jump_threshold; + // Disable forward jump callbacks + jump_threshold.min_forward.nanoseconds = 0; + // Anything backwards is a jump + jump_threshold.min_backward.nanoseconds = -1; + // Callback if the clock changes too + jump_threshold.on_clock_change = true; + + jump_handler_ = clock_->create_jump_callback(nullptr, post_jump_cb, jump_threshold); + + auto node_base = node_interfaces_.get_node_base_interface(); + auto node_services = node_interfaces_.get_node_services_interface(); + + node_logging_interface_ = node_interfaces_.get_node_logging_interface(); + + frames_server_ = rclcpp::create_service( + node_base, node_services, "tf2_frames", std::bind( + &Buffer::getFrames, this, std::placeholders::_1, + std::placeholders::_2), qos, nullptr); +} + geometry_msgs::msg::TransformStamped Buffer::lookupTransform( const std::string & target_frame, const std::string & source_frame, diff --git a/tf2_ros/src/buffer_server_main.cpp b/tf2_ros/src/buffer_server_main.cpp index 7b931b08e..ee81e881f 100644 --- a/tf2_ros/src/buffer_server_main.cpp +++ b/tf2_ros/src/buffer_server_main.cpp @@ -49,7 +49,7 @@ int main(int argc, char ** argv) auto node = std::make_shared("tf_buffer"); double buffer_size = node->declare_parameter("buffer_size", 120.0); - tf2_ros::Buffer buffer(node->get_clock(), tf2::durationFromSec(buffer_size)); + tf2_ros::Buffer buffer(node->get_clock(), *node, tf2::durationFromSec(buffer_size)); tf2_ros::TransformListener listener(buffer); tf2_ros::BufferServer buffer_server(buffer, node, "tf2_buffer_server"); diff --git a/tf2_ros/src/create_timer_ros.cpp b/tf2_ros/src/create_timer_ros.cpp index d90ce2053..99d2b3a08 100644 --- a/tf2_ros/src/create_timer_ros.cpp +++ b/tf2_ros/src/create_timer_ros.cpp @@ -41,12 +41,23 @@ namespace tf2_ros { +CreateTimerROS::CreateTimerROS( + rclcpp::node_interfaces::NodeInterfaces< + rclcpp::node_interfaces::NodeBaseInterface, + rclcpp::node_interfaces::NodeTimersInterface> node_interfaces, + rclcpp::CallbackGroup::SharedPtr callback_group) +: node_interfaces_(std::move(node_interfaces)), next_timer_handle_index_(0), + callback_group_(callback_group) +{ +} + CreateTimerROS::CreateTimerROS( rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, rclcpp::node_interfaces::NodeTimersInterface::SharedPtr node_timers, rclcpp::CallbackGroup::SharedPtr callback_group) -: node_base_(node_base), node_timers_(node_timers), next_timer_handle_index_(0), - callback_group_(callback_group) +: CreateTimerROS(rclcpp::node_interfaces::NodeInterfaces< + rclcpp::node_interfaces::NodeBaseInterface, + rclcpp::node_interfaces::NodeTimersInterface>(node_base, node_timers), callback_group) { } @@ -59,8 +70,8 @@ CreateTimerROS::createTimer( std::lock_guard lock(timers_map_mutex_); auto timer_handle_index = next_timer_handle_index_++; auto timer = rclcpp::create_timer>( - node_base_, - node_timers_, + node_interfaces_.get_node_base_interface(), + node_interfaces_.get_node_timers_interface(), clock, period, std::bind(&CreateTimerROS::timerCallback, this, timer_handle_index, callback), diff --git a/tf2_ros/src/tf2_echo.cpp b/tf2_ros/src/tf2_echo.cpp index 44bd777d7..cd7b81c7f 100644 --- a/tf2_ros/src/tf2_echo.cpp +++ b/tf2_ros/src/tf2_echo.cpp @@ -55,7 +55,7 @@ class echoListener std::shared_ptr tfl_; explicit echoListener(rclcpp::Clock::SharedPtr clock) - : buffer_(clock) + : buffer_(clock, *std::make_shared("default_node")) { tfl_ = std::make_shared(buffer_); } diff --git a/tf2_ros/src/tf2_monitor.cpp b/tf2_ros/src/tf2_monitor.cpp index 936527fac..7fdc85653 100644 --- a/tf2_ros/src/tf2_monitor.cpp +++ b/tf2_ros/src/tf2_monitor.cpp @@ -126,7 +126,7 @@ class TFMonitor using_specific_chain_(using_specific_chain), node_(node), clock_(node->get_clock()), - buffer_(clock_, tf2::Duration(tf2::BUFFER_CORE_DEFAULT_CACHE_TIME), node) + buffer_(clock_, *node, tf2::Duration(tf2::BUFFER_CORE_DEFAULT_CACHE_TIME)) { tf_ = std::make_shared(buffer_); diff --git a/tf2_ros/src/transform_listener.cpp b/tf2_ros/src/transform_listener.cpp index 5e31b6a8d..8dcd9c3d7 100644 --- a/tf2_ros/src/transform_listener.cpp +++ b/tf2_ros/src/transform_listener.cpp @@ -58,10 +58,7 @@ TransformListener::TransformListener(tf2::BufferCore & buffer, bool spin_thread) 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(), + *optional_default_node_, spin_thread, DynamicListenerQoS(), StaticListenerQoS(), detail::get_default_transform_listener_sub_options(), detail::get_default_transform_listener_static_sub_options()); @@ -89,7 +86,7 @@ void TransformListener::subscription_callback( // /\todo Use error reporting std::string temp = ex.what(); RCLCPP_ERROR( - node_logging_interface_->get_logger(), + node_interfaces_.get_node_logging_interface()->get_logger(), "Failure to set received transform from %s to %s with error: %s\n", msg_in.transforms[i].child_frame_id.c_str(), msg_in.transforms[i].header.frame_id.c_str(), temp.c_str()); diff --git a/tf2_ros/test/listener_unittest.cpp b/tf2_ros/test/listener_unittest.cpp index f604e8f4e..f71768c40 100644 --- a/tf2_ros/test/listener_unittest.cpp +++ b/tf2_ros/test/listener_unittest.cpp @@ -46,8 +46,8 @@ TEST(tf2_ros_test_listener, transform_listener) rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); - tf2_ros::Buffer buffer(clock); - tf2_ros::TransformListener tfl(buffer, node, false); + tf2_ros::Buffer buffer(clock, *node); + tf2_ros::TransformListener tfl(buffer, *node, false); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(node); diff --git a/tf2_ros/test/message_filter_test.cpp b/tf2_ros/test/message_filter_test.cpp index dd1634933..9445a6905 100644 --- a/tf2_ros/test/message_filter_test.cpp +++ b/tf2_ros/test/message_filter_test.cpp @@ -58,63 +58,66 @@ TEST(tf2_ros_message_filter, construction_and_destruction) { auto node = rclcpp::Node::make_shared("test_message_filter_node"); rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); - tf2_ros::Buffer buffer(clock); + tf2_ros::Buffer buffer(clock, *node); // Node constructor with defaults { - tf2_ros::MessageFilter filter(buffer, "map", 10, node); + tf2_ros::MessageFilter filter(buffer, "map", 10, *node); } // Node constructor no defaults { tf2_ros::MessageFilter filter( - buffer, "map", 10, node, std::chrono::milliseconds(100)); + buffer, "map", 10, *node, std::chrono::milliseconds(100)); } // Node interface constructor with defaults { tf2_ros::MessageFilter filter( - buffer, "map", 10, node->get_node_logging_interface(), node->get_node_clock_interface()); - } + buffer, "map", 10, rclcpp::node_interfaces::NodeInterfaces< + rclcpp::node_interfaces::NodeLoggingInterface, + rclcpp::node_interfaces::NodeClockInterface>( + node->get_node_logging_interface(), node->get_node_clock_interface())); + } // Node interface constructor no defaults { tf2_ros::MessageFilter filter( - buffer, - "map", - 10, - node->get_node_logging_interface(), - node->get_node_clock_interface(), + buffer, "map", 10, rclcpp::node_interfaces::NodeInterfaces< + rclcpp::node_interfaces::NodeLoggingInterface, + rclcpp::node_interfaces::NodeClockInterface>( + node->get_node_logging_interface(), node->get_node_clock_interface()), std::chrono::seconds(42)); } message_filters::Subscriber sub; // Filter + node constructor with defaults { - tf2_ros::MessageFilter filter(sub, buffer, "map", 10, node); + tf2_ros::MessageFilter filter(sub, buffer, "map", 10, *node); } // Filter + node constructor no defaults { tf2_ros::MessageFilter filter( - sub, buffer, "map", 10, node, std::chrono::hours(1)); + sub, buffer, "map", 10, *node, std::chrono::hours(1)); } // Filter + node interface constructor with defaults { tf2_ros::MessageFilter filter( - sub, buffer, "map", 10, node->get_node_logging_interface(), node->get_node_clock_interface()); + sub, buffer, "map", 10, rclcpp::node_interfaces::NodeInterfaces< + rclcpp::node_interfaces::NodeLoggingInterface, + rclcpp::node_interfaces::NodeClockInterface>( + node->get_node_logging_interface(), node->get_node_clock_interface())); } // Filter + node interface constructor no defaults { tf2_ros::MessageFilter filter( - sub, - buffer, - "map", - 10, - node->get_node_logging_interface(), - node->get_node_clock_interface(), + sub, buffer, "map", 10, rclcpp::node_interfaces::NodeInterfaces< + rclcpp::node_interfaces::NodeLoggingInterface, + rclcpp::node_interfaces::NodeClockInterface>( + node->get_node_logging_interface(), node->get_node_clock_interface()), std::chrono::microseconds(0)); } } @@ -123,9 +126,7 @@ TEST(tf2_ros_message_filter, multiple_frames_and_time_tolerance) { auto node = rclcpp::Node::make_shared("tf2_ros_message_filter"); - auto create_timer_interface = std::make_shared( - node->get_node_base_interface(), - node->get_node_timers_interface()); + auto create_timer_interface = std::make_shared(*node); rclcpp::QoS default_qos = rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); @@ -133,10 +134,10 @@ TEST(tf2_ros_message_filter, multiple_frames_and_time_tolerance) sub.subscribe(node, "point", default_qos); rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); - tf2_ros::Buffer buffer(clock); + tf2_ros::Buffer buffer(clock, *node); buffer.setCreateTimerInterface(create_timer_interface); tf2_ros::TransformListener tfl(buffer); - tf2_ros::MessageFilter filter(buffer, "map", 10, node); + tf2_ros::MessageFilter filter(buffer, "map", 10, *node); filter.connectInput(sub); filter.registerCallback(&filter_callback); @@ -149,7 +150,7 @@ TEST(tf2_ros_message_filter, multiple_frames_and_time_tolerance) filter.setTolerance(rclcpp::Duration(1, 0)); // Publish static transforms so the frame transformations will always be valid - tf2_ros::StaticTransformBroadcaster tfb(node); + tf2_ros::StaticTransformBroadcaster tfb(*node); geometry_msgs::msg::TransformStamped map_to_odom; map_to_odom.header.stamp = rclcpp::Time(0, 0); map_to_odom.header.frame_id = "map"; diff --git a/tf2_ros/test/node_wrapper.hpp b/tf2_ros/test/node_wrapper.hpp index 2534f99fe..a33870001 100644 --- a/tf2_ros/test/node_wrapper.hpp +++ b/tf2_ros/test/node_wrapper.hpp @@ -55,6 +55,9 @@ class NodeWrapper rclcpp::node_interfaces::NodeParametersInterface::SharedPtr get_node_parameters_interface() {return this->node->get_node_parameters_interface();} + rclcpp::node_interfaces::NodeServicesInterface::SharedPtr + get_node_services_interface() {return this->node->get_node_services_interface();} + private: rclcpp::Node::SharedPtr node; }; diff --git a/tf2_ros/test/test_buffer.cpp b/tf2_ros/test/test_buffer.cpp index 69a10d8d9..2fa46447c 100644 --- a/tf2_ros/test/test_buffer.cpp +++ b/tf2_ros/test/test_buffer.cpp @@ -98,9 +98,10 @@ class MockCreateTimerROS final : public tf2_ros::CreateTimerROS { public: MockCreateTimerROS( - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, - rclcpp::node_interfaces::NodeTimersInterface::SharedPtr node_timers) - : CreateTimerROS(node_base, node_timers), next_timer_handle_index_(0) + rclcpp::node_interfaces::NodeInterfaces< + rclcpp::node_interfaces::NodeBaseInterface, + rclcpp::node_interfaces::NodeTimersInterface> node_interfaces) + : CreateTimerROS(node_interfaces), next_timer_handle_index_(0) { } @@ -141,13 +142,17 @@ class MockCreateTimerROS final : public tf2_ros::CreateTimerROS TEST(test_buffer, construct_with_null_clock) { - EXPECT_THROW(tf2_ros::Buffer(nullptr), std::invalid_argument); + std::shared_ptr node = std::make_shared("default_node"); + + EXPECT_THROW(tf2_ros::Buffer(nullptr, *node), std::invalid_argument); } TEST(test_buffer, can_transform_valid_transform) { + std::shared_ptr node = std::make_shared("default_node"); + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); - tf2_ros::Buffer buffer(clock); + tf2_ros::Buffer buffer(clock, *node); // Silence error about dedicated thread's being necessary buffer.setUsingDedicatedThread(true); @@ -186,8 +191,10 @@ TEST(test_buffer, can_transform_valid_transform) TEST(test_buffer, velocity_transform) { + std::shared_ptr node = std::make_shared("default_node"); + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); - tf2_ros::Buffer buffer(clock); + tf2_ros::Buffer buffer(clock, *node); // Silence error about dedicated thread's being necessary buffer.setUsingDedicatedThread(true); @@ -247,8 +254,10 @@ TEST(test_buffer, velocity_transform) TEST(test_buffer, test_twist) { + std::shared_ptr node = std::make_shared("default_node"); + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); - tf2_ros::Buffer buffer(clock); + tf2_ros::Buffer buffer(clock, *node); // Silence error about dedicated thread's being necessary buffer.setUsingDedicatedThread(true); @@ -294,8 +303,10 @@ TEST(test_buffer, test_twist) TEST(test_buffer, can_transform_without_dedicated_thread) { + std::shared_ptr node = std::make_shared("default_node"); + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); - tf2_ros::Buffer buffer(clock); + tf2_ros::Buffer buffer(clock, *node); buffer.setUsingDedicatedThread(false); rclcpp::Time rclcpp_time = clock->now(); @@ -337,8 +348,10 @@ TEST(test_buffer, can_transform_without_dedicated_thread) TEST(test_buffer, wait_for_transform_valid) { + std::shared_ptr node = std::make_shared("default_node"); + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); - tf2_ros::Buffer buffer(clock); + tf2_ros::Buffer buffer(clock, *node); // Silence error about dedicated thread's being necessary buffer.setUsingDedicatedThread(true); auto mock_create_timer = std::make_shared(); @@ -398,8 +411,10 @@ TEST(test_buffer, wait_for_transform_valid) TEST(test_buffer, wait_for_transform_timeout) { + std::shared_ptr node = std::make_shared("default_node"); + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); - tf2_ros::Buffer buffer(clock); + tf2_ros::Buffer buffer(clock, *node); // Silence error about dedicated thread's being necessary buffer.setUsingDedicatedThread(true); auto mock_create_timer = std::make_shared(); @@ -447,8 +462,10 @@ TEST(test_buffer, wait_for_transform_timeout) // Regression test for https://github.com/ros2/geometry2/issues/141 TEST(test_buffer, wait_for_transform_race) { + std::shared_ptr node = std::make_shared("default_node"); + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); - tf2_ros::Buffer buffer(clock); + tf2_ros::Buffer buffer(clock, *node); // Silence error about dedicated thread's being necessary buffer.setUsingDedicatedThread(true); auto mock_create_timer = std::make_shared(); @@ -502,12 +519,10 @@ TEST(test_buffer, timer_ros_wait_for_transform_race) "timer_ros_wait_for_transform_race"); rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); - tf2_ros::Buffer buffer(clock); + tf2_ros::Buffer buffer(clock, *rclcpp_node_); // Silence error about dedicated thread's being necessary buffer.setUsingDedicatedThread(true); - auto mock_create_timer_ros = std::make_shared( - rclcpp_node_->get_node_base_interface(), - rclcpp_node_->get_node_timers_interface()); + auto mock_create_timer_ros = std::make_shared(*rclcpp_node_); buffer.setCreateTimerInterface(mock_create_timer_ros); rclcpp::Time rclcpp_time = clock->now(); diff --git a/tf2_ros/test/test_static_transform_broadcaster.cpp b/tf2_ros/test/test_static_transform_broadcaster.cpp index 37dbbfcc7..f9741361c 100644 --- a/tf2_ros/test/test_static_transform_broadcaster.cpp +++ b/tf2_ros/test/test_static_transform_broadcaster.cpp @@ -81,8 +81,11 @@ TEST(tf2_test_static_transform_broadcaster, transform_broadcaster_rclcpp_node) // Construct static tf broadcaster from node interfaces { tf2_ros::StaticTransformBroadcaster tfb( + rclcpp::node_interfaces::NodeInterfaces< + rclcpp::node_interfaces::NodeParametersInterface, + rclcpp::node_interfaces::NodeTopicsInterface>( node->get_node_parameters_interface(), - node->get_node_topics_interface()); + node->get_node_topics_interface())); } } @@ -110,8 +113,11 @@ TEST(tf2_test_static_transform_broadcaster, transform_broadcaster_custom_rclcpp_ // Construct static tf broadcaster from node interfaces { tf2_ros::StaticTransformBroadcaster tfb( + rclcpp::node_interfaces::NodeInterfaces< + rclcpp::node_interfaces::NodeParametersInterface, + rclcpp::node_interfaces::NodeTopicsInterface>( node->get_node_parameters_interface(), - node->get_node_topics_interface()); + node->get_node_topics_interface())); } } diff --git a/tf2_ros/test/test_transform_broadcaster.cpp b/tf2_ros/test/test_transform_broadcaster.cpp index 499029b98..7554e3bc1 100644 --- a/tf2_ros/test/test_transform_broadcaster.cpp +++ b/tf2_ros/test/test_transform_broadcaster.cpp @@ -64,8 +64,11 @@ TEST(tf2_test_transform_broadcaster, transform_broadcaster_rclcpp_node) // Construct tf broadcaster from node interfaces { tf2_ros::TransformBroadcaster tfb( + rclcpp::node_interfaces::NodeInterfaces< + rclcpp::node_interfaces::NodeParametersInterface, + rclcpp::node_interfaces::NodeTopicsInterface>( node->get_node_parameters_interface(), - node->get_node_topics_interface()); + node->get_node_topics_interface())); } } @@ -84,8 +87,11 @@ TEST(tf2_test_transform_broadcaster, transform_broadcaster_custom_rclcpp_node) // Construct tf broadcaster from node interfaces { tf2_ros::TransformBroadcaster tfb( + rclcpp::node_interfaces::NodeInterfaces< + rclcpp::node_interfaces::NodeParametersInterface, + rclcpp::node_interfaces::NodeTopicsInterface>( node->get_node_parameters_interface(), - node->get_node_topics_interface()); + node->get_node_topics_interface())); } } diff --git a/tf2_ros/test/test_transform_listener.cpp b/tf2_ros/test/test_transform_listener.cpp index d6e28f6ea..30de753e4 100644 --- a/tf2_ros/test/test_transform_listener.cpp +++ b/tf2_ros/test/test_transform_listener.cpp @@ -45,8 +45,8 @@ class CustomNode : public rclcpp::Node void init_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); + tf2_ros::Buffer buffer(clock, *this); + tf_listener_ = std::make_shared(buffer, *shared_from_this(), false); } private: @@ -63,8 +63,8 @@ class CustomComposableNode : public rclcpp::Node void init_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); + tf2_ros::Buffer buffer(clock, *this); + tf_listener_ = std::make_shared(buffer, *shared_from_this(), false); } private: @@ -76,8 +76,8 @@ TEST(tf2_test_transform_listener, transform_listener_rclcpp_node) auto node = rclcpp::Node::make_shared("tf2_ros_message_filter"); rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); - tf2_ros::Buffer buffer(clock); - tf2_ros::TransformListener tfl(buffer, node, false); + tf2_ros::Buffer buffer(clock, *node); + tf2_ros::TransformListener tfl(buffer, *node, false); } TEST(tf2_test_transform_listener, transform_listener_custom_rclcpp_node) @@ -85,8 +85,8 @@ TEST(tf2_test_transform_listener, transform_listener_custom_rclcpp_node) auto node = std::make_shared("tf2_ros_message_filter"); rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); - tf2_ros::Buffer buffer(clock); - tf2_ros::TransformListener tfl(buffer, node, false); + tf2_ros::Buffer buffer(clock, *node); + tf2_ros::TransformListener tfl(buffer, *node, false); } TEST(tf2_test_transform_listener, transform_listener_as_member) diff --git a/tf2_ros/test/time_reset_test.cpp b/tf2_ros/test/time_reset_test.cpp index 44cbd249c..175c10931 100644 --- a/tf2_ros/test/time_reset_test.cpp +++ b/tf2_ros/test/time_reset_test.cpp @@ -56,9 +56,9 @@ TEST(tf2_ros_time_reset_test, time_backwards) "transform_listener_backwards_reset"); rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); - tf2_ros::Buffer buffer(clock); - tf2_ros::TransformListener tfl(buffer); - tf2_ros::TransformBroadcaster tfb(node_); + tf2_ros::Buffer buffer(clock, *node_); + tf2_ros::TransformListener tfl(buffer, *node_); + tf2_ros::TransformBroadcaster tfb(*node_); auto clock_pub = node_->create_publisher("/clock", 1); From 0666135370cf341a22bbad681abeaa37300890fb Mon Sep 17 00:00:00 2001 From: CursedRock17 Date: Sun, 13 Oct 2024 02:21:43 -0400 Subject: [PATCH 2/2] Rebase/Test fix Signed-off-by: CursedRock17 --- tf2_ros/include/tf2_ros/transform_listener.h | 38 +++++++++++++++++--- tf2_ros/test/test_transform_broadcaster.cpp | 2 +- tf2_ros/test/test_transform_listener.cpp | 30 ++++++++-------- 3 files changed, 49 insertions(+), 21 deletions(-) diff --git a/tf2_ros/include/tf2_ros/transform_listener.h b/tf2_ros/include/tf2_ros/transform_listener.h index ea15ae3b9..f356bd807 100644 --- a/tf2_ros/include/tf2_ros/transform_listener.h +++ b/tf2_ros/include/tf2_ros/transform_listener.h @@ -294,6 +294,30 @@ class StaticTransformListener : public TransformListener { } + /** \brief NodeInterfaces constructor */ + template> + StaticTransformListener( + tf2::BufferCore & buffer, + rclcpp::node_interfaces::NodeInterfaces< + rclcpp::node_interfaces::NodeBaseInterface, + rclcpp::node_interfaces::NodeLoggingInterface, + rclcpp::node_interfaces::NodeParametersInterface, + rclcpp::node_interfaces::NodeTopicsInterface> node_interfaces, + 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_interfaces, + spin_thread, + rclcpp::QoS(1), + static_qos, + rclcpp::SubscriptionOptionsWithAllocator(), + static_options) + { + } + /** \brief Node constructor */ template> StaticTransformListener( @@ -310,13 +334,13 @@ class StaticTransformListener : public TransformListener rclcpp::QoS(1), static_qos, rclcpp::SubscriptionOptionsWithAllocator(), - static_options, - true) + static_options) { } /** \brief Node interface constructor */ template> + [[deprecated("Use rclcpp::node_interfaces::NodeInterfaces instead of multiple interfaces")]] StaticTransformListener( tf2::BufferCore & buffer, rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base, @@ -329,16 +353,20 @@ class StaticTransformListener : public TransformListener detail::get_default_transform_listener_static_sub_options()) : TransformListener( buffer, + rclcpp::node_interfaces::NodeInterfaces< + rclcpp::node_interfaces::NodeBaseInterface, + rclcpp::node_interfaces::NodeLoggingInterface, + rclcpp::node_interfaces::NodeParametersInterface, + rclcpp::node_interfaces::NodeTopicsInterface>( node_base, node_logging, node_parameters, - node_topics, + node_topics), spin_thread, rclcpp::QoS(1), static_qos, rclcpp::SubscriptionOptionsWithAllocator(), - static_options, - true) + static_options) { } diff --git a/tf2_ros/test/test_transform_broadcaster.cpp b/tf2_ros/test/test_transform_broadcaster.cpp index 7554e3bc1..017da5982 100644 --- a/tf2_ros/test/test_transform_broadcaster.cpp +++ b/tf2_ros/test/test_transform_broadcaster.cpp @@ -43,7 +43,7 @@ class CustomNode : public rclcpp::Node void init_tf_broadcaster() { - tf_broadcaster_ = std::make_shared(shared_from_this()); + tf_broadcaster_ = std::make_shared(*this); } private: diff --git a/tf2_ros/test/test_transform_listener.cpp b/tf2_ros/test/test_transform_listener.cpp index 25bf3c409..dcc3bd9b3 100644 --- a/tf2_ros/test/test_transform_listener.cpp +++ b/tf2_ros/test/test_transform_listener.cpp @@ -49,15 +49,15 @@ class CustomNode : public rclcpp::Node { rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); tf2_ros::Buffer buffer(clock, *this); - tf_listener_ = std::make_shared(buffer, *shared_from_this(), false); + tf_listener_ = std::make_shared(buffer, *this, false); } void init_static_tf_listener() { rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); - tf2_ros::Buffer buffer(clock); + tf2_ros::Buffer buffer(clock, *this); tf_listener_ = - std::make_shared(buffer, shared_from_this(), false); + std::make_shared(buffer, *this, false); } private: @@ -75,15 +75,15 @@ class CustomComposableNode : public rclcpp::Node { rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); tf2_ros::Buffer buffer(clock, *this); - tf_listener_ = std::make_shared(buffer, *shared_from_this(), false); + tf_listener_ = std::make_shared(buffer, *this, false); } void init_static_tf_listener() { rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); - tf2_ros::Buffer buffer(clock); + tf2_ros::Buffer buffer(clock, *this); tf_listener_ = - std::make_shared(buffer, shared_from_this(), false); + std::make_shared(buffer, *this, false); } private: @@ -128,7 +128,7 @@ 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::Buffer buffer(clock, *node); } TEST(tf2_test_static_transform_listener, static_transform_listener_custom_rclcpp_node) @@ -136,8 +136,8 @@ TEST(tf2_test_static_transform_listener, static_transform_listener_custom_rclcpp 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); + tf2_ros::Buffer buffer(clock, *node); + tf2_ros::StaticTransformListener tfl(buffer, *node, false); } TEST(tf2_test_static_transform_listener, static_transform_listener_as_member) @@ -160,12 +160,12 @@ 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); + tf2_ros::Buffer dynamic_buffer(clock, *node); + tf2_ros::Buffer static_buffer(clock, *node); + 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();