From 0e505aaf09bac77242b483fa8a8ede5047a0ddc4 Mon Sep 17 00:00:00 2001 From: Lucas Wendland <82680922+CursedRock17@users.noreply.github.com> Date: Tue, 26 Mar 2024 04:52:54 -0400 Subject: [PATCH] Adding NodeInterfaces to Buffer (#656) Signed-off-by: CursedRock17 Signed-off-by: Lucas Wendland <82680922+CursedRock17@users.noreply.github.com> --- tf2_ros/include/tf2_ros/buffer.h | 47 +++++++++++++++++++++++++++++--- tf2_ros/src/buffer.cpp | 33 ++-------------------- 2 files changed, 45 insertions(+), 35 deletions(-) diff --git a/tf2_ros/include/tf2_ros/buffer.h b/tf2_ros/include/tf2_ros/buffer.h index fad6f0f4d..13571fa9b 100644 --- a/tf2_ros/include/tf2_ros/buffer.h +++ b/tf2_ros/include/tf2_ros/buffer.h @@ -36,6 +36,7 @@ #include #include #include +#include #include #include "tf2_ros/async_buffer_interface.h" @@ -47,6 +48,10 @@ #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/node_logging_interface.hpp" #include "rclcpp/rclcpp.hpp" namespace tf2_ros @@ -69,11 +74,43 @@ class Buffer : public BufferInterface, public AsyncBufferInterface, public tf2:: * \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 */ - TF2_ROS_PUBLIC Buffer( + template + Buffer( rclcpp::Clock::SharedPtr clock, tf2::Duration cache_time = tf2::Duration(tf2::BUFFER_CORE_DEFAULT_CACHE_TIME), - rclcpp::Node::SharedPtr node = rclcpp::Node::SharedPtr()); + NodeT && node = NodeT(), + 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); + } + } /** \brief Get the transform between two frames by frame ID. * \param target_frame The frame to which data should be transformed @@ -275,10 +312,12 @@ class Buffer : public BufferInterface, public AsyncBufferInterface, public tf2:: TransformStampedFuture future, TransformReadyCallback callback); + TF2_ROS_PUBLIC bool getFrames( const tf2_msgs::srv::FrameGraph::Request::SharedPtr req, tf2_msgs::srv::FrameGraph::Response::SharedPtr res); + TF2_ROS_PUBLIC void onTimeJump(const rcl_time_jump_t & jump); // conditionally error if dedicated_thread unset. @@ -293,8 +332,8 @@ class Buffer : public BufferInterface, public AsyncBufferInterface, public tf2:: /// \brief A clock to use for time and sleeping rclcpp::Clock::SharedPtr clock_; - /// \brief A node to advertise the view_frames service - rclcpp::Node::SharedPtr node_; + /// \brief A node logging interface to access the buffer node's logger + rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface_; /// \brief Interface for creating timers CreateTimerInterface::SharedPtr timer_interface_; diff --git a/tf2_ros/src/buffer.cpp b/tf2_ros/src/buffer.cpp index c3e89ebd1..10ee57cc2 100644 --- a/tf2_ros/src/buffer.cpp +++ b/tf2_ros/src/buffer.cpp @@ -42,36 +42,6 @@ namespace tf2_ros { - -Buffer::Buffer( - rclcpp::Clock::SharedPtr clock, tf2::Duration cache_time, - rclcpp::Node::SharedPtr node) -: BufferCore(cache_time), clock_(clock), node_(node), 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_) { - frames_server_ = node_->create_service( - "tf2_frames", std::bind( - &Buffer::getFrames, this, std::placeholders::_1, - std::placeholders::_2)); - } -} - inline tf2::Duration from_rclcpp(const rclcpp::Duration & rclcpp_duration) @@ -341,7 +311,8 @@ bool Buffer::checkAndErrorDedicatedThreadPresent(std::string * error_str) const rclcpp::Logger Buffer::getLogger() const { - return node_ ? node_->get_logger() : rclcpp::get_logger("tf2_buffer"); + return node_logging_interface_ ? node_logging_interface_->get_logger() : rclcpp::get_logger( + "tf2_buffer"); } } // namespace tf2_ros