Backport of #673 ("Adding static transform listener") to jazzy with ABI compatibility preserved (backport #927)

tf2_ros/include/tf2_ros/transform_listener.hpp

@@ -96,6 +96,18 @@ class TransformListener
     bool spin_thread = true,
     bool static_only = false);
 
+  /** \brief Simplified constructor for transform listener with static_only option.
+   *
+   * This constructor will create a new ROS 2 node under the hood.
+   * If you already have access to a ROS 2 node and you want to associate the TransformListener
+   * to it, then it's recommended to use one of the other constructors.
+   */
+  TF2_ROS_PUBLIC
+  explicit TransformListener(
+    tf2::BufferCore & buffer,
+    bool spin_thread,
+    bool static_only);
+
   /** \brief Node constructor */
   template<class NodeT, class AllocatorT = std::allocator<void>>
   TransformListener(
@@ -123,6 +135,31 @@ class TransformListener
       static_only)
   {}
 
+  /** \brief Node constructor with static_only option */
+  template<class NodeT, class AllocatorT = std::allocator<void>>
+  TransformListener(
+    tf2::BufferCore & buffer,
+    NodeT && node,
+    bool spin_thread,
+    const rclcpp::QoS & qos,
+    const rclcpp::QoS & static_qos,
+    const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options,
+    const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & static_options,
+    bool static_only)
+  : TransformListener(
+      buffer,
+      node->get_node_base_interface(),
+      node->get_node_logging_interface(),
+      node->get_node_parameters_interface(),
+      node->get_node_topics_interface(),
+      spin_thread,
+      qos,
+      static_qos,
+      options,
+      static_options,
+      static_only)
+  {}
+
   /** \brief Node interface constructor */
   template<class AllocatorT = std::allocator<void>>
   TransformListener(
@@ -154,6 +191,35 @@ class TransformListener
       static_only);
   }
 
+  /** \brief Node interface constructor with static_only option */
+  template<class AllocatorT = std::allocator<void>>
+  TransformListener(
+    tf2::BufferCore & buffer,
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
+    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
+    rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters,
+    rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
+    bool spin_thread,
+    const rclcpp::QoS & qos,
+    const rclcpp::QoS & static_qos,
+    const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options,
+    const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & static_options,
+    bool static_only)
+  : buffer_(buffer)
+  {
+    init(
+      node_base,
+      node_logging,
+      node_parameters,
+      node_topics,
+      spin_thread,
+      qos,
+      static_qos,
+      options,
+      static_options,
+      static_only);
+  }
+
   TF2_ROS_PUBLIC
   virtual ~TransformListener();
 
@@ -231,6 +297,71 @@ class TransformListener
     }
   }
 
+  // Overload of init() with the static_only flag
+  template<class AllocatorT = std::allocator<void>>
+  void init(
+    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
+    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
+    rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters,
+    rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
+    bool spin_thread,
+    const rclcpp::QoS & qos,
+    const rclcpp::QoS & static_qos,
+    const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options,
+    const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & static_options,
+    bool static_only)
+  {
+    if (!static_only) {
+      init(
+        node_base,
+        node_logging,
+        node_parameters,
+        node_topics,
+        spin_thread,
+        qos,
+        static_qos,
+        options,
+        static_options);
+      return;
+    }
+
+    spin_thread_ = spin_thread;
+    node_base_interface_ = node_base;
+    node_logging_interface_ = node_logging;
+
+    using callback_t = std::function<void (tf2_msgs::msg::TFMessage::ConstSharedPtr)>;
+    callback_t static_cb = std::bind(
+      &TransformListener::subscription_callback, this, std::placeholders::_1, true);
+
+    if (spin_thread_) {
+      callback_group_ = node_base_interface_->create_callback_group(
+        rclcpp::CallbackGroupType::MutuallyExclusive, false);
+      rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> tf_static_options = static_options;
+      tf_static_options.callback_group = callback_group_;
+
+      message_subscription_tf_static_ = rclcpp::create_subscription<tf2_msgs::msg::TFMessage>(
+        node_parameters,
+        node_topics,
+        "/tf_static",
+        static_qos,
+        std::move(static_cb),
+        tf_static_options);
+
+      executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+      executor_->add_callback_group(callback_group_, node_base_interface_);
+      dedicated_listener_thread_ = std::make_unique<std::thread>([&]() {executor_->spin();});
+      buffer_.setUsingDedicatedThread(true);
+    } else {
+      message_subscription_tf_static_ = rclcpp::create_subscription<tf2_msgs::msg::TFMessage>(
+        node_parameters,
+        node_topics,
+        "/tf_static",
+        static_qos,
+        std::move(static_cb),
+        static_options);
+    }
+  }
+
   bool spin_thread_{false};
   std::unique_ptr<std::thread> dedicated_listener_thread_ {nullptr};
   rclcpp::Executor::SharedPtr executor_ {nullptr};

tf2_ros/src/transform_listener.cpp

@@ -67,6 +67,34 @@ TransformListener::TransformListener(tf2::BufferCore & buffer, bool spin_thread,
     static_only);
 }
 
+TransformListener::TransformListener(tf2::BufferCore & buffer, bool spin_thread, bool static_only)
+: buffer_(buffer)
+{
+  rclcpp::NodeOptions options;
+  // create a unique name for the node
+  // but specify its name in .arguments to override any __node passed on the command line.
+  // avoiding sstream because it's behavior can be overridden by external libraries.
+  // See this issue: https://github.com/ros2/geometry2/issues/540
+  char node_name[42];
+  snprintf(
+    node_name, sizeof(node_name), "transform_listener_impl_%zx",
+    reinterpret_cast<size_t>(this)
+  );
+  options.arguments({"--ros-args", "-r", "__node:=" + std::string(node_name)});
+  options.start_parameter_event_publisher(false);
+  options.start_parameter_services(false);
+  optional_default_node_ = rclcpp::Node::make_shared("_", options);
+  init(
+    optional_default_node_->get_node_base_interface(),
+    optional_default_node_->get_node_logging_interface(),
+    optional_default_node_->get_node_parameters_interface(),
+    optional_default_node_->get_node_topics_interface(),
+    spin_thread, DynamicListenerQoS(), StaticListenerQoS(),
+    detail::get_default_transform_listener_sub_options(),
+    detail::get_default_transform_listener_static_sub_options(),
+    static_only);
+}
+
 TransformListener::~TransformListener()
 {
   if (spin_thread_) {

tf2_ros/test/test_transform_listener.cpp

@@ -37,7 +37,6 @@
 #include <tf2_ros/transform_broadcaster.hpp>
 #include <tf2_ros/static_transform_broadcaster.hpp>
 
-
 #include "node_wrapper.hpp"
 
 class CustomNode : public rclcpp::Node
@@ -131,6 +130,7 @@ TEST(tf2_test_static_transform_listener, static_transform_listener_rclcpp_node)
 
   rclcpp::Clock::SharedPtr clock = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);
   tf2_ros::Buffer buffer(clock);
+  tf2_ros::StaticTransformListener stfl(buffer, node, false);
 }
 
 TEST(tf2_test_static_transform_listener, static_transform_listener_custom_rclcpp_node)
@@ -139,7 +139,7 @@ TEST(tf2_test_static_transform_listener, static_transform_listener_custom_rclcpp
 
   rclcpp::Clock::SharedPtr clock = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);
   tf2_ros::Buffer buffer(clock);
-  tf2_ros::StaticTransformListener tfl(buffer, node, false);
+  tf2_ros::StaticTransformListener stfl(buffer, node, false);
 }
 
 TEST(tf2_test_static_transform_listener, static_transform_listener_as_member)
@@ -192,13 +192,15 @@ TEST(tf2_test_listeners, static_vs_dynamic)
   // Dynamic buffer should have both dynamic and static transforms available
   EXPECT_NO_THROW(
     dynamic_buffer.lookupTransform("parent_dynamic", "child_dynamic", tf2::TimePointZero));
-  EXPECT_NO_THROW(dynamic_buffer.lookupTransform("parent_static", "child_static", clock->now()));
+  EXPECT_NO_THROW(
+    dynamic_buffer.lookupTransform("parent_static", "child_static", tf2::TimePointZero));
 
   // Static buffer should have only static transforms available
   EXPECT_THROW(
     static_buffer.lookupTransform("parent_dynamic", "child_dynamic", tf2::TimePointZero),
     tf2::LookupException);
-  EXPECT_NO_THROW(static_buffer.lookupTransform("parent_static", "child_static", clock->now()));
+  EXPECT_NO_THROW(
+    static_buffer.lookupTransform("parent_static", "child_static", tf2::TimePointZero));
 }
 
 int main(int argc, char ** argv)