diff --git a/test_tf2/test/test_buffer_server.cpp b/test_tf2/test/test_buffer_server.cpp index 24f519604..deb27a893 100644 --- a/test_tf2/test/test_buffer_server.cpp +++ b/test_tf2/test/test_buffer_server.cpp @@ -48,7 +48,7 @@ int main(int argc, char ** argv) rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); tf2_ros::Buffer buffer(clock); - tf2_ros::TransformListener tfl(buffer, node, false); + tf2_ros::TransformListener tfl(buffer, *node, false); std::unique_ptr server = std::make_unique( buffer, node, diff --git a/test_tf2/test/test_message_filter.cpp b/test_tf2/test/test_message_filter.cpp index f636c901d..e9757046b 100644 --- a/test_tf2/test/test_message_filter.cpp +++ b/test_tf2/test/test_message_filter.cpp @@ -79,14 +79,12 @@ 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); buffer.setCreateTimerInterface(create_timer_interface); - tf2_ros::MessageFilter filter(buffer, "frame1", 10, node); + tf2_ros::MessageFilter filter(buffer, "frame1", 10, *node); Notification n(1); filter.registerCallback(std::bind(&Notification::notify, &n, std::placeholders::_1)); @@ -102,14 +100,12 @@ 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); buffer.setCreateTimerInterface(create_timer_interface); - tf2_ros::MessageFilter filter(buffer, "frame1", 10, node); + tf2_ros::MessageFilter filter(buffer, "frame1", 10, *node); Notification n(1); filter.registerCallback(std::bind(&Notification::notify, &n, std::placeholders::_1)); @@ -145,14 +141,12 @@ 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); buffer.setCreateTimerInterface(create_timer_interface); - tf2_ros::MessageFilter filter(buffer, "frame1", 10, node); + tf2_ros::MessageFilter filter(buffer, "frame1", 10, *node); Notification n(1); filter.registerCallback(std::bind(&Notification::notify, &n, std::placeholders::_1)); @@ -176,14 +170,12 @@ 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); buffer.setCreateTimerInterface(create_timer_interface); - tf2_ros::MessageFilter filter(buffer, "frame1", 10, node); + tf2_ros::MessageFilter filter(buffer, "frame1", 10, *node); Notification n(1); filter.registerCallback(std::bind(&Notification::notify, &n, std::placeholders::_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); @@ -229,7 +219,7 @@ TEST(MessageFilter, concurrentTransforms) for (int i = 0; i < 50; i++) { buffer.setCreateTimerInterface(create_timer_interface); tf2_ros::MessageFilter filter(buffer, "frame1", buffer_size, - node); + *node); Notification n(1); filter.registerCallback(std::bind(&Notification::notify, &n, std::placeholders::_1)); @@ -294,14 +284,12 @@ 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); buffer.setCreateTimerInterface(create_timer_interface); - tf2_ros::MessageFilter filter(buffer, "frame1", 10, node); + tf2_ros::MessageFilter filter(buffer, "frame1", 10, *node); Notification n(1); filter.registerCallback(std::bind(&Notification::notify, &n, std::placeholders::_1)); filter.setTargetFrame("frame1000"); @@ -325,14 +313,12 @@ 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); buffer.setCreateTimerInterface(create_timer_interface); - tf2_ros::MessageFilter filter(buffer, "", 10, node); + tf2_ros::MessageFilter filter(buffer, "", 10, *node); Notification n(1); filter.registerCallback(std::bind(&Notification::notify, &n, std::placeholders::_1)); @@ -366,14 +352,12 @@ 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); buffer.setCreateTimerInterface(create_timer_interface); - tf2_ros::MessageFilter filter(buffer, "frame1", 10, node); + tf2_ros::MessageFilter filter(buffer, "frame1", 10, *node); Notification n(1); filter.registerCallback(std::bind(&Notification::notify, &n, std::placeholders::_1)); @@ -497,14 +481,12 @@ 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); buffer.setCreateTimerInterface(create_timer_interface); - tf2_ros::MessageFilter filter(buffer, "frame1", 10, node); + tf2_ros::MessageFilter filter(buffer, "frame1", 10, *node); Notification n(1); filter.registerCallback(std::bind(&Notification::notify, &n, std::placeholders::_1)); filter.setTargetFrame("frame1"); diff --git a/test_tf2/test/test_static_publisher.cpp b/test_tf2/test/test_static_publisher.cpp index f43d87334..54f3ed8f7 100644 --- a/test_tf2/test/test_static_publisher.cpp +++ b/test_tf2/test/test_static_publisher.cpp @@ -54,7 +54,7 @@ TEST(StaticTransformPublisher, a_b_different_times) rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); tf2_ros::Buffer mB(clock); - tf2_ros::TransformListener tfl(mB, node, false); + tf2_ros::TransformListener tfl(mB, *node, false); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(node); @@ -89,7 +89,7 @@ TEST(StaticTransformPublisher, a_c_different_times) rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); tf2_ros::Buffer mB(clock); - tf2_ros::TransformListener tfl(mB, node, false); + tf2_ros::TransformListener tfl(mB, *node, false); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(node); @@ -123,7 +123,7 @@ TEST(StaticTransformPublisher, a_d_different_times) rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); tf2_ros::Buffer mB(clock); - tf2_ros::TransformListener tfl(mB, node, false); + tf2_ros::TransformListener tfl(mB, *node, false); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(node); @@ -174,7 +174,7 @@ TEST(StaticTransformPublisher, multiple_parent_test) rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); tf2_ros::Buffer mB(clock); - tf2_ros::TransformListener tfl(mB, node, false); + tf2_ros::TransformListener tfl(mB, *node, false); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(node); @@ -194,7 +194,7 @@ TEST(StaticTransformPublisher, multiple_parent_test) } } - tf2_ros::StaticTransformBroadcaster stb(node); + tf2_ros::StaticTransformBroadcaster stb(*node); geometry_msgs::msg::TransformStamped ts; ts.transform.rotation.w = 1; ts.header.frame_id = "c"; diff --git a/tf2_ros/CMakeLists.txt b/tf2_ros/CMakeLists.txt index 0fd0c996f..63a58cf34 100644 --- a/tf2_ros/CMakeLists.txt +++ b/tf2_ros/CMakeLists.txt @@ -18,6 +18,7 @@ find_package(rcl_interfaces REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(rclcpp_components REQUIRED) +find_package(rcpputils REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_msgs REQUIRED) @@ -40,6 +41,7 @@ target_link_libraries(${PROJECT_NAME} PUBLIC message_filters::message_filters rclcpp::rclcpp rclcpp_action::rclcpp_action + rcpputils::rcpputils tf2::tf2 ${tf2_msgs_TARGETS}) @@ -236,6 +238,7 @@ ament_export_dependencies( message_filters rclcpp rclcpp_action + rcpputils tf2 tf2_msgs ) diff --git a/tf2_ros/include/tf2_ros/buffer.hpp b/tf2_ros/include/tf2_ros/buffer.hpp index e3c135347..066956f0f 100644 --- a/tf2_ros/include/tf2_ros/buffer.hpp +++ b/tf2_ros/include/tf2_ros/buffer.hpp @@ -36,6 +36,7 @@ #include #include #include +#include #include #include @@ -51,7 +52,10 @@ #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/node_base_interface.hpp" +#include "rclcpp/node_interfaces/node_services_interface.hpp" #include "rclcpp/node_interfaces/node_logging_interface.hpp" +#include "rclcpp/node_interfaces/node_interfaces.hpp" #include "rclcpp/rclcpp.hpp" namespace tf2_ros @@ -70,46 +74,41 @@ class Buffer : public BufferInterface, public AsyncBufferInterface, public tf2:: using tf2::BufferCore::canTransform; using SharedPtr = std::shared_ptr; + using NodeBaseInterface = rclcpp::node_interfaces::NodeBaseInterface; + using NodeLoggingInterface = rclcpp::node_interfaces::NodeLoggingInterface; + using NodeServicesInterface = rclcpp::node_interfaces::NodeServicesInterface; + using RequiredInterfaces = rclcpp::node_interfaces::NodeInterfaces; + + /** \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 If passed 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 + */ + TF2_ROS_PUBLIC + Buffer( + rclcpp::Clock::SharedPtr clock, + tf2::Duration cache_time = tf2::Duration(tf2::BUFFER_CORE_DEFAULT_CACHE_TIME), + RequiredInterfaces node_interfaces = RequiredInterfaces(), + 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 * \param node If passed advertise the view_frames service that exposes debugging information from the buffer * \param qos If passed change the quality of service of the frames_server_ service */ - template + template, + std::enable_if_t::value, bool> = true> + [[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), NodeT && node = NodeT(), const rclcpp::QoS & qos = rclcpp::ServicesQoS()) - : BufferCore(cache_time), clock_(clock), timer_interface_(nullptr) + : Buffer(clock, cache_time, *node, qos) { - 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); - } } /** \brief Get the transform between two frames by frame ID. @@ -332,8 +331,11 @@ class Buffer : public BufferInterface, public AsyncBufferInterface, public tf2:: /// \brief A clock to use for time and sleeping rclcpp::Clock::SharedPtr clock_; + /// \brief A set of interface to access the buffer's node + RequiredInterfaces node_interfaces_; + /// \brief A node logging interface to access the buffer node's logger - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface_; + NodeLoggingInterface::SharedPtr node_logging_interface_; /// \brief Interface for creating timers CreateTimerInterface::SharedPtr timer_interface_; diff --git a/tf2_ros/include/tf2_ros/create_timer_ros.hpp b/tf2_ros/include/tf2_ros/create_timer_ros.hpp index 5accc00eb..353533917 100644 --- a/tf2_ros/include/tf2_ros/create_timer_ros.hpp +++ b/tf2_ros/include/tf2_ros/create_timer_ros.hpp @@ -38,6 +38,9 @@ #include "tf2/time.hpp" #include "rclcpp/rclcpp.hpp" +#include "rclcpp/node_interfaces/node_interfaces.hpp" +#include "rclcpp/node_interfaces/get_node_base_interface.hpp" +#include "rclcpp/node_interfaces/get_node_timers_interface.hpp" namespace tf2_ros { @@ -50,10 +53,21 @@ namespace tf2_ros class CreateTimerROS : public CreateTimerInterface { public: + using NodeBaseInterface = rclcpp::node_interfaces::NodeBaseInterface; + using NodeTimersInterface = rclcpp::node_interfaces::NodeTimersInterface; + using RequiredInterfaces = rclcpp::node_interfaces::NodeInterfaces; + + TF2_ROS_PUBLIC + CreateTimerROS( + RequiredInterfaces 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, - rclcpp::node_interfaces::NodeTimersInterface::SharedPtr node_timers, + NodeBaseInterface::SharedPtr node_base, + NodeTimersInterface::SharedPtr node_timers, rclcpp::CallbackGroup::SharedPtr callback_group = nullptr); virtual ~CreateTimerROS() = default; @@ -119,8 +133,7 @@ class CreateTimerROS : public CreateTimerInterface const TimerHandle & timer_handle, TimerCallbackType callback); - rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_; - rclcpp::node_interfaces::NodeTimersInterface::SharedPtr node_timers_; + RequiredInterfaces 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.hpp b/tf2_ros/include/tf2_ros/message_filter.hpp index c860a8f9d..1a6dcb0cd 100644 --- a/tf2_ros/include/tf2_ros/message_filter.hpp +++ b/tf2_ros/include/tf2_ros/message_filter.hpp @@ -34,6 +34,7 @@ #include #include +#include #include #include #include @@ -57,6 +58,9 @@ #include "builtin_interfaces/msg/time.hpp" #include "rclcpp/rclcpp.hpp" +#include "rclcpp/node_interfaces/node_interfaces.hpp" +#include "rclcpp/node_interfaces/get_node_logging_interface.hpp" +#include "rclcpp/node_interfaces/get_node_clock_interface.hpp" #define TF2_ROS_MESSAGEFILTER_DEBUG(fmt, ...) \ RCUTILS_LOG_DEBUG_NAMED( \ @@ -152,24 +156,30 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi using MConstPtr = std::shared_ptr; typedef message_filters::MessageEvent MEvent; + using NodeLoggingInterface = rclcpp::node_interfaces::NodeLoggingInterface; + using NodeClockInterface = rclcpp::node_interfaces::NodeClockInterface; + using RequiredInterfaces = rclcpp::node_interfaces::NodeInterfaces; + /** * \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 The ros2 node to use for logging and clock operations + * \param node_interfaces The ros2 NodeInterfaces to use for logging and clock 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, - const rclcpp::Node::SharedPtr & node, + RequiredInterfaces node_interfaces, 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) + : node_interfaces_(std::move(node_interfaces)), + buffer_(buffer), + queue_size_(queue_size), + buffer_timeout_(buffer_timeout) { static_assert( std::is_base_of::value, @@ -177,33 +187,82 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi static_assert( std::is_base_of::value, "Buffer type must implement tf2_ros::AsyncBufferInterface"); + + init(); + setTargetFrame(target_frame); } /** * \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_logging The logging interface to use for any log messages - * \param node_clock The clock interface to use to get the node clock + * \param node_interfaces The ros2 NodeInterfaces to use for logging and clock operations * \param buffer_timeout The timeout duration after requesting transforms from the buffer. */ - template + template 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, + F & f, BufferT & buffer, const std::string & target_frame, uint32_t queue_size, + RequiredInterfaces node_interfaces, std::chrono::duration buffer_timeout = std::chrono::duration::max()) - : node_logging_(node_logging), - node_clock_(node_clock), + : node_interfaces_(std::move(node_interfaces)), buffer_(buffer), queue_size_(queue_size), buffer_timeout_(buffer_timeout) { init(); setTargetFrame(target_frame); + connectInput(f); + } + + /** + * \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 The ros2 node to use for logging and clock operations + * \param buffer_timeout The timeout duration after requesting transforms from the buffer. + */ + template + [[deprecated("Use rclcpp::node_interfaces::NodeInterfaces instead of Node::SharedPtr&")]] + 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, + RequiredInterfaces(node->get_node_logging_interface(), node->get_node_clock_interface()), + buffer_timeout) + { + } + + /** + * \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_logging The logging interface to use for any log messages + * \param node_clock The clock interface to use to get the node clock + * \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 NodeLoggingInterface::SharedPtr & node_logging, + const NodeClockInterface::SharedPtr & node_clock, + std::chrono::duration buffer_timeout = + std::chrono::duration::max()) + : MessageFilter( + buffer, target_frame, queue_size, + RequiredInterfaces(node_logging, node_clock), buffer_timeout) + { } /** @@ -217,14 +276,16 @@ 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 Node::SharedPtr&")]] 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) + f, buffer, target_frame, queue_size, + RequiredInterfaces(node->get_node_logging_interface(), node->get_node_clock_interface()), + buffer_timeout) { } @@ -240,21 +301,17 @@ 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, + const NodeLoggingInterface::SharedPtr & node_logging, + const 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, + RequiredInterfaces(node_logging, node_clock), buffer_timeout) { - init(); - setTargetFrame(target_frame); - connectInput(f); } /** @@ -461,7 +518,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)); } @@ -611,10 +668,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; } @@ -627,7 +685,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) @@ -709,7 +768,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()); } @@ -725,10 +784,8 @@ 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. + RequiredInterfaces 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.hpp b/tf2_ros/include/tf2_ros/static_transform_broadcaster.hpp index b92a75662..0970af478 100644 --- a/tf2_ros/include/tf2_ros/static_transform_broadcaster.hpp +++ b/tf2_ros/include/tf2_ros/static_transform_broadcaster.hpp @@ -34,13 +34,16 @@ #define TF2_ROS__STATIC_TRANSFORM_BROADCASTER_HPP_ #include +#include #include #include "tf2_ros/visibility_control.hpp" +#include "rclcpp/node_interfaces/node_interfaces.hpp" #include "rclcpp/node_interfaces/get_node_parameters_interface.hpp" #include "rclcpp/node_interfaces/get_node_topics_interface.hpp" #include "rclcpp/rclcpp.hpp" +#include "rcpputils/pointer_traits.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" #include "tf2_msgs/msg/tf_message.hpp" #include "tf2_ros/qos.hpp" @@ -51,12 +54,39 @@ namespace tf2_ros /** \brief This class provides an easy way to publish coordinate frame transform information. * It will handle all the messaging and stuffing of messages. And the function prototypes lay out all the * necessary data needed for each message. */ - class StaticTransformBroadcaster { public: + using NodeParametersInterface = rclcpp::node_interfaces::NodeParametersInterface; + using NodeTopicsInterface = rclcpp::node_interfaces::NodeTopicsInterface; + using RequiredInterfaces = rclcpp::node_interfaces::NodeInterfaces; + + /** \brief NodeInterfaces constructor */ + template> + StaticTransformBroadcaster( + RequiredInterfaces node_interfaces, + const rclcpp::QoS & qos = StaticBroadcasterQoS(), + const rclcpp::PublisherOptionsWithAllocator & options = [] () { + rclcpp::PublisherOptionsWithAllocator options; + options.qos_overriding_options = rclcpp::QosOverridingOptions{ + rclcpp::QosPolicyKind::Depth, + 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_static", qos, options); + } + /** \brief Node constructor */ - template> + template, + std::enable_if_t::value, bool> = true> + [[deprecated("Use rclcpp::node_interfaces::NodeInterfaces instead of NodeT")]] StaticTransformBroadcaster( NodeT && node, const rclcpp::QoS & qos = StaticBroadcasterQoS(), @@ -69,17 +99,17 @@ class StaticTransformBroadcaster return options; } ()) : StaticTransformBroadcaster( - rclcpp::node_interfaces::get_node_parameters_interface(node), - rclcpp::node_interfaces::get_node_topics_interface(node), - qos, - options) - {} + RequiredInterfaces(node->get_node_parameters_interface(), + node->get_node_topics_interface()), 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, + NodeParametersInterface::SharedPtr node_parameters, + NodeTopicsInterface::SharedPtr node_topics, const rclcpp::QoS & qos = StaticBroadcasterQoS(), const rclcpp::PublisherOptionsWithAllocator & options = [] () { rclcpp::PublisherOptionsWithAllocator options; @@ -89,9 +119,9 @@ class StaticTransformBroadcaster rclcpp::QosPolicyKind::Reliability}; return options; } ()) + : StaticTransformBroadcaster( + RequiredInterfaces(node_parameters, node_topics), qos, options) { - publisher_ = rclcpp::create_publisher( - node_parameters, node_topics, "/tf_static", qos, options); } /** \brief Send a TransformStamped message diff --git a/tf2_ros/include/tf2_ros/transform_broadcaster.hpp b/tf2_ros/include/tf2_ros/transform_broadcaster.hpp index e4b2e6610..ed35fde35 100644 --- a/tf2_ros/include/tf2_ros/transform_broadcaster.hpp +++ b/tf2_ros/include/tf2_ros/transform_broadcaster.hpp @@ -34,13 +34,16 @@ #define TF2_ROS__TRANSFORM_BROADCASTER_HPP_ #include +#include #include #include "tf2_ros/visibility_control.hpp" +#include "rclcpp/node_interfaces/node_interfaces.hpp" #include "rclcpp/node_interfaces/get_node_parameters_interface.hpp" #include "rclcpp/node_interfaces/get_node_topics_interface.hpp" #include "rclcpp/rclcpp.hpp" +#include "rcpputils/pointer_traits.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" #include "tf2_msgs/msg/tf_message.hpp" #include "tf2_ros/qos.hpp" @@ -55,8 +58,37 @@ namespace tf2_ros class TransformBroadcaster { public: + using NodeParametersInterface = rclcpp::node_interfaces::NodeParametersInterface; + using NodeTopicsInterface = rclcpp::node_interfaces::NodeTopicsInterface; + using RequiredInterfaces = rclcpp::node_interfaces::NodeInterfaces; + + /** \brief Node interfaces constructor */ + template> + TransformBroadcaster( + RequiredInterfaces 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, + std::enable_if_t::value, bool> = true> + [[deprecated("Use rclcpp::node_interfaces::NodeInterfaces instead of NodeT")]] TransformBroadcaster( NodeT && node, const rclcpp::QoS & qos = DynamicBroadcasterQoS(), @@ -70,17 +102,17 @@ class TransformBroadcaster return options; } ()) : TransformBroadcaster( - rclcpp::node_interfaces::get_node_parameters_interface(node), - rclcpp::node_interfaces::get_node_topics_interface(node), - qos, - options) - {} + RequiredInterfaces(node->get_node_parameters_interface(), + node->get_node_topics_interface()), 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, + NodeParametersInterface::SharedPtr node_parameters, + NodeTopicsInterface::SharedPtr node_topics, const rclcpp::QoS & qos = DynamicBroadcasterQoS(), const rclcpp::PublisherOptionsWithAllocator & options = [] () { rclcpp::PublisherOptionsWithAllocator options; @@ -91,9 +123,8 @@ class TransformBroadcaster rclcpp::QosPolicyKind::Reliability}; return options; } ()) + : TransformBroadcaster(RequiredInterfaces(node_parameters, node_topics), qos, options) { - publisher_ = rclcpp::create_publisher( - node_parameters, node_topics, "/tf", qos, options); } /** \brief Send a TransformStamped message diff --git a/tf2_ros/include/tf2_ros/transform_listener.hpp b/tf2_ros/include/tf2_ros/transform_listener.hpp index e04c028cb..3d6829a52 100644 --- a/tf2_ros/include/tf2_ros/transform_listener.hpp +++ b/tf2_ros/include/tf2_ros/transform_listener.hpp @@ -34,6 +34,7 @@ #include #include +#include #include #include @@ -43,6 +44,12 @@ #include "tf2_msgs/msg/tf_message.hpp" #include "rclcpp/rclcpp.hpp" +#include "rcpputils/pointer_traits.hpp" +#include "rclcpp/node_interfaces/node_interfaces.hpp" +#include "rclcpp/node_interfaces/get_node_base_interface.hpp" +#include "rclcpp/node_interfaces/get_node_logging_interface.hpp" +#include "rclcpp/node_interfaces/get_node_parameters_interface.hpp" +#include "rclcpp/node_interfaces/get_node_topics_interface.hpp" #include "tf2_ros/qos.hpp" @@ -82,6 +89,13 @@ get_default_transform_listener_static_sub_options() class TransformListener { public: + using NodeBaseInterface = rclcpp::node_interfaces::NodeBaseInterface; + using NodeLoggingInterface = rclcpp::node_interfaces::NodeLoggingInterface; + using NodeParametersInterface = rclcpp::node_interfaces::NodeParametersInterface; + using NodeTopicsInterface = rclcpp::node_interfaces::NodeTopicsInterface; + using RequiredInterfaces = rclcpp::node_interfaces::NodeInterfaces; + /** \brief Simplified constructor for transform listener. * * This constructor will create a new ROS 2 node under the hood. @@ -94,8 +108,35 @@ class TransformListener bool spin_thread = true, bool static_only = false); + /** \brief NodeInterfaces constructor */ + template> + TransformListener( + tf2::BufferCore & buffer, + RequiredInterfaces 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(), + bool static_only = false) + : buffer_(buffer) + { + init( + node_interfaces, + spin_thread, + qos, + static_qos, + options, + static_options, + static_only); + } + /** \brief Node constructor */ - template> + template, + std::enable_if_t::value, bool> = true> + [[deprecated("Use rclcpp::node_interfaces::NodeInterfaces instead of NodeT")]] TransformListener( tf2::BufferCore & buffer, NodeT && node, @@ -109,26 +150,26 @@ class TransformListener bool static_only = false) : TransformListener( buffer, - node->get_node_base_interface(), - node->get_node_logging_interface(), - node->get_node_parameters_interface(), - node->get_node_topics_interface(), + RequiredInterfaces(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> + [[deprecated("Use rclcpp::node_interfaces::NodeInterfaces instead of multiple interfaces")]] 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, + NodeBaseInterface::SharedPtr node_base, + NodeLoggingInterface::SharedPtr node_logging, + NodeParametersInterface::SharedPtr node_parameters, + NodeTopicsInterface::SharedPtr node_topics, bool spin_thread = true, const rclcpp::QoS & qos = DynamicListenerQoS(), const rclcpp::QoS & static_qos = StaticListenerQoS(), @@ -137,19 +178,16 @@ class TransformListener const rclcpp::SubscriptionOptionsWithAllocator & static_options = detail::get_default_transform_listener_static_sub_options(), bool static_only = false) - : buffer_(buffer) - { - init( - node_base, - node_logging, - node_parameters, - node_topics, + : TransformListener( + buffer, + RequiredInterfaces(node_base, node_logging, node_parameters, node_topics), spin_thread, qos, static_qos, options, static_options, - static_only); + static_only) + { } TF2_ROS_PUBLIC @@ -162,10 +200,7 @@ 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, + RequiredInterfaces node_interfaces, bool spin_thread, const rclcpp::QoS & qos, const rclcpp::QoS & static_qos, @@ -174,8 +209,10 @@ class TransformListener bool static_only = false) { spin_thread_ = spin_thread; - node_base_interface_ = node_base; - node_logging_interface_ = node_logging; + node_interfaces_ = std::move(node_interfaces); + + auto node_parameters = node_interfaces_.get_node_parameters_interface(); + auto node_topics = node_interfaces_.get_node_topics_interface(); using callback_t = std::function; callback_t cb = std::bind( @@ -185,7 +222,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_interfaces_.get_node_base_interface()->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive, false); if (!static_only) { @@ -194,15 +231,15 @@ class TransformListener tf_options.callback_group = callback_group_; message_subscription_tf_ = rclcpp::create_subscription( - node_parameters, node_topics, "/tf", qos, std::move(cb), tf_options); + node_parameters, node_topics, + "/tf", qos, std::move(cb), tf_options); } 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, + node_parameters, node_topics, "/tf_static", static_qos, std::move(static_cb), @@ -210,18 +247,18 @@ 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_interfaces_.get_node_base_interface()); dedicated_listener_thread_ = std::make_unique([&]() {executor_->spin();}); // Tell the buffer we have a dedicated thread to enable timeouts buffer_.setUsingDedicatedThread(true); } else { if (!static_only) { message_subscription_tf_ = rclcpp::create_subscription( - node_parameters, node_topics, "/tf", qos, std::move(cb), options); + node_parameters, node_topics, + "/tf", qos, std::move(cb), options); } message_subscription_tf_static_ = rclcpp::create_subscription( - node_parameters, - node_topics, + node_parameters, node_topics, "/tf_static", static_qos, std::move(static_cb), @@ -240,8 +277,7 @@ 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}; + RequiredInterfaces node_interfaces_; rclcpp::CallbackGroup::SharedPtr callback_group_{nullptr}; }; @@ -253,6 +289,13 @@ class TransformListener class StaticTransformListener : public TransformListener { public: + using NodeBaseInterface = rclcpp::node_interfaces::NodeBaseInterface; + using NodeLoggingInterface = rclcpp::node_interfaces::NodeLoggingInterface; + using NodeParametersInterface = rclcpp::node_interfaces::NodeParametersInterface; + using NodeTopicsInterface = rclcpp::node_interfaces::NodeTopicsInterface; + using RequiredInterfaces = rclcpp::node_interfaces::NodeInterfaces; + /** \brief Simplified constructor for a static transform listener * \see the simplified TransformListener documentation */ @@ -262,8 +305,31 @@ class StaticTransformListener : public TransformListener { } + /** \brief NodeInterfaces constructor */ + template> + StaticTransformListener( + tf2::BufferCore & buffer, + RequiredInterfaces 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, + true) + { + } + /** \brief Node constructor */ - template> + template, + std::enable_if_t::value, bool> = true> + [[deprecated("Use rclcpp::node_interfaces::NodeInterfaces instead of NodeT")]] StaticTransformListener( tf2::BufferCore & buffer, NodeT && node, @@ -273,7 +339,8 @@ class StaticTransformListener : public TransformListener detail::get_default_transform_listener_static_sub_options()) : TransformListener( buffer, - node, + RequiredInterfaces(node->get_node_base_interface(), node->get_node_logging_interface(), + node->get_node_parameters_interface(), node->get_node_topics_interface()), spin_thread, rclcpp::QoS(1), static_qos, @@ -285,22 +352,20 @@ class StaticTransformListener : public TransformListener /** \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, - rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging, - rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters, - rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics, + NodeBaseInterface::SharedPtr node_base, + NodeLoggingInterface::SharedPtr node_logging, + NodeParametersInterface::SharedPtr node_parameters, + 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, + RequiredInterfaces(node_base, node_logging, node_parameters, node_topics), spin_thread, rclcpp::QoS(1), static_qos, diff --git a/tf2_ros/package.xml b/tf2_ros/package.xml index 235a849d1..a027f6d11 100644 --- a/tf2_ros/package.xml +++ b/tf2_ros/package.xml @@ -24,6 +24,7 @@ rclcpp rclcpp_action rclcpp_components + rcpputils tf2 tf2_msgs diff --git a/tf2_ros/src/buffer.cpp b/tf2_ros/src/buffer.cpp index a5da4c57c..2d6f43f8a 100644 --- a/tf2_ros/src/buffer.cpp +++ b/tf2_ros/src/buffer.cpp @@ -33,6 +33,7 @@ #include "tf2_ros/buffer.hpp" #include +#include #include #include #include @@ -56,6 +57,43 @@ to_rclcpp(const tf2::Duration & duration) return rclcpp::Duration(std::chrono::nanoseconds(duration)); } +Buffer::Buffer( + rclcpp::Clock::SharedPtr clock, + tf2::Duration cache_time, + RequiredInterfaces node_interfaces, + 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); + + if (node_interfaces.get()) { + 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 cdc4cd94d..b7d082af8 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(), tf2::durationFromSec(buffer_size), *node); 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 2f75379aa..6f24f02fd 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/static_transform_broadcaster_node.cpp b/tf2_ros/src/static_transform_broadcaster_node.cpp index 37f2cf767..0647404d8 100644 --- a/tf2_ros/src/static_transform_broadcaster_node.cpp +++ b/tf2_ros/src/static_transform_broadcaster_node.cpp @@ -92,7 +92,7 @@ StaticTransformBroadcasterNode::StaticTransformBroadcasterNode(const rclcpp::Nod throw std::runtime_error("child_frame_id cannot equal frame_id"); } - broadcaster_ = std::make_unique(this); + broadcaster_ = std::make_unique(*this); // send transform broadcaster_->sendTransform(tf_msg); diff --git a/tf2_ros/src/tf2_monitor.cpp b/tf2_ros/src/tf2_monitor.cpp index 4e35da1aa..6bb203299 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_, tf2::Duration(tf2::BUFFER_CORE_DEFAULT_CACHE_TIME), *node) { tf_ = std::make_shared(buffer_); diff --git a/tf2_ros/src/transform_listener.cpp b/tf2_ros/src/transform_listener.cpp index 29039b3b9..242e8b608 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(), @@ -90,7 +87,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 c41baa409..c077df717 100644 --- a/tf2_ros/test/listener_unittest.cpp +++ b/tf2_ros/test/listener_unittest.cpp @@ -40,15 +40,77 @@ #include "rclcpp/rclcpp.hpp" #include "builtin_interfaces/msg/time.hpp" -TEST(tf2_ros_test_listener, transform_listener) +TEST(tf2_ros_test_listener, transform_listener_deprecated) { auto node = rclcpp::Node::make_shared("tf2_ros_test_listener_transform_listener"); rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); + #ifdef _MSC_VER + #pragma warning(push) + #pragma warning(disable : 4996) + #else + #pragma GCC diagnostic push + #pragma GCC diagnostic ignored "-Wdeprecated-declarations" + #endif + tf2_ros::Buffer buffer(clock); tf2_ros::TransformListener tfl(buffer, node, false); + #ifdef _MSC_VER + #pragma warning(pop) + #else + #pragma GCC diagnostic pop + #endif + + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + // Start spinning in a thread + std::thread spin_thread = std::thread([&executor] () { + executor.spin(); + }); + + geometry_msgs::msg::TransformStamped ts; + ts.transform.rotation.w = 1; + ts.header.frame_id = "a"; + ts.header.stamp = rclcpp::Time(10, 0); + ts.child_frame_id = "b"; + ts.transform.translation.x = 1; + ts.transform.translation.y = 2; + ts.transform.translation.z = 3; + + buffer.setTransform(ts, "authority"); + + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + + EXPECT_TRUE(buffer.canTransform("a", "b", tf2::timeFromSec(0))); + + geometry_msgs::msg::TransformStamped out_rootc = buffer.lookupTransform( + "a", "b", + rclcpp::Time()); + + EXPECT_EQ(1, out_rootc.transform.translation.x); + EXPECT_EQ(2, out_rootc.transform.translation.y); + EXPECT_EQ(3, out_rootc.transform.translation.z); + EXPECT_EQ(1, out_rootc.transform.rotation.w); + EXPECT_EQ(0, out_rootc.transform.rotation.x); + EXPECT_EQ(0, out_rootc.transform.rotation.y); + EXPECT_EQ(0, out_rootc.transform.rotation.z); + + executor.cancel(); + spin_thread.join(); + node.reset(); +} + +TEST(tf2_ros_test_listener, transform_listener) +{ + auto node = rclcpp::Node::make_shared("tf2_ros_test_listener_transform_listener"); + + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); + + tf2_ros::Buffer buffer(clock, tf2::Duration(tf2::BUFFER_CORE_DEFAULT_CACHE_TIME), *node); + tf2_ros::TransformListener tfl(buffer, *node, false); + rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(node); // Start spinning in a thread diff --git a/tf2_ros/test/message_filter_test.cpp b/tf2_ros/test/message_filter_test.cpp index a84be4368..b9c5ea4a3 100644 --- a/tf2_ros/test/message_filter_test.cpp +++ b/tf2_ros/test/message_filter_test.cpp @@ -28,6 +28,7 @@ */ #include +#include #include #include #include @@ -55,10 +56,19 @@ void filter_callback(const geometry_msgs::msg::PointStamped & msg) filter_callback_fired++; } -TEST(tf2_ros_message_filter, construction_and_destruction) +TEST(tf2_ros_message_filter, construction_and_destruction_deprecated) { auto node = rclcpp::Node::make_shared("test_message_filter_node"); rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); + + #ifdef _MSC_VER + #pragma warning(push) + #pragma warning(disable : 4996) + #else + #pragma GCC diagnostic push + #pragma GCC diagnostic ignored "-Wdeprecated-declarations" + #endif + tf2_ros::Buffer buffer(clock); // Node constructor with defaults @@ -81,11 +91,7 @@ TEST(tf2_ros_message_filter, construction_and_destruction) // 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, node->get_node_logging_interface(), node->get_node_clock_interface(), std::chrono::seconds(42)); } @@ -104,29 +110,113 @@ TEST(tf2_ros_message_filter, construction_and_destruction) // 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, 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, node->get_node_logging_interface(), node->get_node_clock_interface(), std::chrono::microseconds(0)); } + + #ifdef _MSC_VER + #pragma warning(pop) + #else + #pragma GCC diagnostic pop + #endif } -TEST(tf2_ros_message_filter, get_target_frames) +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, tf2::Duration(tf2::BUFFER_CORE_DEFAULT_CACHE_TIME), *node); + + // Node constructor with defaults + { + tf2_ros::MessageFilter filter(buffer, "map", 10, *node); + } + + // Node constructor no defaults + { + tf2_ros::MessageFilter filter( + buffer, "map", 10, *node, std::chrono::milliseconds(100)); + } + + // Node interface constructor with defaults + { + tf2_ros::MessageFilter filter( + 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, 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); + } + + // Filter + node constructor no defaults + { + tf2_ros::MessageFilter filter( + sub, buffer, "map", 10, *node, std::chrono::hours(1)); + } + + // Filter + node interface constructor with defaults + { + tf2_ros::MessageFilter filter( + 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, 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)); + } +} + +TEST(tf2_ros_message_filter, get_target_frames_deprecated) +{ + auto node = rclcpp::Node::make_shared("test_message_filter_node"); + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); + + #ifdef _MSC_VER + #pragma warning(push) + #pragma warning(disable : 4996) + #else + #pragma GCC diagnostic push + #pragma GCC diagnostic ignored "-Wdeprecated-declarations" + #endif + + tf2_ros::Buffer buffer(clock); tf2_ros::MessageFilter filter(buffer, "map", 10, node); + + #ifdef _MSC_VER + #pragma warning(pop) + #else + #pragma GCC diagnostic pop + #endif ASSERT_STREQ(filter.getTargetFramesString().c_str(), "map"); std::vector frames; @@ -136,10 +226,34 @@ TEST(tf2_ros_message_filter, get_target_frames) ASSERT_STREQ(filter.getTargetFramesString().c_str(), "odom, map"); } -TEST(tf2_ros_message_filter, multiple_frames_and_time_tolerance) +TEST(tf2_ros_message_filter, get_target_frames) +{ + 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::Duration(tf2::BUFFER_CORE_DEFAULT_CACHE_TIME), *node); + + tf2_ros::MessageFilter filter(buffer, "map", 10, *node); + ASSERT_STREQ(filter.getTargetFramesString().c_str(), "map"); + + std::vector frames; + frames.push_back("odom"); + frames.push_back("map"); + filter.setTargetFrames(frames); + ASSERT_STREQ(filter.getTargetFramesString().c_str(), "odom, map"); +} + +TEST(tf2_ros_message_filter, multiple_frames_and_time_tolerance_deprecated) { auto node = rclcpp::Node::make_shared("tf2_ros_message_filter"); + #ifdef _MSC_VER + #pragma warning(push) + #pragma warning(disable : 4996) + #else + #pragma GCC diagnostic push + #pragma GCC diagnostic ignored "-Wdeprecated-declarations" + #endif + auto create_timer_interface = std::make_shared( node->get_node_base_interface(), node->get_node_timers_interface()); @@ -167,6 +281,91 @@ TEST(tf2_ros_message_filter, multiple_frames_and_time_tolerance) // Publish static transforms so the frame transformations will always be valid tf2_ros::StaticTransformBroadcaster tfb(node); + + #ifdef _MSC_VER + #pragma warning(pop) + #else + #pragma GCC diagnostic pop + #endif + + geometry_msgs::msg::TransformStamped map_to_odom; + map_to_odom.header.stamp = rclcpp::Time(0, 0); + map_to_odom.header.frame_id = "map"; + map_to_odom.child_frame_id = "odom"; + map_to_odom.transform.translation.x = 0.0; + map_to_odom.transform.translation.y = 0.0; + map_to_odom.transform.translation.z = 0.0; + map_to_odom.transform.rotation.x = 0.0; + map_to_odom.transform.rotation.y = 0.0; + map_to_odom.transform.rotation.z = 0.0; + map_to_odom.transform.rotation.w = 1.0; + tfb.sendTransform(map_to_odom); + + geometry_msgs::msg::TransformStamped odom_to_base; + odom_to_base.header.stamp = rclcpp::Time(0, 0); + odom_to_base.header.frame_id = "odom"; + odom_to_base.child_frame_id = "base"; + odom_to_base.transform.translation.x = 0.0; + odom_to_base.transform.translation.y = 0.0; + odom_to_base.transform.translation.z = 0.0; + odom_to_base.transform.rotation.x = 0.0; + odom_to_base.transform.rotation.y = 0.0; + odom_to_base.transform.rotation.z = 0.0; + odom_to_base.transform.rotation.w = 1.0; + tfb.sendTransform(odom_to_base); + + rclcpp::Publisher::SharedPtr pub; + pub = node->create_publisher("point", 10); + geometry_msgs::msg::PointStamped point; + point.header.stamp = rclcpp::Clock().now(); + point.header.frame_id = "base"; + point.point.x = 0.1; + point.point.y = 0.2; + point.point.z = 0.3; + + rclcpp::WallRate loop_rate(1); + while (rclcpp::ok()) { + pub->publish(point); + rclcpp::spin_some(node); + loop_rate.sleep(); + RCLCPP_INFO(node->get_logger(), "filter callback: trigger(%d)", filter_callback_fired.load()); + if (filter_callback_fired.load() > 5) { + break; + } + } + + ASSERT_TRUE(filter_callback_fired); +} + +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); + + rclcpp::QoS default_qos = + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default)); + message_filters::Subscriber sub; + sub.subscribe(node, "point", default_qos); + + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); + tf2_ros::Buffer buffer(clock); + buffer.setCreateTimerInterface(create_timer_interface); + tf2_ros::TransformListener tfl(buffer); + tf2_ros::MessageFilter filter(buffer, "map", 10, *node); + filter.connectInput(sub); + filter.registerCallback(&filter_callback); + + // Register multiple target frames + std::vector frames; + frames.push_back("odom"); + frames.push_back("map"); + filter.setTargetFrames(frames); + // Set a non-zero time tolerance + filter.setTolerance(rclcpp::Duration(1, 0)); + + // Publish static transforms so the frame transformations will always be valid + 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"; @@ -213,7 +412,6 @@ TEST(tf2_ros_message_filter, multiple_frames_and_time_tolerance) } } - rclcpp::shutdown(); ASSERT_TRUE(filter_callback_fired); } 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 877f91c7a..d85a1eaf1 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) { } @@ -495,9 +496,6 @@ TEST(test_buffer, wait_for_transform_race) TEST(test_buffer, timer_ros_wait_for_transform_race) { - int argc = 1; - char const * const argv[] = {"timer_ros_wait_for_transform_race"}; - rclcpp::init(argc, argv); std::shared_ptr rclcpp_node_ = std::make_shared( "timer_ros_wait_for_transform_race"); @@ -505,9 +503,7 @@ TEST(test_buffer, timer_ros_wait_for_transform_race) tf2_ros::Buffer buffer(clock); // 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(); @@ -553,6 +549,7 @@ TEST(test_buffer, timer_ros_wait_for_transform_race) int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); auto ret = RUN_ALL_TESTS(); rclcpp::shutdown(); return ret; diff --git a/tf2_ros/test/test_static_transform_broadcaster.cpp b/tf2_ros/test/test_static_transform_broadcaster.cpp index 2acd958a4..3d79f39c1 100644 --- a/tf2_ros/test/test_static_transform_broadcaster.cpp +++ b/tf2_ros/test/test_static_transform_broadcaster.cpp @@ -44,7 +44,7 @@ class CustomNode : public rclcpp::Node void init_tf_broadcaster() { - tf_broadcaster_ = std::make_shared(shared_from_this()); + tf_broadcaster_ = std::make_shared(*shared_from_this()); } private: @@ -60,30 +60,59 @@ class CustomComposableNode : public rclcpp::Node void init_tf_broadcaster() { - tf_broadcaster_ = std::make_shared(shared_from_this()); + tf_broadcaster_ = std::make_shared(*shared_from_this()); } private: std::shared_ptr tf_broadcaster_; }; -TEST(tf2_test_static_transform_broadcaster, transform_broadcaster_rclcpp_node) +TEST(tf2_test_static_transform_broadcaster, transform_broadcaster_rclcpp_node_deprecated) { auto node = rclcpp::Node::make_shared("tf2_ros_message_filter"); + #ifdef _MSC_VER + #pragma warning(push) + #pragma warning(disable : 4996) + #else + #pragma GCC diagnostic push + #pragma GCC diagnostic ignored "-Wdeprecated-declarations" + #endif + // Construct static tf broadcaster from node pointer { tf2_ros::StaticTransformBroadcaster tfb(node); } + // Construct static tf broadcaster from node interfaces + { + tf2_ros::StaticTransformBroadcaster tfb( + node->get_node_parameters_interface(), + node->get_node_topics_interface()); + } + + #ifdef _MSC_VER + #pragma warning(pop) + #else + #pragma GCC diagnostic pop + #endif +} + +TEST(tf2_test_static_transform_broadcaster, transform_broadcaster_rclcpp_node) +{ + auto node = rclcpp::Node::make_shared("tf2_ros_message_filter"); + // Construct static tf broadcaster from node object { tf2_ros::StaticTransformBroadcaster tfb(*node); } - // Construct static tf broadcaster from node interfaces + // Construct static tf broadcaster from NodeInterfaces { 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())); } } @@ -96,14 +125,40 @@ TEST(tf2_test_static_transform_broadcaster, transform_broadcaster_with_intraproc custom_node->init_tf_broadcaster(); } -TEST(tf2_test_static_transform_broadcaster, transform_broadcaster_custom_rclcpp_node) +TEST(tf2_test_static_transform_broadcaster, transform_broadcaster_custom_rclcpp_node_deprecated) { auto node = std::make_shared("tf2_ros_message_filter"); + #ifdef _MSC_VER + #pragma warning(push) + #pragma warning(disable : 4996) + #else + #pragma GCC diagnostic push + #pragma GCC diagnostic ignored "-Wdeprecated-declarations" + #endif + // Construct static tf broadcaster from node pointer { tf2_ros::StaticTransformBroadcaster tfb(node); } + // Construct static tf broadcaster from node interfaces + { + tf2_ros::StaticTransformBroadcaster tfb( + node->get_node_parameters_interface(), + node->get_node_topics_interface()); + } + + #ifdef _MSC_VER + #pragma warning(pop) + #else + #pragma GCC diagnostic pop + #endif +} + +TEST(tf2_test_static_transform_broadcaster, transform_broadcaster_custom_rclcpp_node) +{ + auto node = std::make_shared("tf2_ros_message_filter"); + // Construct static tf broadcaster from node object { tf2_ros::StaticTransformBroadcaster tfb(*node); @@ -111,8 +166,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 31715d976..26867bac4 100644 --- a/tf2_ros/test/test_transform_broadcaster.cpp +++ b/tf2_ros/test/test_transform_broadcaster.cpp @@ -44,20 +44,46 @@ class CustomNode : public rclcpp::Node void init_tf_broadcaster() { - tf_broadcaster_ = std::make_shared(shared_from_this()); + tf_broadcaster_ = std::make_shared(*this); } private: std::shared_ptr tf_broadcaster_; }; -TEST(tf2_test_transform_broadcaster, transform_broadcaster_rclcpp_node) +TEST(tf2_test_transform_broadcaster, transform_broadcaster_rclcpp_node_deprecated) { auto node = rclcpp::Node::make_shared("tf2_ros_message_filter"); + + #ifdef _MSC_VER + #pragma warning(push) + #pragma warning(disable : 4996) + #else + #pragma GCC diagnostic push + #pragma GCC diagnostic ignored "-Wdeprecated-declarations" + #endif + // Construct tf broadcaster from node pointer { tf2_ros::TransformBroadcaster tfb(node); } + // Construct tf broadcaster from node interfaces + { + tf2_ros::TransformBroadcaster tfb( + node->get_node_parameters_interface(), + node->get_node_topics_interface()); + } + + #ifdef _MSC_VER + #pragma warning(pop) + #else + #pragma GCC diagnostic pop + #endif +} + +TEST(tf2_test_transform_broadcaster, transform_broadcaster_rclcpp_node) +{ + auto node = rclcpp::Node::make_shared("tf2_ros_message_filter"); // Construct tf broadcaster from node object { tf2_ros::TransformBroadcaster tfb(*node); @@ -65,19 +91,47 @@ 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())); } } -TEST(tf2_test_transform_broadcaster, transform_broadcaster_custom_rclcpp_node) +TEST(tf2_test_transform_broadcaster, transform_broadcaster_custom_rclcpp_node_deprecated) { auto node = std::make_shared("tf2_ros_message_filter"); + #ifdef _MSC_VER + #pragma warning(push) + #pragma warning(disable : 4996) + #else + #pragma GCC diagnostic push + #pragma GCC diagnostic ignored "-Wdeprecated-declarations" + #endif + // Construct tf broadcaster from node pointer { tf2_ros::TransformBroadcaster tfb(node); } + // Construct tf broadcaster from node interfaces + { + tf2_ros::TransformBroadcaster tfb( + node->get_node_parameters_interface(), + node->get_node_topics_interface()); + } + + #ifdef _MSC_VER + #pragma warning(pop) + #else + #pragma GCC diagnostic pop + #endif +} + +TEST(tf2_test_transform_broadcaster, transform_broadcaster_custom_rclcpp_node) +{ + auto node = std::make_shared("tf2_ros_message_filter"); // Construct tf broadcaster from node object { tf2_ros::TransformBroadcaster tfb(*node); @@ -85,8 +139,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 df38d481c..f97821ee0 100644 --- a/tf2_ros/test/test_transform_listener.cpp +++ b/tf2_ros/test/test_transform_listener.cpp @@ -51,7 +51,7 @@ class CustomNode : public rclcpp::Node { 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); + tf_listener_ = std::make_shared(buffer, *this, false); } void init_static_tf_listener() @@ -59,7 +59,7 @@ class CustomNode : public rclcpp::Node 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); + std::make_shared(buffer, *this, false); } private: @@ -77,7 +77,7 @@ class CustomComposableNode : public rclcpp::Node { 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); + tf_listener_ = std::make_shared(buffer, *this, false); } void init_static_tf_listener() @@ -85,20 +85,68 @@ class CustomComposableNode : public rclcpp::Node 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); + std::make_shared(buffer, *this, false); } private: std::shared_ptr tf_listener_; }; +TEST(tf2_test_transform_listener, transform_listener_rclcpp_node_deprecated) +{ + 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); + + #ifdef _MSC_VER + #pragma warning(push) + #pragma warning(disable : 4996) + #else + #pragma GCC diagnostic push + #pragma GCC diagnostic ignored "-Wdeprecated-declarations" + #endif + + tf2_ros::TransformListener tfl(buffer, node, false); + + #ifdef _MSC_VER + #pragma warning(pop) + #else + #pragma GCC diagnostic pop + #endif +} + 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); +} + +TEST(tf2_test_transform_listener, transform_listener_custom_rclcpp_node_deprecated) +{ + auto node = std::make_shared("tf2_ros_message_filter"); + + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); + tf2_ros::Buffer buffer(clock); + + #ifdef _MSC_VER + #pragma warning(push) + #pragma warning(disable : 4996) + #else + #pragma GCC diagnostic push + #pragma GCC diagnostic ignored "-Wdeprecated-declarations" + #endif + tf2_ros::TransformListener tfl(buffer, node, false); + + #ifdef _MSC_VER + #pragma warning(pop) + #else + #pragma GCC diagnostic pop + #endif } TEST(tf2_test_transform_listener, transform_listener_custom_rclcpp_node) @@ -107,7 +155,7 @@ TEST(tf2_test_transform_listener, transform_listener_custom_rclcpp_node) rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); tf2_ros::Buffer buffer(clock); - tf2_ros::TransformListener tfl(buffer, node, false); + tf2_ros::TransformListener tfl(buffer, *node, false); } TEST(tf2_test_transform_listener, transform_listener_as_member) @@ -133,13 +181,37 @@ TEST(tf2_test_static_transform_listener, static_transform_listener_rclcpp_node) tf2_ros::Buffer buffer(clock); } -TEST(tf2_test_static_transform_listener, static_transform_listener_custom_rclcpp_node) +TEST(tf2_test_static_transform_listener, static_transform_listener_custom_rclcpp_node_deprecated) { 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); + + #ifdef _MSC_VER + #pragma warning(push) + #pragma warning(disable : 4996) + #else + #pragma GCC diagnostic push + #pragma GCC diagnostic ignored "-Wdeprecated-declarations" + #endif + tf2_ros::StaticTransformListener tfl(buffer, node, false); + + #ifdef _MSC_VER + #pragma warning(pop) + #else + #pragma GCC diagnostic pop + #endif +} + +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) @@ -157,11 +229,19 @@ TEST(tf2_test_static_transform_listener, static_transform_listener_with_intrapro custom_node->init_static_tf_listener(); } -TEST(tf2_test_listeners, static_vs_dynamic) +TEST(tf2_test_listeners, static_vs_dynamic_deprecated) { auto node = rclcpp::Node::make_shared("tf2_ros_static_transform_listener"); - rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); + + #ifdef _MSC_VER + #pragma warning(push) + #pragma warning(disable : 4996) + #else + #pragma GCC diagnostic push + #pragma GCC diagnostic ignored "-Wdeprecated-declarations" + #endif + tf2_ros::Buffer dynamic_buffer(clock); tf2_ros::Buffer static_buffer(clock); tf2_ros::TransformListener tfl(dynamic_buffer, node, true); @@ -169,6 +249,56 @@ TEST(tf2_test_listeners, static_vs_dynamic) tf2_ros::TransformBroadcaster broadcaster(node); tf2_ros::StaticTransformBroadcaster static_broadcaster(node); + #ifdef _MSC_VER + #pragma warning(pop) + #else + #pragma GCC diagnostic pop + #endif + + 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())); +} + +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"; diff --git a/tf2_ros/test/time_reset_test.cpp b/tf2_ros/test/time_reset_test.cpp index ea8981070..40e5879db 100644 --- a/tf2_ros/test/time_reset_test.cpp +++ b/tf2_ros/test/time_reset_test.cpp @@ -50,16 +50,95 @@ void spin_for_a_second(std::shared_ptr & node) } } -TEST(tf2_ros_time_reset_test, time_backwards) +TEST(tf2_ros_time_reset_test, time_backwards_deprecated) { std::shared_ptr node_ = std::make_shared( "transform_listener_backwards_reset"); rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); + #ifdef _MSC_VER + #pragma warning(push) + #pragma warning(disable : 4996) + #else + #pragma GCC diagnostic push + #pragma GCC diagnostic ignored "-Wdeprecated-declarations" + #endif + tf2_ros::Buffer buffer(clock); - tf2_ros::TransformListener tfl(buffer); + tf2_ros::TransformListener tfl(buffer, node_); tf2_ros::TransformBroadcaster tfb(node_); + #ifdef _MSC_VER + #pragma warning(pop) + #else + #pragma GCC diagnostic pop + #endif + + auto clock_pub = node_->create_publisher("/clock", 1); + + // basic test + ASSERT_FALSE(buffer.canTransform("foo", "bar", rclcpp::Time(101, 0))); + + // make sure endpoints have discovered each other + spin_for_a_second(node_); + + rosgraph_msgs::msg::Clock c; + c.clock = rclcpp::Time(100, 0); + clock_pub->publish(c); + + // set the transform + geometry_msgs::msg::TransformStamped msg; + msg.header.stamp = rclcpp::Time(100, 0); + msg.header.frame_id = "foo"; + msg.child_frame_id = "bar"; + msg.transform.rotation.w = 0.0; + msg.transform.rotation.x = 1.0; + msg.transform.rotation.y = 0.0; + msg.transform.rotation.z = 0.0; + tfb.sendTransform(msg); + msg.header.stamp = rclcpp::Time(102, 0); + tfb.sendTransform(msg); + + // make sure it arrives + spin_for_a_second(node_); + + // verify it's been set + ASSERT_TRUE(buffer.canTransform("foo", "bar", rclcpp::Time(101, 0))); + + // TODO(ahcorde): review this + // c.clock.sec = 90; + // c.clock.nanosec = 0; + // clock_pub->publish(c); + // + // // make sure it arrives + // rclcpp::spin_some(node_); + // sleep(1); + // + // //Send anoterh message to trigger clock test on an unrelated frame + // msg.header.stamp.sec = 110; + // msg.header.stamp.nanosec = 0; + // msg.header.frame_id = "foo2"; + // msg.child_frame_id = "bar2"; + // tfb.sendTransform(msg); + // + // // make sure it arrives + // rclcpp::spin_some(node_); + // sleep(1); + // + // //verify the data's been cleared + // ASSERT_FALSE(buffer.canTransform("foo", "bar", tf2::timeFromSec(101))); +} + +TEST(tf2_ros_time_reset_test, time_backwards) +{ + std::shared_ptr node_ = std::make_shared( + "transform_listener_backwards_reset"); + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); + + tf2_ros::Buffer buffer(clock); + tf2_ros::TransformListener tfl(buffer, *node_); + tf2_ros::TransformBroadcaster tfb(*node_); + auto clock_pub = node_->create_publisher("/clock", 1); // basic test diff --git a/tf2_ros/test/velocity_test.cpp b/tf2_ros/test/velocity_test.cpp index 7f87b2f86..c5ad61507 100644 --- a/tf2_ros/test/velocity_test.cpp +++ b/tf2_ros/test/velocity_test.cpp @@ -48,6 +48,7 @@ class LinearVelocitySquareTest : public ::testing::Test LinearVelocitySquareTest() { + auto node = rclcpp::Node::make_shared("tf2_ros_message_filter"); rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_STEADY_TIME); buffer_ = std::make_shared(clock); @@ -158,6 +159,7 @@ class AngularVelocitySquareTest : public ::testing::Test AngularVelocitySquareTest() { + auto node = rclcpp::Node::make_shared("tf2_ros_message_filter"); rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_STEADY_TIME); buffer_ = std::make_shared(clock); @@ -541,6 +543,7 @@ TEST_F(AngularVelocitySquareTest, AngularVelocityOffsetParentFrameInZ) int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); auto ret = RUN_ALL_TESTS(); rclcpp::shutdown(); return ret;