From 8f447c06fae4129dd88256ea105de527f2eee10c Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Mon, 25 Jul 2022 15:16:03 -0700 Subject: [PATCH 1/2] Add test checking for authority present in tf_listener Signed-off-by: Shane Loretz --- test_tf2/CMakeLists.txt | 6 ++ test_tf2/test/test_transform_listener.cpp | 92 +++++++++++++++++++++++ 2 files changed, 98 insertions(+) create mode 100644 test_tf2/test/test_transform_listener.cpp diff --git a/test_tf2/CMakeLists.txt b/test_tf2/CMakeLists.txt index a05818f39..7457495a0 100644 --- a/test_tf2/CMakeLists.txt +++ b/test_tf2/CMakeLists.txt @@ -123,6 +123,12 @@ if(TARGET test_tf2_bullet) tf2::tf2) endif() +ament_add_gtest(test_transform_listener test/test_transform_listener.cpp) +if(TARGET test_transform_listener) + target_link_libraries(test_transform_listener + tf2_ros::tf2_ros) +endif() + # TODO(ahcorde): enable once python part of tf2_geometry_msgs is working # add_launch_test(test/test_buffer_client.launch.py) diff --git a/test_tf2/test/test_transform_listener.cpp b/test_tf2/test/test_transform_listener.cpp new file mode 100644 index 000000000..00bfb26cb --- /dev/null +++ b/test_tf2/test/test_transform_listener.cpp @@ -0,0 +1,92 @@ +/********************************************************************* +* +* Software License Agreement (BSD License) +* +* Copyright (c) 2022, Open Source Robotics Foundation, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of Willow Garage, Inc. nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + + +TEST(transform_listener, authority_present) +{ + const std::string pub_namespace = "/tflistnertest/ns"; + const std::string pub_node_name = "sentinel_node_name"; + auto pub_node = std::make_shared(pub_node_name, pub_namespace); + auto listener_node = std::make_shared("listener"); + + auto pub = rclcpp::create_publisher( + pub_node, "/tf", tf2_ros::DynamicBroadcasterQoS()); + + + auto buffer = tf2_ros::Buffer(listener_node->get_clock()); + auto listener = tf2_ros::TransformListener(buffer, listener_node); + + // Wait for pub/sub to match + while(pub->get_subscription_count() == 0) { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + auto msg = tf2_msgs::msg::TFMessage(); + auto tf_stamped = geometry_msgs::msg::TransformStamped(); + tf_stamped.header.frame_id = "parent"; + tf_stamped.child_frame_id = "child"; + tf_stamped.header.stamp = pub_node->get_clock()->now(); + msg.transforms.push_back(tf_stamped); + + pub->publish(msg); + + // Wait for sub to get message + while(!buffer.canTransform("parent", "child", tf2::TimePointZero)) { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + const std::string as_yaml = buffer.allFramesAsYAML(); + EXPECT_TRUE(as_yaml.find(pub_node_name) != std::string::npos) << as_yaml; + EXPECT_TRUE(as_yaml.find(pub_namespace) != std::string::npos) << as_yaml; +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + return RUN_ALL_TESTS(); +} From 4357c3b795e479905f7c25b17ec1b8641dd51e58 Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Mon, 25 Jul 2022 15:16:20 -0700 Subject: [PATCH 2/2] Enable TFListener to get authority string Signed-off-by: Shane Loretz --- tf2_ros/include/tf2_ros/transform_listener.h | 24 +++++++-- tf2_ros/src/transform_listener.cpp | 55 +++++++++++++++++++- tf2_ros/test/node_wrapper.hpp | 3 ++ 3 files changed, 76 insertions(+), 6 deletions(-) diff --git a/tf2_ros/include/tf2_ros/transform_listener.h b/tf2_ros/include/tf2_ros/transform_listener.h index 01620a8f4..5f44fc155 100644 --- a/tf2_ros/include/tf2_ros/transform_listener.h +++ b/tf2_ros/include/tf2_ros/transform_listener.h @@ -34,8 +34,10 @@ #include #include +#include #include #include +#include #include "tf2/buffer_core.h" #include "tf2/time.h" @@ -118,12 +120,16 @@ class TransformListener spin_thread_ = spin_thread; node_base_interface_ = node->get_node_base_interface(); node_logging_interface_ = node->get_node_logging_interface(); + node_graph_interface_ = node->get_node_graph_interface(); - using callback_t = std::function; + using callback_t = std::function; callback_t cb = std::bind( - &TransformListener::subscription_callback, this, std::placeholders::_1, false); + &TransformListener::subscription_callback, this, + std::placeholders::_1, std::placeholders::_2, false); callback_t static_cb = std::bind( - &TransformListener::subscription_callback, this, std::placeholders::_1, true); + &TransformListener::subscription_callback, this, + std::placeholders::_1, std::placeholders::_2, true); if (spin_thread_) { // Create new callback group for message_subscription of tf and tf_static @@ -155,7 +161,13 @@ class TransformListener } /// Callback function for ros message subscriptoin TF2_ROS_PUBLIC - void subscription_callback(tf2_msgs::msg::TFMessage::ConstSharedPtr msg, bool is_static); + void subscription_callback( + tf2_msgs::msg::TFMessage::ConstSharedPtr msg, + const rclcpp::MessageInfo & msg_info, + bool is_static); + + std::string + make_authority_str(const rmw_gid_t & pub_gid, bool is_static); // ros::CallbackQueue tf_message_callback_queue_; bool spin_thread_{false}; @@ -170,6 +182,10 @@ class TransformListener tf2::TimePoint last_update_; rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_interface_; rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface_; + rclcpp::node_interfaces::NodeGraphInterface::SharedPtr node_graph_interface_; + + std::vector tf_publishers_; + std::vector tf_static_publishers_; }; } // namespace tf2_ros diff --git a/tf2_ros/src/transform_listener.cpp b/tf2_ros/src/transform_listener.cpp index 3f59d5f0c..3de8ade77 100644 --- a/tf2_ros/src/transform_listener.cpp +++ b/tf2_ros/src/transform_listener.cpp @@ -67,13 +67,64 @@ TransformListener::~TransformListener() } +inline +bool +_make_authority_str( + const std::vector & endpoints, + const rmw_gid_t & pub_gid, + std::string & authority) +{ + for (const rclcpp::TopicEndpointInfo & pub : endpoints) { + // Compare gid to see if this is the right publisher + bool match = true; + for (decltype(RMW_GID_STORAGE_SIZE) i = 0; i < RMW_GID_STORAGE_SIZE; ++i) { + if (pub.endpoint_gid()[i] != pub_gid.data[i]) { + match = false; + break; + } + } + if (match) { + authority = pub.node_namespace() + '/' + pub.node_name(); + return true; + } + } + return false; +} + + +std::string +TransformListener::make_authority_str(const rmw_gid_t & pub_gid, bool is_static) +{ + std::string authority = "Authority undetectable"; + std::vector & endpoints = tf_publishers_; + if (is_static) { + endpoints = tf_static_publishers_; + } + + if (!_make_authority_str(endpoints, pub_gid, authority)) { + // Update the cached endpoints + tf_publishers_ = node_graph_interface_->get_publishers_info_by_topic( + message_subscription_tf_->get_topic_name()); + tf_static_publishers_ = node_graph_interface_->get_publishers_info_by_topic( + message_subscription_tf_static_->get_topic_name()); + + // Try making the authority string again + _make_authority_str(endpoints, pub_gid, authority); + } + + return authority; +} + + void TransformListener::subscription_callback( const tf2_msgs::msg::TFMessage::ConstSharedPtr msg, + const rclcpp::MessageInfo & msg_info, bool is_static) { const tf2_msgs::msg::TFMessage & msg_in = *msg; - // TODO(tfoote) find a way to get the authority - std::string authority = "Authority undetectable"; + const rmw_gid_t & pub_gid = msg_info.get_rmw_message_info().publisher_gid; + + std::string authority = make_authority_str(pub_gid, is_static); for (size_t i = 0u; i < msg_in.transforms.size(); i++) { try { buffer_.setTransform(msg_in.transforms[i], authority, is_static); diff --git a/tf2_ros/test/node_wrapper.hpp b/tf2_ros/test/node_wrapper.hpp index 2534f99fe..a09256756 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::NodeGraphInterface::SharedPtr + get_node_graph_interface() {return this->node->get_node_graph_interface();} + private: rclcpp::Node::SharedPtr node; };