From fa49f23389c7b73c3382cf1db63d13868949e176 Mon Sep 17 00:00:00 2001 From: "Joao C. Monteiro" Date: Fri, 30 Aug 2019 16:19:50 -0300 Subject: [PATCH 1/4] adapt Subscriber class to be used with LifecycleNode instances --- CMakeLists.txt | 4 ++- include/message_filters/subscriber.h | 37 ++++++++++++++++------------ package.xml | 1 + test/test_subscriber.cpp | 21 ++++++++++++++++ 4 files changed, 46 insertions(+), 17 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 26187625..072891a3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,6 +15,7 @@ find_package(ament_cmake_python REQUIRED) find_package(ament_cmake_ros REQUIRED) find_package(rclcpp REQUIRED) find_package(rcutils REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) find_package(builtin_interfaces REQUIRED) add_library(${PROJECT_NAME} src/connection.cpp) @@ -28,6 +29,7 @@ target_include_directories(${PROJECT_NAME} PUBLIC ament_target_dependencies(${PROJECT_NAME} "rclcpp" + "rclcpp_lifecycle" "rcutils" "builtin_interfaces" ) @@ -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) diff --git a/include/message_filters/subscriber.h b/include/message_filters/subscriber.h index 3764ebd9..57c226f4 100644 --- a/include/message_filters/subscriber.h +++ b/include/message_filters/subscriber.h @@ -45,9 +45,12 @@ namespace message_filters { +template class SubscriberBase { public: + typedef std::shared_ptr 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, + NoteType * 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 SubscriberBasePtr; +template +using SubscriberBasePtr = std::shared_ptr>; /** * \brief ROS subscription filter. @@ -143,10 +147,11 @@ typedef std::shared_ptr SubscriberBasePtr; void callback(const std::shared_ptr&); \endverbatim */ -template -class Subscriber : public SubscriberBase, public SimpleFilter +template +class Subscriber : public SubscriberBase, public SimpleFilter { public: + typedef std::shared_ptr NodePtr; typedef MessageEvent EventType; /** @@ -158,12 +163,12 @@ class Subscriber : public SubscriberBase, public SimpleFilter * \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); } @@ -215,7 +220,7 @@ class Subscriber : public SubscriberBase, public SimpleFilter * \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()); } @@ -230,7 +235,7 @@ class Subscriber : public SubscriberBase, public SimpleFilter * \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()); } @@ -282,7 +287,7 @@ class Subscriber : public SubscriberBase, public SimpleFilter rclcpp_qos.get_rmw_qos_profile() = qos; qos_ = qos; options_ = options; - sub_ = node->create_subscription(topic, rclcpp_qos, + sub_ = node->template create_subscription(topic, rclcpp_qos, [this](std::shared_ptr msg) { this->cb(EventType(msg)); }, options); @@ -294,7 +299,7 @@ class Subscriber : public SubscriberBase, public SimpleFilter /** * \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 /** * \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 typename rclcpp::Subscription::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_; diff --git a/package.xml b/package.xml index 6a9ea92a..c722fcc0 100644 --- a/package.xml +++ b/package.xml @@ -20,6 +20,7 @@ builtin_interfaces rclcpp + rclcpp_lifecycle rclpy ament_lint_auto diff --git a/test/test_subscriber.cpp b/test/test_subscriber.cpp index 79e5e35a..ee86a29b 100644 --- a/test/test_subscriber.cpp +++ b/test/test_subscriber.cpp @@ -35,6 +35,7 @@ #include #include +#include #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("test_node"); + Helper h; + Subscriber sub(node, "test_topic"); + sub.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); + auto pub = node->create_publisher("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); From 8a781bec2caca2d6cf95ca55ec55bd24b6f132c2 Mon Sep 17 00:00:00 2001 From: "Hunter L. Allen" Date: Mon, 14 Jun 2021 15:37:35 -0400 Subject: [PATCH 2/4] Update to new subscriber calls Signed-off-by: Hunter L. Allen --- include/message_filters/subscriber.h | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/include/message_filters/subscriber.h b/include/message_filters/subscriber.h index 57c226f4..7d76d3b2 100644 --- a/include/message_filters/subscriber.h +++ b/include/message_filters/subscriber.h @@ -104,7 +104,7 @@ class SubscriberBase * \param qos (optional) The rmw qos profile to use to subscribe */ virtual void subscribe( - NoteType * node, + NodeType * node, const std::string& topic, const rmw_qos_profile_t qos, rclcpp::SubscriptionOptions options) @@ -168,7 +168,7 @@ class Subscriber : public SubscriberBase, public SimpleFilter subscribe(node, topic, qos); } - Subscriber(NodeType* 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); } @@ -184,16 +184,16 @@ class Subscriber : public SubscriberBase, public SimpleFilter * \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) @@ -222,7 +222,7 @@ class Subscriber : public SubscriberBase, public SimpleFilter */ 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()); } /** @@ -251,7 +251,7 @@ class Subscriber : public SubscriberBase, public SimpleFilter * \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) @@ -273,7 +273,7 @@ class Subscriber : public SubscriberBase, public SimpleFilter */ // 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) From dbc693209363606c3deb751f0f93d72658eed9d5 Mon Sep 17 00:00:00 2001 From: "Hunter L. Allen" Date: Thu, 24 Jun 2021 22:26:24 -0400 Subject: [PATCH 3/4] Change rclcpp_lifecycle to a test dependency Signed-off-by: Hunter L. Allen --- CMakeLists.txt | 2 +- package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 072891a3..ccc82b64 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,7 +15,6 @@ find_package(ament_cmake_python REQUIRED) find_package(ament_cmake_ros REQUIRED) find_package(rclcpp REQUIRED) find_package(rcutils REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) find_package(builtin_interfaces REQUIRED) add_library(${PROJECT_NAME} src/connection.cpp) @@ -63,6 +62,7 @@ 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) diff --git a/package.xml b/package.xml index c722fcc0..102a8573 100644 --- a/package.xml +++ b/package.xml @@ -20,7 +20,6 @@ builtin_interfaces rclcpp - rclcpp_lifecycle rclpy ament_lint_auto @@ -28,6 +27,7 @@ ament_cmake_pytest sensor_msgs std_msgs + rclcpp_lifecycle ament_cmake From 825220db158a69484ac85dc0acf66412ce5e7438 Mon Sep 17 00:00:00 2001 From: "Hunter L. Allen" Date: Tue, 6 Jul 2021 18:30:13 -0400 Subject: [PATCH 4/4] Fix dependencies Signed-off-by: Hunter L. Allen --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ccc82b64..18e450d9 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -28,7 +28,6 @@ target_include_directories(${PROJECT_NAME} PUBLIC ament_target_dependencies(${PROJECT_NAME} "rclcpp" - "rclcpp_lifecycle" "rcutils" "builtin_interfaces" ) @@ -63,6 +62,7 @@ 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)