Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Adding NodeInterfaces API Design #714

Draft
wants to merge 3 commits into
base: rolling
Choose a base branch
from
Draft

Adding NodeInterfaces API Design #714

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
6e5f043
Adding NodeInterfaces API Design
CursedRock17 Oct 12, 2024
404c51e
Merge branch 'rolling' into node_templating
CursedRock17 Oct 12, 2024
0666135
Rebase/Test fix
CursedRock17 Oct 13, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion test_tf2/test/test_buffer_server.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,7 @@ int main(int argc, char ** argv)
auto node = rclcpp::Node::make_shared("tf2_ros_message_filter");

rclcpp::Clock::SharedPtr clock = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);
tf2_ros::Buffer buffer(clock);
tf2_ros::Buffer buffer(clock, *node);
tf2_ros::TransformListener tfl(buffer, node, false);
std::unique_ptr<tf2_ros::BufferServer> server = std::make_unique<tf2_ros::BufferServer>(
buffer,
Expand Down
54 changes: 18 additions & 36 deletions test_tf2/test/test_message_filter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -79,12 +79,10 @@ class Notification
TEST(MessageFilter, noTransforms)
{
auto node = rclcpp::Node::make_shared("tf2_ros_message_filter");
auto create_timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
node->get_node_base_interface(),
node->get_node_timers_interface());
auto create_timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(*node);

rclcpp::Clock::SharedPtr clock = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);
tf2_ros::Buffer buffer(clock);
tf2_ros::Buffer buffer(clock, *node);
buffer.setCreateTimerInterface(create_timer_interface);
tf2_ros::MessageFilter<geometry_msgs::msg::PointStamped> filter(buffer, "frame1", 10, node);
Notification n(1);
Expand All @@ -102,12 +100,10 @@ TEST(MessageFilter, noTransforms)
TEST(MessageFilter, noTransformsSameFrame)
{
auto node = rclcpp::Node::make_shared("tf2_ros_message_filter");
auto create_timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
node->get_node_base_interface(),
node->get_node_timers_interface());
auto create_timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(*node);

rclcpp::Clock::SharedPtr clock = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);
tf2_ros::Buffer buffer(clock);
tf2_ros::Buffer buffer(clock, *node);
buffer.setCreateTimerInterface(create_timer_interface);
tf2_ros::MessageFilter<geometry_msgs::msg::PointStamped> filter(buffer, "frame1", 10, node);
Notification n(1);
Expand Down Expand Up @@ -145,12 +141,10 @@ geometry_msgs::msg::TransformStamped createTransform(
TEST(MessageFilter, preexistingTransforms)
{
auto node = rclcpp::Node::make_shared("tf2_ros_message_filter");
auto create_timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
node->get_node_base_interface(),
node->get_node_timers_interface());
auto create_timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(*node);

rclcpp::Clock::SharedPtr clock = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);
tf2_ros::Buffer buffer(clock);
tf2_ros::Buffer buffer(clock, *node);
buffer.setCreateTimerInterface(create_timer_interface);
tf2_ros::MessageFilter<geometry_msgs::msg::PointStamped> filter(buffer, "frame1", 10, node);
Notification n(1);
Expand All @@ -176,12 +170,10 @@ TEST(MessageFilter, preexistingTransforms)
TEST(MessageFilter, postTransforms)
{
auto node = rclcpp::Node::make_shared("tf2_ros_message_filter");
auto create_timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
node->get_node_base_interface(),
node->get_node_timers_interface());
auto create_timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(*node);

rclcpp::Clock::SharedPtr clock = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);
tf2_ros::Buffer buffer(clock);
tf2_ros::Buffer buffer(clock, *node);
buffer.setCreateTimerInterface(create_timer_interface);
tf2_ros::MessageFilter<geometry_msgs::msg::PointStamped> filter(buffer, "frame1", 10, node);
Notification n(1);
Expand Down Expand Up @@ -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<tf2_ros::CreateTimerROS>(
node->get_node_base_interface(),
node->get_node_timers_interface());
auto create_timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(*node);

rclcpp::Clock::SharedPtr clock = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);

Expand All @@ -225,7 +215,7 @@ TEST(MessageFilter, concurrentTransforms)
msg->header.stamp = stamp;
msg->header.frame_id = "frame2";

tf2_ros::Buffer buffer(clock);
tf2_ros::Buffer buffer(clock, *node);
for (int i = 0; i < 50; i++) {
buffer.setCreateTimerInterface(create_timer_interface);
tf2_ros::MessageFilter<geometry_msgs::msg::PointStamped> filter(buffer, "frame1", buffer_size,
Expand Down Expand Up @@ -294,12 +284,10 @@ TEST(MessageFilter, concurrentTransforms)
TEST(MessageFilter, setTargetFrame)
{
auto node = rclcpp::Node::make_shared("tf2_ros_message_filter");
auto create_timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
node->get_node_base_interface(),
node->get_node_timers_interface());
auto create_timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(*node);

rclcpp::Clock::SharedPtr clock = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);
tf2_ros::Buffer buffer(clock);
tf2_ros::Buffer buffer(clock, *node);
buffer.setCreateTimerInterface(create_timer_interface);
tf2_ros::MessageFilter<geometry_msgs::msg::PointStamped> filter(buffer, "frame1", 10, node);
Notification n(1);
Expand All @@ -325,12 +313,10 @@ TEST(MessageFilter, setTargetFrame)
TEST(MessageFilter, multipleTargetFrames)
{
auto node = rclcpp::Node::make_shared("tf2_ros_message_filter");
auto create_timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
node->get_node_base_interface(),
node->get_node_timers_interface());
auto create_timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(*node);

rclcpp::Clock::SharedPtr clock = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);
tf2_ros::Buffer buffer(clock);
tf2_ros::Buffer buffer(clock, *node);
buffer.setCreateTimerInterface(create_timer_interface);
tf2_ros::MessageFilter<geometry_msgs::msg::PointStamped> filter(buffer, "", 10, node);
Notification n(1);
Expand Down Expand Up @@ -366,12 +352,10 @@ TEST(MessageFilter, multipleTargetFrames)
TEST(MessageFilter, tolerance)
{
auto node = rclcpp::Node::make_shared("tf2_ros_message_filter");
auto create_timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
node->get_node_base_interface(),
node->get_node_timers_interface());
auto create_timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(*node);

rclcpp::Clock::SharedPtr clock = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);
tf2_ros::Buffer buffer(clock);
tf2_ros::Buffer buffer(clock, *node);
buffer.setCreateTimerInterface(create_timer_interface);
tf2_ros::MessageFilter<geometry_msgs::msg::PointStamped> filter(buffer, "frame1", 10, node);
Notification n(1);
Expand Down Expand Up @@ -497,12 +481,10 @@ TEST(MessageFilter, tolerance)
TEST(MessageFilter, checkStampPrecisionLoss)
{
auto node = rclcpp::Node::make_shared("tf2_ros_message_filter");
auto create_timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
node->get_node_base_interface(),
node->get_node_timers_interface());
auto create_timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(*node);

rclcpp::Clock::SharedPtr clock = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);
tf2_ros::Buffer buffer(clock);
tf2_ros::Buffer buffer(clock, *node);
buffer.setCreateTimerInterface(create_timer_interface);
tf2_ros::MessageFilter<geometry_msgs::msg::PointStamped> filter(buffer, "frame1", 10, node);
Notification n(1);
Expand Down
8 changes: 4 additions & 4 deletions test_tf2/test/test_static_publisher.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,7 @@ TEST(StaticTransformPublisher, a_b_different_times)
auto node = rclcpp::Node::make_shared("StaticTransformPublisher_a_b_different_times_test");

rclcpp::Clock::SharedPtr clock = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);
tf2_ros::Buffer mB(clock);
tf2_ros::Buffer mB(clock, *node);
tf2_ros::TransformListener tfl(mB, node, false);

rclcpp::executors::SingleThreadedExecutor executor;
Expand Down Expand Up @@ -88,7 +88,7 @@ TEST(StaticTransformPublisher, a_c_different_times)
auto node = rclcpp::Node::make_shared("StaticTransformPublisher_a_c_different_times_test");

rclcpp::Clock::SharedPtr clock = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);
tf2_ros::Buffer mB(clock);
tf2_ros::Buffer mB(clock, *node);
tf2_ros::TransformListener tfl(mB, node, false);

rclcpp::executors::SingleThreadedExecutor executor;
Expand Down Expand Up @@ -122,7 +122,7 @@ TEST(StaticTransformPublisher, a_d_different_times)
auto node = rclcpp::Node::make_shared("StaticTransformPublisher_a_d_different_times_test");

rclcpp::Clock::SharedPtr clock = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);
tf2_ros::Buffer mB(clock);
tf2_ros::Buffer mB(clock, *node);
tf2_ros::TransformListener tfl(mB, node, false);

rclcpp::executors::SingleThreadedExecutor executor;
Expand Down Expand Up @@ -173,7 +173,7 @@ TEST(StaticTransformPublisher, multiple_parent_test)
auto node = rclcpp::Node::make_shared("StaticTransformPublisher_a_d_different_times_test");

rclcpp::Clock::SharedPtr clock = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);
tf2_ros::Buffer mB(clock);
tf2_ros::Buffer mB(clock, *node);
tf2_ros::TransformListener tfl(mB, node, false);

rclcpp::executors::SingleThreadedExecutor executor;
Expand Down
57 changes: 29 additions & 28 deletions tf2_ros/include/tf2_ros/buffer.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,9 +49,12 @@
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "tf2_msgs/srv/frame_graph.hpp"
#include "rclcpp/node_interfaces/get_node_base_interface.hpp"
#include "rclcpp/node_interfaces/get_node_services_interface.hpp"
#include "rclcpp/node_interfaces/get_node_logging_interface.hpp"
#include "rclcpp/node_interfaces/get_node_services_interface.hpp"
#include "rclcpp/node_interfaces/node_base_interface.hpp"
#include "rclcpp/node_interfaces/node_logging_interface.hpp"
#include "rclcpp/node_interfaces/node_services_interface.hpp"
#include "rclcpp/node_interfaces/node_interfaces.hpp"
#include "rclcpp/rclcpp.hpp"

namespace tf2_ros
Expand All @@ -70,46 +73,38 @@ class Buffer : public BufferInterface, public AsyncBufferInterface, public tf2::
using tf2::BufferCore::canTransform;
using SharedPtr = std::shared_ptr<tf2_ros::Buffer>;

/** \brief Constructor for a Buffer object
* \param clock A clock to use for time and sleeping
* \param cache_time How long to keep a history of transforms
* \param interfaces Advertise the view_frames service that exposes debugging information from the buffer, based on a set of node interfaces
* \param qos If passed change the quality of service of the frames_server_ service
*/
Buffer(
rclcpp::Clock::SharedPtr clock,
rclcpp::node_interfaces::NodeInterfaces<
rclcpp::node_interfaces::NodeBaseInterface,
rclcpp::node_interfaces::NodeLoggingInterface,
rclcpp::node_interfaces::NodeServicesInterface
> node_interfaces,
tf2::Duration cache_time = tf2::Duration(tf2::BUFFER_CORE_DEFAULT_CACHE_TIME),
const rclcpp::QoS & qos = rclcpp::ServicesQoS());

/** \brief Constructor for a Buffer object
* \param clock A clock to use for time and sleeping
* \param cache_time How long to keep a history of transforms
* \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<typename NodeT = rclcpp::Node::SharedPtr>
[[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)
{
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<tf2_msgs::srv::FrameGraph>(
rclcpp::node_interfaces::get_node_base_interface(node),
rclcpp::node_interfaces::get_node_services_interface(node),
"tf2_frames", std::bind(
&Buffer::getFrames, this, std::placeholders::_1,
std::placeholders::_2), qos, nullptr);
}
Buffer(clock, *node, cache_time, qos);
}

/** \brief Get the transform between two frames by frame ID.
Expand Down Expand Up @@ -332,7 +327,13 @@ class Buffer : public BufferInterface, public AsyncBufferInterface, public tf2::
/// \brief A clock to use for time and sleeping
rclcpp::Clock::SharedPtr clock_;

/// \brief A node logging interface to access the buffer node's logger
/// \brief A set of interface to access the buffer's node
rclcpp::node_interfaces::NodeInterfaces<
rclcpp::node_interfaces::NodeBaseInterface,
rclcpp::node_interfaces::NodeLoggingInterface,
rclcpp::node_interfaces::NodeServicesInterface
> node_interfaces_;

rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface_;

/// \brief Interface for creating timers
Expand Down
14 changes: 12 additions & 2 deletions tf2_ros/include/tf2_ros/create_timer_ros.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,7 @@
#include "tf2/time.h"

#include "rclcpp/rclcpp.hpp"
#include "rclcpp/node_interfaces/node_interfaces.hpp"

namespace tf2_ros
{
Expand All @@ -50,6 +51,13 @@ namespace tf2_ros
class CreateTimerROS : public CreateTimerInterface
{
public:
CreateTimerROS(
rclcpp::node_interfaces::NodeInterfaces<
rclcpp::node_interfaces::NodeBaseInterface,
rclcpp::node_interfaces::NodeTimersInterface> node_interfaces,
rclcpp::CallbackGroup::SharedPtr callback_group = nullptr);

[[deprecated("Use rclcpp::node_interfaces::NodeInterfaces instead of multiple interfaces")]]
TF2_ROS_PUBLIC
CreateTimerROS(
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
Expand Down Expand Up @@ -119,8 +127,10 @@ class CreateTimerROS : public CreateTimerInterface
const TimerHandle & timer_handle,
TimerCallbackType callback);

rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_;
rclcpp::node_interfaces::NodeTimersInterface::SharedPtr node_timers_;
rclcpp::node_interfaces::NodeInterfaces<
rclcpp::node_interfaces::NodeBaseInterface,
rclcpp::node_interfaces::NodeTimersInterface> node_interfaces_;

TimerHandle next_timer_handle_index_;
std::unordered_map<TimerHandle, rclcpp::TimerBase::SharedPtr> timers_map_;
std::mutex timers_map_mutex_;
Expand Down
Loading