Adding static transform listener

tf2_ros/include/tf2_ros/transform_listener.h

@@ -89,7 +89,10 @@ class 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 = true);
+  explicit TransformListener(
+    tf2::BufferCore & buffer,
+    bool spin_thread = true,
+    bool static_only = false);
 
   /** \brief Node constructor */
   template<class NodeT, class AllocatorT = std::allocator<void>>
@@ -102,7 +105,8 @@ class TransformListener
     const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options =
     detail::get_default_transform_listener_sub_options<AllocatorT>(),
     const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & static_options =
-    detail::get_default_transform_listener_static_sub_options<AllocatorT>())
+    detail::get_default_transform_listener_static_sub_options<AllocatorT>(),
+    bool static_only = false)
   : TransformListener(
       buffer,
       node->get_node_base_interface(),
@@ -113,7 +117,8 @@ class TransformListener
       qos,
       static_qos,
       options,
-      static_options)
+      static_options,
+      static_only)
   {}
 
   /** \brief Node interface constructor */
@@ -130,7 +135,8 @@ class TransformListener
     const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options =
     detail::get_default_transform_listener_sub_options<AllocatorT>(),
     const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & static_options =
-    detail::get_default_transform_listener_static_sub_options<AllocatorT>())
+    detail::get_default_transform_listener_static_sub_options<AllocatorT>(),
+    bool static_only = false)
   : buffer_(buffer)
   {
     init(
@@ -142,7 +148,8 @@ class TransformListener
       qos,
       static_qos,
       options,
-      static_options);
+      static_options,
+      static_only);
   }
 
   TF2_ROS_PUBLIC
@@ -163,7 +170,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;
@@ -179,14 +187,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,
@@ -202,8 +215,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,
@@ -229,6 +244,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_H_

tf2_ros/src/transform_listener.cpp

@@ -40,7 +40,7 @@
 namespace tf2_ros
 {
 
-TransformListener::TransformListener(tf2::BufferCore & buffer, bool spin_thread)
+TransformListener::TransformListener(tf2::BufferCore & buffer, bool spin_thread, bool static_only)
 : buffer_(buffer)
 {
   rclcpp::NodeOptions options;
@@ -64,7 +64,8 @@ TransformListener::TransformListener(tf2::BufferCore & buffer, bool spin_thread)
     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());
+    detail::get_default_transform_listener_static_sub_options(),
+    static_only);
 }
 
 TransformListener::~TransformListener()

tf2_ros/test/test_transform_listener.cpp

@@ -30,7 +30,10 @@
 #include <gtest/gtest.h>
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros/static_transform_broadcaster.h>
 
+#include <chrono>
 #include <memory>
 
 #include "node_wrapper.hpp"
@@ -49,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_;
 };
@@ -67,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_;
 };
@@ -104,6 +123,82 @@ 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);
+}
+
+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 tfl(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", clock->now()));
+
+  // 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()));
+}
+
 int main(int argc, char ** argv)
 {
   testing::InitGoogleTest(&argc, argv);