Enable TFListener to get Authority

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)
 

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 <gtest/gtest.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <chrono>
+#include <thread>
+
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <tf2_msgs/msg/tf_message.hpp>
+#include <tf2_ros/qos.hpp>
+
+
+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<rclcpp::Node>(pub_node_name, pub_namespace);
+  auto listener_node = std::make_shared<rclcpp::Node>("listener");
+
+  auto pub = rclcpp::create_publisher<tf2_msgs::msg::TFMessage>(
+    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();
+}

tf2_ros/include/tf2_ros/transform_listener.h

@@ -34,8 +34,10 @@
 
 #include <functional>
 #include <memory>
+#include <string>
 #include <thread>
 #include <utility>
+#include <vector>
 
 #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<void (tf2_msgs::msg::TFMessage::ConstSharedPtr)>;
+    using callback_t = std::function<void (tf2_msgs::msg::TFMessage::ConstSharedPtr,
+        const rclcpp::MessageInfo &)>;
     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<rclcpp::TopicEndpointInfo> tf_publishers_;
+  std::vector<rclcpp::TopicEndpointInfo> tf_static_publishers_;
 };
 }  // namespace tf2_ros
 

tf2_ros/src/transform_listener.cpp

@@ -67,13 +67,64 @@ TransformListener::~TransformListener()
 }
 
 
+inline
+bool
+_make_authority_str(
+  const std::vector<rclcpp::TopicEndpointInfo> & 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<rclcpp::TopicEndpointInfo> & 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);

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;
 };