Add ABI-compatible static_only support for TransformListener

Signed-off-by: Taiga Arai <araitaiga2020.service@gmail.com>

Author: araitaiga

tf2_ros/include/tf2_ros/transform_listener.hpp

@@ -93,6 +93,18 @@ class TransformListener
   TF2_ROS_PUBLIC
   explicit TransformListener(tf2::BufferCore & buffer, bool spin_thread = true);
 
+  /** \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(
@@ -118,6 +130,31 @@ class TransformListener
       static_options)
   {}
 
+  /** \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(
@@ -147,6 +184,35 @@ class TransformListener
       static_options);
   }
 
+  /** \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();
 
@@ -165,7 +231,8 @@ class TransformListener
     const rclcpp::QoS & qos,
     const rclcpp::QoS & static_qos,
     const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options,
-    const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & static_options)
+    const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & static_options,
+    bool static_only = false)
   {
     spin_thread_ = spin_thread;
     node_base_interface_ = node_base;
@@ -181,14 +248,19 @@ class TransformListener
       // Create new callback group for message_subscription of tf and tf_static
       callback_group_ = node_base_interface_->create_callback_group(
         rclcpp::CallbackGroupType::MutuallyExclusive, false);
-      // Duplicate to modify option of subscription
-      rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> tf_options = options;
+
+      if (!static_only) {
+        // Duplicate to modify subscription options
+        rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> tf_options = options;
+        tf_options.callback_group = callback_group_;
+
+        message_subscription_tf_ = rclcpp::create_subscription<tf2_msgs::msg::TFMessage>(
+          node_parameters, node_topics, "/tf", qos, std::move(cb), tf_options);
+      }
+
       rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> tf_static_options = static_options;
-      tf_options.callback_group = callback_group_;
       tf_static_options.callback_group = callback_group_;
 
-      message_subscription_tf_ = rclcpp::create_subscription<tf2_msgs::msg::TFMessage>(
-        node_parameters, node_topics, "/tf", qos, std::move(cb), tf_options);
       message_subscription_tf_static_ = rclcpp::create_subscription<tf2_msgs::msg::TFMessage>(
         node_parameters,
         node_topics,
@@ -204,8 +276,10 @@ class TransformListener
       // Tell the buffer we have a dedicated thread to enable timeouts
       buffer_.setUsingDedicatedThread(true);
     } else {
-      message_subscription_tf_ = rclcpp::create_subscription<tf2_msgs::msg::TFMessage>(
-        node_parameters, node_topics, "/tf", qos, std::move(cb), options);
+      if (!static_only) {
+        message_subscription_tf_ = rclcpp::create_subscription<tf2_msgs::msg::TFMessage>(
+          node_parameters, node_topics, "/tf", qos, std::move(cb), options);
+      }
       message_subscription_tf_static_ = rclcpp::create_subscription<tf2_msgs::msg::TFMessage>(
         node_parameters,
         node_topics,
@@ -231,6 +305,78 @@ class TransformListener
   rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface_ {nullptr};
   rclcpp::CallbackGroup::SharedPtr callback_group_{nullptr};
 };
+
+/** \brief Convenience class for subscribing to static-only transforms
+ *
+ * While users can achieve the same thing with a normal TransformListener, the number of parameters that must be
+ * provided is unwieldy.
+ */
+class StaticTransformListener : public TransformListener
+{
+public:
+  /** \brief Simplified constructor for a static transform listener
+   * \see the simplified TransformListener documentation
+   */
+  TF2_ROS_PUBLIC
+  explicit StaticTransformListener(tf2::BufferCore & buffer, bool spin_thread = true)
+  : TransformListener(buffer, spin_thread, true)
+  {
+  }
+
+  /** \brief Node constructor */
+  template<class NodeT, class AllocatorT = std::allocator<void>>
+  StaticTransformListener(
+    tf2::BufferCore & buffer,
+    NodeT && node,
+    bool spin_thread = true,
+    const rclcpp::QoS & static_qos = StaticListenerQoS(),
+    const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & static_options =
+    detail::get_default_transform_listener_static_sub_options<AllocatorT>())
+  : TransformListener(
+      buffer,
+      node,
+      spin_thread,
+      rclcpp::QoS(1),
+      static_qos,
+      rclcpp::SubscriptionOptionsWithAllocator<AllocatorT>(),
+      static_options,
+      true)
+  {
+  }
+
+  /** \brief Node interface constructor */
+  template<class AllocatorT = std::allocator<void>>
+  StaticTransformListener(
+    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 = true,
+    const rclcpp::QoS & static_qos = StaticListenerQoS(),
+    const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & static_options =
+    detail::get_default_transform_listener_static_sub_options<AllocatorT>())
+  : TransformListener(
+      buffer,
+      node_base,
+      node_logging,
+      node_parameters,
+      node_topics,
+      spin_thread,
+      rclcpp::QoS(1),
+      static_qos,
+      rclcpp::SubscriptionOptionsWithAllocator<AllocatorT>(),
+      static_options,
+      true)
+  {
+  }
+
+  TF2_ROS_PUBLIC
+  virtual ~StaticTransformListener()
+  {
+  }
+};
+
 }  // namespace tf2_ros
 
 #endif  // TF2_ROS__TRANSFORM_LISTENER_HPP_

tf2_ros/src/transform_listener.cpp

@@ -67,6 +67,34 @@ TransformListener::TransformListener(tf2::BufferCore & buffer, bool spin_thread)
     detail::get_default_transform_listener_static_sub_options());
 }
 
+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

@@ -28,14 +28,13 @@
  */
 
 #include <gtest/gtest.h>
-
-#include <memory>
-
 #include <tf2_ros/buffer.hpp>
 #include <tf2_ros/transform_listener.hpp>
 #include <tf2_ros/transform_broadcaster.hpp>
 #include <tf2_ros/static_transform_broadcaster.hpp>
 
+#include <chrono>
+#include <memory>
 
 #include "node_wrapper.hpp"
 
@@ -53,6 +52,14 @@ class CustomNode : public rclcpp::Node
     tf_listener_ = std::make_shared<tf2_ros::TransformListener>(buffer, shared_from_this(), false);
   }
 
+  void init_static_tf_listener()
+  {
+    rclcpp::Clock::SharedPtr clock = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);
+    tf2_ros::Buffer buffer(clock);
+    tf_listener_ =
+      std::make_shared<tf2_ros::StaticTransformListener>(buffer, shared_from_this(), false);
+  }
+
 private:
   std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
 };
@@ -71,6 +78,14 @@ class CustomComposableNode : public rclcpp::Node
     tf_listener_ = std::make_shared<tf2_ros::TransformListener>(buffer, shared_from_this(), false);
   }
 
+  void init_static_tf_listener()
+  {
+    rclcpp::Clock::SharedPtr clock = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);
+    tf2_ros::Buffer buffer(clock);
+    tf_listener_ =
+      std::make_shared<tf2_ros::StaticTransformListener>(buffer, shared_from_this(), false);
+  }
+
 private:
   std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
 };
@@ -108,6 +123,85 @@ TEST(tf2_test_transform_listener, transform_listener_with_intraprocess)
   custom_node->init_tf_listener();
 }
 
+TEST(tf2_test_static_transform_listener, static_transform_listener_rclcpp_node)
+{
+  auto node = rclcpp::Node::make_shared("tf2_ros_static_transform_listener");
+
+  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)
+{
+  auto node = std::make_shared<NodeWrapper>("tf2_ros_static_transform_listener");
+
+  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_as_member)
+{
+  auto custom_node = std::make_shared<CustomNode>();
+  custom_node->init_static_tf_listener();
+}
+
+TEST(tf2_test_static_transform_listener, static_transform_listener_with_intraprocess)
+{
+  rclcpp::executors::SingleThreadedExecutor exec;
+  rclcpp::NodeOptions options;
+  options = options.use_intra_process_comms(true);
+  auto custom_node = std::make_shared<CustomComposableNode>(options);
+  custom_node->init_static_tf_listener();
+}
+
+TEST(tf2_test_listeners, static_vs_dynamic)
+{
+  auto node = rclcpp::Node::make_shared("tf2_ros_static_transform_listener");
+
+  rclcpp::Clock::SharedPtr clock = std::make_shared<rclcpp::Clock>(RCL_SYSTEM_TIME);
+  tf2_ros::Buffer dynamic_buffer(clock);
+  tf2_ros::Buffer static_buffer(clock);
+  tf2_ros::TransformListener tfl(dynamic_buffer, node, true);
+  tf2_ros::StaticTransformListener stfl(static_buffer, node, true);
+  tf2_ros::TransformBroadcaster broadcaster(node);
+  tf2_ros::StaticTransformBroadcaster static_broadcaster(node);
+
+  geometry_msgs::msg::TransformStamped static_trans;
+  static_trans.header.stamp = clock->now();
+  static_trans.header.frame_id = "parent_static";
+  static_trans.child_frame_id = "child_static";
+  static_trans.transform.rotation.w = 1.0;
+  static_broadcaster.sendTransform(static_trans);
+
+  geometry_msgs::msg::TransformStamped dynamic_trans;
+  dynamic_trans.header.frame_id = "parent_dynamic";
+  dynamic_trans.child_frame_id = "child_dynamic";
+  dynamic_trans.transform.rotation.w = 1.0;
+
+  for (int i = 0; i < 10; ++i) {
+    dynamic_trans.header.stamp = clock->now();
+    broadcaster.sendTransform(dynamic_trans);
+
+    rclcpp::spin_some(node);
+    rclcpp::sleep_for(std::chrono::milliseconds(10));
+  }
+
+  // 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", 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", tf2::TimePointZero));
+}
+
 int main(int argc, char ** argv)
 {
   testing::InitGoogleTest(&argc, argv);