Add lifecycle node support

CMakeLists.txt

@@ -61,6 +61,8 @@ ament_export_dependencies(rclcpp rcutils builtin_interfaces)
 if(BUILD_TESTING)
   find_package(sensor_msgs REQUIRED)
   find_package(ament_lint_auto REQUIRED)
+  find_package(rclcpp_lifecycle REQUIRED)
+
   ament_lint_auto_find_test_dependencies()
 
   find_package(ament_cmake_gtest)
@@ -89,7 +91,7 @@ if(BUILD_TESTING)
   if(TARGET ${PROJECT_NAME}-test_subscriber)
     target_link_libraries(${PROJECT_NAME}-test_subscriber ${PROJECT_NAME})
     target_include_directories(${PROJECT_NAME}-test_subscriber PUBLIC include)
-    ament_target_dependencies(${PROJECT_NAME}-test_subscriber "rclcpp" "sensor_msgs")
+    ament_target_dependencies(${PROJECT_NAME}-test_subscriber "rclcpp" "rclcpp_lifecycle" "sensor_msgs")
   endif()
 
   ament_add_gtest(${PROJECT_NAME}-test_synchronizer test/test_synchronizer.cpp)

include/message_filters/subscriber.h

@@ -45,9 +45,12 @@
 namespace message_filters
 {
 
+template<class NodeType = rclcpp::Node>
 class SubscriberBase
 {
 public:
+  typedef std::shared_ptr<NodeType> NodePtr;
+
   virtual ~SubscriberBase() = default;
   /**
    * \brief Subscribe to a topic.
@@ -58,7 +61,7 @@ class SubscriberBase
    * \param topic The topic to subscribe to.
    * \param qos (optional) The rmw qos profile to use to subscribe
    */
-  virtual void subscribe(rclcpp::Node::SharedPtr node, const std::string& topic, const rmw_qos_profile_t qos = rmw_qos_profile_default) = 0;
+  virtual void subscribe(NodePtr node, const std::string& topic, const rmw_qos_profile_t qos = rmw_qos_profile_default) = 0;
 
   /**
    * \brief Subscribe to a topic.
@@ -69,7 +72,7 @@ class SubscriberBase
    * \param topic The topic to subscribe to.
    * \param qos (optional) The rmw qos profile to use to subscribe
    */
-  virtual void subscribe(rclcpp::Node * node, const std::string& topic, const rmw_qos_profile_t qos = rmw_qos_profile_default) = 0;
+  virtual void subscribe(NodeType * node, const std::string& topic, const rmw_qos_profile_t qos = rmw_qos_profile_default) = 0;
 
   /**
    * \brief Subscribe to a topic.
@@ -83,7 +86,7 @@ class SubscriberBase
    * \param options The subscription options to use to subscribe.
    */
   virtual void subscribe(
-    rclcpp::Node::SharedPtr node,
+    NodePtr node,
     const std::string& topic,
     const rmw_qos_profile_t qos,
     rclcpp::SubscriptionOptions options)
@@ -101,7 +104,7 @@ class SubscriberBase
    * \param qos (optional) The rmw qos profile to use to subscribe
    */
   virtual void subscribe(
-    rclcpp::Node * node,
+    NodeType * node,
     const std::string& topic,
     const rmw_qos_profile_t qos,
     rclcpp::SubscriptionOptions options)
@@ -122,7 +125,8 @@ class SubscriberBase
    */
   virtual void unsubscribe() = 0;
 };
-typedef std::shared_ptr<SubscriberBase> SubscriberBasePtr;
+template <typename T>
+using SubscriberBasePtr = std::shared_ptr<SubscriberBase<T>>;
 
 /**
  * \brief ROS subscription filter.
@@ -143,10 +147,11 @@ typedef std::shared_ptr<SubscriberBase> SubscriberBasePtr;
 void callback(const std::shared_ptr<M const>&);
 \endverbatim
  */
-template<class M>
-class Subscriber : public SubscriberBase, public SimpleFilter<M>
+template<class M, class NodeType = rclcpp::Node>
+class Subscriber : public SubscriberBase<NodeType>, public SimpleFilter<M>
 {
 public:
+  typedef std::shared_ptr<NodeType> NodePtr;
   typedef MessageEvent<M const> EventType;
 
   /**
@@ -158,12 +163,12 @@ class Subscriber : public SubscriberBase, public SimpleFilter<M>
    * \param topic The topic to subscribe to.
    * \param qos (optional) The rmw qos profile to use to subscribe
    */
-  Subscriber(rclcpp::Node::SharedPtr node, const std::string& topic, const rmw_qos_profile_t qos = rmw_qos_profile_default)
+  Subscriber(NodePtr node, const std::string& topic, const rmw_qos_profile_t qos = rmw_qos_profile_default)
   {
     subscribe(node, topic, qos);
   }
 
-  Subscriber(rclcpp::Node* node, const std::string& topic, const rmw_qos_profile_t qos = rmw_qos_profile_default)
+  Subscriber(NodeType * node, const std::string& topic, const rmw_qos_profile_t qos = rmw_qos_profile_default)
   {
     subscribe(node, topic, qos);
   }
@@ -179,16 +184,16 @@ class Subscriber : public SubscriberBase, public SimpleFilter<M>
    * \param options The subscription options to use to subscribe.
    */
   Subscriber(
-    rclcpp::Node::SharedPtr node,
+    NodePtr node,
     const std::string& topic,
     const rmw_qos_profile_t qos,
     rclcpp::SubscriptionOptions options)
   {
-    subscribe(node, topic, qos, options);
+      subscribe(node.get(), topic, qos, options);
   }
 
   Subscriber(
-    rclcpp::Node* node,
+    NodeType * node,
     const std::string& topic,
     const rmw_qos_profile_t qos,
     rclcpp::SubscriptionOptions options)
@@ -215,9 +220,9 @@ class Subscriber : public SubscriberBase, public SimpleFilter<M>
    * \param topic The topic to subscribe to.
    * \param qos (optional) The rmw qos profile to use to subscribe
    */
-  void subscribe(rclcpp::Node::SharedPtr node, const std::string& topic, const rmw_qos_profile_t qos = rmw_qos_profile_default)
+  void subscribe(NodePtr node, const std::string& topic, const rmw_qos_profile_t qos = rmw_qos_profile_default) override
   {
-    subscribe(node, topic, qos, rclcpp::SubscriptionOptions());
+    subscribe(node.get(), topic, qos, rclcpp::SubscriptionOptions());
   }
 
   /**
@@ -230,7 +235,7 @@ class Subscriber : public SubscriberBase, public SimpleFilter<M>
    * \param qos (optional) The rmw qos profile to use to subscribe
    */
   // TODO(wjwwood): deprecate in favor of API's that use `rclcpp::QoS` instead.
-  void subscribe(rclcpp::Node * node, const std::string& topic, const rmw_qos_profile_t qos = rmw_qos_profile_default)
+  void subscribe(NodeType * node, const std::string& topic, const rmw_qos_profile_t qos = rmw_qos_profile_default) override
   {
     subscribe(node, topic, qos, rclcpp::SubscriptionOptions());
   }
@@ -246,7 +251,7 @@ class Subscriber : public SubscriberBase, public SimpleFilter<M>
    * \param options The subscription options to use to subscribe.
    */
   void subscribe(
-    rclcpp::Node::SharedPtr node,
+    NodePtr node,
     const std::string& topic,
     const rmw_qos_profile_t qos,
     rclcpp::SubscriptionOptions options)
@@ -268,7 +273,7 @@ class Subscriber : public SubscriberBase, public SimpleFilter<M>
    */
   // TODO(wjwwood): deprecate in favor of API's that use `rclcpp::QoS` instead.
   void subscribe(
-    rclcpp::Node * node,
+    NodeType * node,
     const std::string& topic,
     const rmw_qos_profile_t qos,
     rclcpp::SubscriptionOptions options)
@@ -282,7 +287,7 @@ class Subscriber : public SubscriberBase, public SimpleFilter<M>
       rclcpp_qos.get_rmw_qos_profile() = qos;
       qos_ = qos;
       options_ = options;
-      sub_ = node->create_subscription<M>(topic, rclcpp_qos,
+      sub_ = node->template create_subscription<M>(topic, rclcpp_qos,
                [this](std::shared_ptr<M const> msg) {
                  this->cb(EventType(msg));
                }, options);
@@ -294,7 +299,7 @@ class Subscriber : public SubscriberBase, public SimpleFilter<M>
   /**
    * \brief Re-subscribe to a topic.  Only works if this subscriber has previously been subscribed to a topic.
    */
-  void subscribe()
+  void subscribe() override
   {
     if (!topic_.empty())
     {
@@ -309,7 +314,7 @@ class Subscriber : public SubscriberBase, public SimpleFilter<M>
   /**
    * \brief Force immediate unsubscription of this subscriber from its topic
    */
-  void unsubscribe()
+  void unsubscribe() override
   {
     sub_.reset();
   }
@@ -350,8 +355,8 @@ class Subscriber : public SubscriberBase, public SimpleFilter<M>
 
   typename rclcpp::Subscription<M>::SharedPtr sub_;
 
-  rclcpp::Node::SharedPtr node_shared_;
-  rclcpp::Node * node_raw_ {nullptr};
+  NodePtr node_shared_;
+  NodeType * node_raw_ {nullptr};
 
   std::string topic_;
   rmw_qos_profile_t qos_;

package.xml

@@ -27,6 +27,7 @@
   <test_depend>ament_cmake_pytest</test_depend>
   <test_depend>sensor_msgs</test_depend>
   <test_depend>std_msgs</test_depend>
+  <test_depend>rclcpp_lifecycle</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

test/test_subscriber.cpp

@@ -35,6 +35,7 @@
 #include <gtest/gtest.h>
 
 #include <rclcpp/rclcpp.hpp>
+#include <rclcpp_lifecycle/lifecycle_node.hpp>
 #include "message_filters/subscriber.h"
 #include "message_filters/chain.h"
 #include "sensor_msgs/msg/imu.hpp"
@@ -270,6 +271,26 @@ TEST(Subscriber, multipleCallbacksSomeFilterSomeDirect)
   EXPECT_NE(h.msg_.get(), h2.msg_.get());
 }
 
+TEST(Subscriber, lifecycle)
+{
+  auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test_node");
+  Helper h;
+  Subscriber<Msg, rclcpp_lifecycle::LifecycleNode> sub(node, "test_topic");
+  sub.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1));
+  auto pub = node->create_publisher<Msg>("test_topic", 10);
+  pub->on_activate();
+  rclcpp::Clock ros_clock;
+  auto start = ros_clock.now();
+  while (h.count_ == 0 && (ros_clock.now() - start) < rclcpp::Duration(1, 0))
+  {
+    pub->publish(Msg());
+    rclcpp::Rate(50).sleep();
+    rclcpp::spin_some(node->get_node_base_interface());
+  }
+
+  ASSERT_GT(h.count_, 0);
+}
+
 
 int main(int argc, char **argv){
   testing::InitGoogleTest(&argc, argv);