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

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();
 
@@ -216,6 +282,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};
@@ -231,6 +362,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

@@ -66,6 +66,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_) {