add constructor to static tf broadcaster accepting node interfaces (#576

) Signed-off-by: Alberto Soragna <[email protected]>
ros2 · Dec 23, 2022 · 9139e0c · 9139e0c
1 parent a91b4ce
commit 9139e0c
Show file tree

Hide file tree

Showing 3 changed files with 73 additions and 5 deletions.
diff --git a/tf2_ros/include/tf2_ros/static_transform_broadcaster.h b/tf2_ros/include/tf2_ros/static_transform_broadcaster.h
@@ -38,6 +38,8 @@
 
 #include "tf2_ros/visibility_control.h"
 
+#include "rclcpp/node_interfaces/get_node_parameters_interface.hpp"
+#include "rclcpp/node_interfaces/get_node_topics_interface.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "geometry_msgs/msg/transform_stamped.hpp"
 #include "tf2_msgs/msg/tf_message.hpp"
@@ -53,7 +55,7 @@ namespace tf2_ros
 class StaticTransformBroadcaster
 {
 public:
-  /** \brief Node interface constructor */
+  /** \brief Node constructor */
   template<class NodeT, class AllocatorT = std::allocator<void>>
   StaticTransformBroadcaster(
     NodeT && node,
@@ -66,9 +68,30 @@ class StaticTransformBroadcaster
         rclcpp::QosPolicyKind::Reliability};
       return options;
     } ())
+    : StaticTransformBroadcaster(
+      rclcpp::node_interfaces::get_node_parameters_interface(node),
+      rclcpp::node_interfaces::get_node_topics_interface(node),
+      qos,
+      options)
+  {}
+
+  /** \brief Node interfaces constructor */
+  template<class AllocatorT = std::allocator<void>>
+  StaticTransformBroadcaster(
+    rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_parameters,
+    rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
+    const rclcpp::QoS & qos = StaticBroadcasterQoS(),
+    const rclcpp::PublisherOptionsWithAllocator<AllocatorT> & options = [] () {
+      rclcpp::PublisherOptionsWithAllocator<AllocatorT> options;
+      options.qos_overriding_options = rclcpp::QosOverridingOptions{
+        rclcpp::QosPolicyKind::Depth,
+        rclcpp::QosPolicyKind::History,
+        rclcpp::QosPolicyKind::Reliability};
+      return options;
+    } ())
   {
     publisher_ = rclcpp::create_publisher<tf2_msgs::msg::TFMessage>(
-      node, "/tf_static", qos, options);
+      node_parameters, node_topics, "/tf_static", qos, options);
   }
 
   /** \brief Send a TransformStamped message

diff --git a/tf2_ros/test/test_static_transform_broadcaster.cpp b/tf2_ros/test/test_static_transform_broadcaster.cpp
@@ -54,14 +54,40 @@ TEST(tf2_test_static_transform_broadcaster, transform_broadcaster_rclcpp_node)
 {
   auto node = rclcpp::Node::make_shared("tf2_ros_message_filter");
 
-  tf2_ros::StaticTransformBroadcaster tfb(node);
+  // Construct static tf broadcaster from node pointer
+  {
+    tf2_ros::StaticTransformBroadcaster tfb(node);
+  }
+  // Construct static tf broadcaster from node object
+  {
+    tf2_ros::StaticTransformBroadcaster tfb(*node);
+  }
+  // Construct static tf broadcaster from node interfaces
+  {
+    tf2_ros::StaticTransformBroadcaster tfb(
+      node->get_node_parameters_interface(),
+      node->get_node_topics_interface());
+  }
 }
 
 TEST(tf2_test_static_transform_broadcaster, transform_broadcaster_custom_rclcpp_node)
 {
   auto node = std::make_shared<NodeWrapper>("tf2_ros_message_filter");
 
-  tf2_ros::StaticTransformBroadcaster tfb(node);
+  // Construct static tf broadcaster from node pointer
+  {
+    tf2_ros::StaticTransformBroadcaster tfb(node);
+  }
+  // Construct static tf broadcaster from node object
+  {
+    tf2_ros::StaticTransformBroadcaster tfb(*node);
+  }
+  // Construct static tf broadcaster from node interfaces
+  {
+    tf2_ros::StaticTransformBroadcaster tfb(
+      node->get_node_parameters_interface(),
+      node->get_node_topics_interface());
+  }
 }
 
 TEST(tf2_test_static_transform_broadcaster, transform_broadcaster_as_member)

diff --git a/tf2_ros/test/test_transform_broadcaster.cpp b/tf2_ros/test/test_transform_broadcaster.cpp
@@ -61,13 +61,32 @@ TEST(tf2_test_transform_broadcaster, transform_broadcaster_rclcpp_node)
   {
     tf2_ros::TransformBroadcaster tfb(*node);
   }
+  // Construct tf broadcaster from node interfaces
+  {
+    tf2_ros::TransformBroadcaster tfb(
+      node->get_node_parameters_interface(),
+      node->get_node_topics_interface());
+  }
 }
 
 TEST(tf2_test_transform_broadcaster, transform_broadcaster_custom_rclcpp_node)
 {
   auto node = std::make_shared<NodeWrapper>("tf2_ros_message_filter");
 
-  tf2_ros::TransformBroadcaster tfb(node);
+  // Construct tf broadcaster from node pointer
+  {
+    tf2_ros::TransformBroadcaster tfb(node);
+  }
+  // Construct tf broadcaster from node object
+  {
+    tf2_ros::TransformBroadcaster tfb(*node);
+  }
+  // Construct tf broadcaster from node interfaces
+  {
+    tf2_ros::TransformBroadcaster tfb(
+      node->get_node_parameters_interface(),
+      node->get_node_topics_interface());
+  }
 }
 
 TEST(tf2_test_transform_broadcaster, transform_broadcaster_as_member)