use node interfaces throughout tf2_ros (#108)

* Use node interface pointers in the TransformListener class In the TransformListener class, use node interfaces instead of using the node directly so that the code works with either rclcpp::Node or rclcpp_lifecycle::LifecycleNode. Retain the existing node-based interface for backwards compatibility. * Format code based on ament_cpplint and ament_uncrustify * use node interfaces consistently Signed-off-by: Karsten Knese <[email protected]> * adher to new rclcpp API Signed-off-by: Karsten Knese <[email protected]> * remove std::forward Signed-off-by: Karsten Knese <[email protected]> * export symbols for windows Signed-off-by: Karsten Knese <[email protected]> * callback also has to be public Signed-off-by: Karsten Knese <[email protected]> * address review Signed-off-by: Karsten Knese <[email protected]> * refactor node wrapper class Signed-off-by: Karsten Knese <[email protected]> * restore latched todo Signed-off-by: Karsten Knese <[email protected]>
ros2 · May 10, 2019 · db7493c · db7493c
1 parent 3323ebd
commit db7493c
Show file tree

Hide file tree

Showing 11 changed files with 418 additions and 133 deletions.
diff --git a/tf2_ros/CMakeLists.txt b/tf2_ros/CMakeLists.txt
@@ -126,6 +126,14 @@ if(BUILD_TESTING)
   ament_add_gtest(${PROJECT_NAME}_test_message_filter test/message_filter_test.cpp)
   target_link_libraries(${PROJECT_NAME}_test_message_filter ${PROJECT_NAME})
 
+  ament_add_gtest(${PROJECT_NAME}_test_transform_listener test/test_transform_listener.cpp)
+  target_link_libraries(${PROJECT_NAME}_test_transform_listener ${PROJECT_NAME})
+
+  ament_add_gtest(${PROJECT_NAME}_test_static_transform_broadcaster test/test_static_transform_broadcaster.cpp)
+  target_link_libraries(${PROJECT_NAME}_test_static_transform_broadcaster ${PROJECT_NAME})
+
+  ament_add_gtest(${PROJECT_NAME}_test_transform_broadcaster test/test_transform_broadcaster.cpp)
+  target_link_libraries(${PROJECT_NAME}_test_transform_broadcaster ${PROJECT_NAME})
 # TODO(tfoote) port tests to use ROS2 instead of ROS1 api.
 if (false)
 

diff --git a/tf2_ros/include/tf2_ros/static_transform_broadcaster.h b/tf2_ros/include/tf2_ros/static_transform_broadcaster.h
@@ -43,29 +43,24 @@
 namespace tf2_ros
 {
 
-
-/** \brief This class provides an easy way to publish coordinate frame transform information.  
- * It will handle all the messaging and stuffing of messages.  And the function prototypes lay out all the 
+/** \brief This class provides an easy way to publish coordinate frame transform information.
+ * It will handle all the messaging and stuffing of messages.  And the function prototypes lay out all the
  * necessary data needed for each message.  */
 
 class StaticTransformBroadcaster{
 public:
-
-  TF2_ROS_PUBLIC
-  StaticTransformBroadcaster(rclcpp::Node::SharedPtr node);
-
-  template<class AllocatorT = std::allocator<void>>
+  /** \brief Node interface constructor */
+  template<class NodeT, class AllocatorT = std::allocator<void>>
   StaticTransformBroadcaster(
-    rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_interface)
+    NodeT && node,
+    const rclcpp::QoS & qos = rclcpp::QoS(100),
+    const rclcpp::PublisherOptionsWithAllocator<AllocatorT> & options =
+      rclcpp::PublisherOptionsWithAllocator<AllocatorT>())
   {
-    auto qos = rclcpp::QoS(rclcpp::KeepLast(100));
     // TODO(tfoote) latched equivalent
-
-    using MessageT = tf2_msgs::msg::TFMessage;
-    using PublisherT = ::rclcpp::Publisher<MessageT, AllocatorT>;
-    publisher_ = rclcpp::create_publisher<MessageT, AllocatorT, PublisherT>(
-      node_topics_interface, "/tf_static", qos);
-  }
+    publisher_ = rclcpp::create_publisher<tf2_msgs::msg::TFMessage>(
+      node, "/tf_static", qos, options);
+  };
 
   /** \brief Send a TransformStamped message
    * The stamped data structure includes frame_id, and time, and parent_id already.  */

diff --git a/tf2_ros/include/tf2_ros/transform_broadcaster.h b/tf2_ros/include/tf2_ros/transform_broadcaster.h
@@ -41,21 +41,29 @@
 namespace tf2_ros
 {
 
-/** \brief This class provides an easy way to publish coordinate frame transform information.  
- * It will handle all the messaging and stuffing of messages.  And the function prototypes lay out all the 
+/** \brief This class provides an easy way to publish coordinate frame transform information.
+ * It will handle all the messaging and stuffing of messages.  And the function prototypes lay out all the
  * necessary data needed for each message.  */
 
 class TransformBroadcaster{
 public:
-  /** \brief Constructor (needs a ros::Node reference) */
-  TF2_ROS_PUBLIC
-  TransformBroadcaster(rclcpp::Node::SharedPtr node);
+  /** \brief Node interface constructor */
+  template<class NodeT, class AllocatorT = std::allocator<void>>
+  TransformBroadcaster(
+    NodeT && node,
+    const rclcpp::QoS & qos = rclcpp::QoS(100),
+    const rclcpp::PublisherOptionsWithAllocator<AllocatorT> & options =
+      rclcpp::PublisherOptionsWithAllocator<AllocatorT>())
+  {
+    publisher_ = rclcpp::create_publisher<tf2_msgs::msg::TFMessage>(
+      node, "/tf", qos, options);
+  };
 
-  /** \brief Send a StampedTransform 
+  /** \brief Send a StampedTransform
    * The stamped data structure includes frame_id, and time, and parent_id already.  */
   //  void sendTransform(const StampedTransform & transform);
 
-  /** \brief Send a vector of StampedTransforms 
+  /** \brief Send a vector of StampedTransforms
    * The stamped data structure includes frame_id, and time, and parent_id already.  */
   //void sendTransform(const std::vector<StampedTransform> & transforms);
 
@@ -70,8 +78,6 @@ class TransformBroadcaster{
   void sendTransform(const std::vector<geometry_msgs::msg::TransformStamped> & transforms);
 
 private:
-  /// Internal reference to ros::Node
-  rclcpp::Node::SharedPtr node_;
   rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr publisher_;
 
 };

diff --git a/tf2_ros/include/tf2_ros/transform_listener.h b/tf2_ros/include/tf2_ros/transform_listener.h
@@ -29,65 +29,104 @@
 
 /** \author Tully Foote */
 
-#ifndef TF2_ROS_TRANSFORMLISTENER_H
-#define TF2_ROS_TRANSFORMLISTENER_H
+#ifndef TF2_ROS__TRANSFORM_LISTENER_H_
+#define TF2_ROS__TRANSFORM_LISTENER_H_
 
+#include <memory>
+#include <string>
 #include <thread>
 #include "tf2_msgs/msg/tf_message.hpp"
 #include "rclcpp/rclcpp.hpp"
 
 #include "tf2_ros/buffer.h"
 #include "tf2_ros/visibility_control.h"
 
-namespace tf2_ros{
+
+namespace tf2_ros
+{
 
 /** \brief This class provides an easy way to request and receive coordinate frame transform information.
  */
 class TransformListener
 {
-
 public:
   /**@brief Constructor for transform listener */
   TF2_ROS_PUBLIC
-  TransformListener(tf2::BufferCore& buffer, bool spin_thread = true);
-
-  TF2_ROS_PUBLIC
-  TransformListener(tf2::BufferCore& buffer, rclcpp::Node::SharedPtr nh, bool spin_thread = true);
+  explicit TransformListener(tf2::BufferCore & buffer, bool spin_thread = true);
+
+  template<class NodeT, class AllocatorT = std::allocator<void>>
+  TransformListener(
+    tf2::BufferCore & buffer,
+    NodeT && node,
+    bool spin_thread = true,
+    const rclcpp::QoS & qos = rclcpp::QoS(100),
+    const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options =
+      rclcpp::SubscriptionOptionsWithAllocator<AllocatorT>())
+  : buffer_(buffer)
+  {
+    init(node, spin_thread, qos, options);
+  }
 
   TF2_ROS_PUBLIC
-  ~TransformListener();
+  virtual ~TransformListener();
 
 private:
+  template<class NodeT, class AllocatorT = std::allocator<void>>
+  void init(
+    NodeT && node,
+    bool spin_thread,
+    const rclcpp::QoS & qos = rclcpp::QoS(100),
+    const rclcpp::SubscriptionOptionsWithAllocator<AllocatorT> & options =
+      rclcpp::SubscriptionOptionsWithAllocator<AllocatorT>())
+  {
+    using callback_t = std::function<void(tf2_msgs::msg::TFMessage::SharedPtr)>;
+    callback_t cb = std::bind(&TransformListener::subscription_callback, this, std::placeholders::_1, false);
+    callback_t static_cb = std::bind(&TransformListener::subscription_callback, this, std::placeholders::_1, true);
 
-  /// Initialize this transform listener, subscribing, advertising services, etc.
-  void init();
-  void initThread();
+    message_subscription_tf_ = rclcpp::create_subscription<tf2_msgs::msg::TFMessage>(
+      node,
+      "/tf",
+      qos,
+      std::move(cb),
+      options);
+    message_subscription_tf_static_ = rclcpp::create_subscription<tf2_msgs::msg::TFMessage>(
+      node,
+      "/tf_static",
+      qos,
+      std::move(static_cb),
+      options);
+
+    if (spin_thread) {
+      initThread(node->get_node_base_interface());
+    }
+  }
+
+  TF2_ROS_PUBLIC
+  void initThread(rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface);
 
   /// Callback function for ros message subscriptoin
-  void subscription_callback(const tf2_msgs::msg::TFMessage::SharedPtr msg);
-  void static_subscription_callback(const tf2_msgs::msg::TFMessage::SharedPtr msg);
-  void subscription_callback_impl(const tf2_msgs::msg::TFMessage::SharedPtr msg, bool is_static);
+  TF2_ROS_PUBLIC
+  void subscription_callback(tf2_msgs::msg::TFMessage::SharedPtr msg, bool is_static);
 
   // ros::CallbackQueue tf_message_callback_queue_;
-  std::thread* dedicated_listener_thread_;
-  rclcpp::Node::SharedPtr node_;
+  std::thread * dedicated_listener_thread_ = nullptr;
+
+  rclcpp::Node::SharedPtr optional_default_node_ = nullptr;
   rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr message_subscription_tf_;
   rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr message_subscription_tf_static_;
-  tf2::BufferCore& buffer_;
-  bool using_dedicated_thread_;
+  tf2::BufferCore & buffer_;
+  bool using_dedicated_thread_ = false;
   tf2::TimePoint last_update_;
- 
+
   void dedicatedListenerThread()
   {
-    while (using_dedicated_thread_)
-    {
+    while (using_dedicated_thread_) {
       break;
-      //TODO(tfoote) reenable callback queue processing 
-      //tf_message_callback_queue_.callAvailable(ros::WallDuration(0.01));
+      // TODO(tfoote) reenable callback queue processing
+      // tf_message_callback_queue_.callAvailable(ros::WallDuration(0.01));
     }
-  };
-
+  }
 };
-}
+}  // namespace tf2_ros
 
-#endif //TF_TRANSFORMLISTENER_H
+#endif  // TF2_ROS__TRANSFORM_LISTENER_H_
diff --git a/tf2_ros/src/static_transform_broadcaster.cpp b/tf2_ros/src/static_transform_broadcaster.cpp
@@ -37,10 +37,6 @@
 
 namespace tf2_ros {
 
-StaticTransformBroadcaster::StaticTransformBroadcaster(rclcpp::Node::SharedPtr node)
-: StaticTransformBroadcaster(node->get_node_topics_interface())
-{}
-
 void StaticTransformBroadcaster::sendTransform(const geometry_msgs::msg::TransformStamped & msgtf)
 {
   std::vector<geometry_msgs::msg::TransformStamped> v1;

diff --git a/tf2_ros/src/transform_broadcaster.cpp b/tf2_ros/src/transform_broadcaster.cpp
@@ -37,20 +37,13 @@
 
 namespace tf2_ros {
 
-TransformBroadcaster::TransformBroadcaster(rclcpp::Node::SharedPtr node) :
-  node_(node)
-{
-  publisher_ = node_->create_publisher<tf2_msgs::msg::TFMessage>("/tf", 100);
-};
-
 void TransformBroadcaster::sendTransform(const geometry_msgs::msg::TransformStamped & msgtf)
 {
   std::vector<geometry_msgs::msg::TransformStamped> v1;
   v1.push_back(msgtf);
   sendTransform(v1);
 }
 
-
 void TransformBroadcaster::sendTransform(const std::vector<geometry_msgs::msg::TransformStamped> & msgtf)
 {
   tf2_msgs::msg::TFMessage message;
@@ -61,5 +54,4 @@ void TransformBroadcaster::sendTransform(const std::vector<geometry_msgs::msg::T
   publisher_->publish(message);
 }
 
-
 }