From 7d0e2be71b52a0577b96ef6374003cfcba2858a2 Mon Sep 17 00:00:00 2001 From: CursedRock17 Date: Sun, 10 Mar 2024 22:52:46 -0400 Subject: [PATCH 1/6] Using Node Interfaces in Buffer Signed-off-by: CursedRock17 --- tf2_ros/include/tf2_ros/buffer.h | 44 +++++++++++++++++++++++++++++--- tf2_ros/src/buffer.cpp | 33 ++---------------------- 2 files changed, 42 insertions(+), 35 deletions(-) diff --git a/tf2_ros/include/tf2_ros/buffer.h b/tf2_ros/include/tf2_ros/buffer.h index fad6f0f4d..5acfb3176 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 @@ -70,10 +75,41 @@ class Buffer : public BufferInterface, public AsyncBufferInterface, public tf2:: * \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 */ - 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 = std::move(rclcpp::Node::SharedPtr()), + const rclcpp::QoS & qos = rclcpp::QoS(100)) + : BufferCore(cache_time), clock_(clock), timer_interface_(nullptr) + { + node_logging_interface_ = rclcpp::node_interfaces::get_node_logging_interface(node); + + 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_ = 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 @@ -293,8 +329,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 From eb7e53377393f2ce6d15dbcad3ff14d5fbf46bb2 Mon Sep 17 00:00:00 2001 From: CursedRock17 Date: Sun, 10 Mar 2024 23:22:04 -0400 Subject: [PATCH 2/6] adding slight adjustement Signed-off-by: CursedRock17 --- tf2_ros/include/tf2_ros/buffer.h | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/tf2_ros/include/tf2_ros/buffer.h b/tf2_ros/include/tf2_ros/buffer.h index 5acfb3176..ee3b81030 100644 --- a/tf2_ros/include/tf2_ros/buffer.h +++ b/tf2_ros/include/tf2_ros/buffer.h @@ -74,13 +74,14 @@ 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 */ - template> + template Buffer( rclcpp::Clock::SharedPtr clock, tf2::Duration cache_time = tf2::Duration(tf2::BUFFER_CORE_DEFAULT_CACHE_TIME), - NodeT && node = std::move(rclcpp::Node::SharedPtr()), - const rclcpp::QoS & qos = rclcpp::QoS(100)) + NodeT && node = rclcpp::Node::SharedPtr(), + const rclcpp::QoS & qos = rclcpp::ServicesQoS()) : BufferCore(cache_time), clock_(clock), timer_interface_(nullptr) { node_logging_interface_ = rclcpp::node_interfaces::get_node_logging_interface(node); From c0868f02ba93b2eff2904a96afae7ff26afbc572 Mon Sep 17 00:00:00 2001 From: CursedRock17 Date: Mon, 11 Mar 2024 21:41:33 -0400 Subject: [PATCH 3/6] Fixing nullptr node Signed-off-by: CursedRock17 --- tf2_ros/include/tf2_ros/buffer.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tf2_ros/include/tf2_ros/buffer.h b/tf2_ros/include/tf2_ros/buffer.h index ee3b81030..3ec696aa5 100644 --- a/tf2_ros/include/tf2_ros/buffer.h +++ b/tf2_ros/include/tf2_ros/buffer.h @@ -84,8 +84,6 @@ class Buffer : public BufferInterface, public AsyncBufferInterface, public tf2:: const rclcpp::QoS & qos = rclcpp::ServicesQoS()) : BufferCore(cache_time), clock_(clock), timer_interface_(nullptr) { - node_logging_interface_ = rclcpp::node_interfaces::get_node_logging_interface(node); - if (nullptr == clock_) { throw std::invalid_argument("clock must be a valid instance"); } @@ -103,6 +101,8 @@ class Buffer : public BufferInterface, public AsyncBufferInterface, public tf2:: 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), From cc51b0d588106624d1e231bce9028b0e8db146fa Mon Sep 17 00:00:00 2001 From: Lucas Wendland <82680922+CursedRock17@users.noreply.github.com> Date: Tue, 12 Mar 2024 18:08:18 -0400 Subject: [PATCH 4/6] Make Node Any Starting Type by Default Co-authored-by: Chris Lalancette Signed-off-by: Lucas Wendland <82680922+CursedRock17@users.noreply.github.com> --- tf2_ros/include/tf2_ros/buffer.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tf2_ros/include/tf2_ros/buffer.h b/tf2_ros/include/tf2_ros/buffer.h index 3ec696aa5..7049f64ee 100644 --- a/tf2_ros/include/tf2_ros/buffer.h +++ b/tf2_ros/include/tf2_ros/buffer.h @@ -80,7 +80,7 @@ class Buffer : public BufferInterface, public AsyncBufferInterface, public tf2:: Buffer( rclcpp::Clock::SharedPtr clock, tf2::Duration cache_time = tf2::Duration(tf2::BUFFER_CORE_DEFAULT_CACHE_TIME), - NodeT && node = rclcpp::Node::SharedPtr(), + NodeT && node = NodeT(), const rclcpp::QoS & qos = rclcpp::ServicesQoS()) : BufferCore(cache_time), clock_(clock), timer_interface_(nullptr) { From f12edec4f44fd68959d743da00aa363c17f6c412 Mon Sep 17 00:00:00 2001 From: Lucas Wendland <82680922+CursedRock17@users.noreply.github.com> Date: Mon, 18 Mar 2024 13:25:29 -0400 Subject: [PATCH 5/6] Adding Macro back Adding `TF2_ROS_PUBLIC` to `Buffer` Signed-off-by: Lucas Wendland <82680922+CursedRock17@users.noreply.github.com> --- tf2_ros/include/tf2_ros/buffer.h | 1 + 1 file changed, 1 insertion(+) diff --git a/tf2_ros/include/tf2_ros/buffer.h b/tf2_ros/include/tf2_ros/buffer.h index 7049f64ee..91b2074ff 100644 --- a/tf2_ros/include/tf2_ros/buffer.h +++ b/tf2_ros/include/tf2_ros/buffer.h @@ -77,6 +77,7 @@ class Buffer : public BufferInterface, public AsyncBufferInterface, public tf2:: * \param qos If passed change the quality of service of the frames_server_ service */ template + TF2_ROS_PUBLIC Buffer( rclcpp::Clock::SharedPtr clock, tf2::Duration cache_time = tf2::Duration(tf2::BUFFER_CORE_DEFAULT_CACHE_TIME), From 698c5ae61d25f83fa83d437f8ccaf38cddf871e1 Mon Sep 17 00:00:00 2001 From: CursedRock17 Date: Wed, 20 Mar 2024 22:50:46 -0400 Subject: [PATCH 6/6] Fixing Windows Linking Error Signed-off-by: CursedRock17 --- tf2_ros/include/tf2_ros/buffer.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/tf2_ros/include/tf2_ros/buffer.h b/tf2_ros/include/tf2_ros/buffer.h index 91b2074ff..13571fa9b 100644 --- a/tf2_ros/include/tf2_ros/buffer.h +++ b/tf2_ros/include/tf2_ros/buffer.h @@ -77,7 +77,6 @@ class Buffer : public BufferInterface, public AsyncBufferInterface, public tf2:: * \param qos If passed change the quality of service of the frames_server_ service */ template - TF2_ROS_PUBLIC Buffer( rclcpp::Clock::SharedPtr clock, tf2::Duration cache_time = tf2::Duration(tf2::BUFFER_CORE_DEFAULT_CACHE_TIME), @@ -313,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.