From 0522d7b77fd82b1c6a33fcd10477be5819c024e1 Mon Sep 17 00:00:00 2001 From: Adam Milner Date: Tue, 1 Aug 2023 11:11:09 -0700 Subject: [PATCH 1/2] Add lifecycle subscriber and refactor create_subscription and subscription_factory as necessary. Signed-of-by: Adam Milner Signed-off-by: Adam Milner --- rclcpp/include/rclcpp/create_subscription.hpp | 5 +- .../include/rclcpp/subscription_factory.hpp | 10 +-- .../rclcpp_lifecycle/lifecycle_node.hpp | 3 +- .../rclcpp_lifecycle/lifecycle_node_impl.hpp | 8 +- .../rclcpp_lifecycle/lifecycle_subscriber.hpp | 84 +++++++++++++++++++ 5 files changed, 97 insertions(+), 13 deletions(-) create mode 100644 rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_subscriber.hpp diff --git a/rclcpp/include/rclcpp/create_subscription.hpp b/rclcpp/include/rclcpp/create_subscription.hpp index 5b84930ff7..2ee83bbeee 100644 --- a/rclcpp/include/rclcpp/create_subscription.hpp +++ b/rclcpp/include/rclcpp/create_subscription.hpp @@ -119,12 +119,13 @@ create_subscription( subscription_topic_stats->set_publisher_timer(timer); } - auto factory = rclcpp::create_subscription_factory( + auto factory = + rclcpp::create_subscription_factory( std::forward(callback), options, msg_mem_strat, subscription_topic_stats - ); + ); const rclcpp::QoS & actual_qos = options.qos_overriding_options.get_policy_kinds().size() ? rclcpp::detail::declare_qos_parameters( diff --git a/rclcpp/include/rclcpp/subscription_factory.hpp b/rclcpp/include/rclcpp/subscription_factory.hpp index a1727eab5a..6cff806ff4 100644 --- a/rclcpp/include/rclcpp/subscription_factory.hpp +++ b/rclcpp/include/rclcpp/subscription_factory.hpp @@ -99,12 +99,9 @@ create_subscription_factory( rclcpp::node_interfaces::NodeBaseInterface * node_base, const std::string & topic_name, const rclcpp::QoS & qos - ) -> rclcpp::SubscriptionBase::SharedPtr + ) -> std::shared_ptr { - using rclcpp::Subscription; - using rclcpp::SubscriptionBase; - - auto sub = Subscription::make_shared( + auto sub = std::make_shared( node_base, rclcpp::get_message_type_support_handle(), topic_name, @@ -117,8 +114,7 @@ create_subscription_factory( // require this->shared_from_this() which cannot be called from // the constructor. sub->post_init_setup(node_base, qos, options); - auto sub_base_ptr = std::dynamic_pointer_cast(sub); - return sub_base_ptr; + return sub; } }; diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 097537b53a..da1b5217a3 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -88,6 +88,7 @@ #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/lifecycle_publisher.hpp" +#include "rclcpp_lifecycle/lifecycle_subscriber.hpp" #include "rclcpp_lifecycle/state.hpp" #include "rclcpp_lifecycle/transition.hpp" #include "rclcpp_lifecycle/visibility_control.h" @@ -240,7 +241,7 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, typename MessageT, typename CallbackT, typename AllocatorT = std::allocator, - typename SubscriptionT = rclcpp::Subscription, + typename SubscriptionT = rclcpp_lifecycle::LifecycleSubscription, typename MessageMemoryStrategyT = typename SubscriptionT::MessageMemoryStrategyType> std::shared_ptr create_subscription( diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp index 5ffc8a5f1b..3684eb3dec 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp @@ -43,6 +43,7 @@ #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "rclcpp_lifecycle/lifecycle_publisher.hpp" +#include "rclcpp_lifecycle/lifecycle_subscriber.hpp" #include "rclcpp_lifecycle/visibility_control.h" namespace rclcpp_lifecycle @@ -65,12 +66,11 @@ LifecycleNode::create_publisher( return pub; } -// TODO(karsten1987): Create LifecycleSubscriber template< typename MessageT, typename CallbackT, typename AllocatorT, - typename SubscriptionT, + typename SubscriptionT = rclcpp_lifecycle::LifecycleSubscription, typename MessageMemoryStrategyT> std::shared_ptr LifecycleNode::create_subscription( @@ -80,13 +80,15 @@ LifecycleNode::create_subscription( const rclcpp::SubscriptionOptionsWithAllocator & options, typename MessageMemoryStrategyT::SharedPtr msg_mem_strat) { - return rclcpp::create_subscription( + auto sub = rclcpp::create_subscription( *this, topic_name, qos, std::forward(callback), options, msg_mem_strat); + this->add_managed_entity(sub); + return sub; } template diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_subscriber.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_subscriber.hpp new file mode 100644 index 0000000000..d42d615939 --- /dev/null +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_subscriber.hpp @@ -0,0 +1,84 @@ +// Copyright 2023 Elroy Air, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef RCLCPP_LIFECYCLE__LIFECYCLE_SUBSCRIPTION_HPP_ +#define RCLCPP_LIFECYCLE__LIFECYCLE_SUBSCRIPTION_HPP_ + +// #include +// #include + +#include "rclcpp/subscription.hpp" + +namespace rclcpp_lifecycle +{ + +/// @brief Child class of rclcpp::Subscription that adds lifecycle functionality. +/** + * This class is a child class of rclcpp::Subscription that adds a lifecycle + * check to the callback function. If the node is in an inactive state, the + * callback will not be called. + */ +template< + typename MessageT, + typename AllocatorT = std::allocator, + typename SubscribedT = typename rclcpp::TypeAdapter::custom_type, + typename ROSMessageT = typename rclcpp::TypeAdapter::ros_message_type, + typename MessageMemoryStrategyT = rclcpp::message_memory_strategy::MessageMemoryStrategy< + ROSMessageT, + AllocatorT + >> +class LifecycleSubscription : public SimpleManagedEntity, + public rclcpp::Subscription +{ + +private: + using SubscriptionTopicStatisticsSharedPtr = + std::shared_ptr>; + +public: + LifecycleSubscription( + rclcpp::node_interfaces::NodeBaseInterface * node_base, + const rosidl_message_type_support_t & type_support_handle, + const std::string & topic_name, + const rclcpp::QoS & qos, + rclcpp::AnySubscriptionCallback callback, + const rclcpp::SubscriptionOptionsWithAllocator & options, + typename MessageMemoryStrategyT::SharedPtr message_memory_strategy, + SubscriptionTopicStatisticsSharedPtr subscription_topic_statistics = nullptr) + : rclcpp::Subscription( + node_base, + type_support_handle, + topic_name, + qos, + callback, + options, + message_memory_strategy, + subscription_topic_statistics) + { + } + + /// Check if we need to handle the message, and execute the callback if we do. + virtual void handle_message( + std::shared_ptr & message, const rclcpp::MessageInfo & message_info) override + { + if (!this->is_activated()) { + return; + } + rclcpp::Subscription::handle_message(message, message_info); + } +}; + +} // namespace rclcpp_lifecycle + +#endif // RCLCPP_LIFECYCLE__LIFECYCLE_SUBSCRIPTION_HPP_ From d388ebfbb374b8a66b95c14485562784af571acc Mon Sep 17 00:00:00 2001 From: Adam Milner Date: Tue, 1 Aug 2023 11:57:42 -0700 Subject: [PATCH 2/2] Update for github tests. Signed-off-by: Adam Milner --- .../rclcpp_lifecycle/lifecycle_subscriber.hpp | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_subscriber.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_subscriber.hpp index d42d615939..c00b2cf833 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_subscriber.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_subscriber.hpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef RCLCPP_LIFECYCLE__LIFECYCLE_SUBSCRIPTION_HPP_ -#define RCLCPP_LIFECYCLE__LIFECYCLE_SUBSCRIPTION_HPP_ +#ifndef RCLCPP_LIFECYCLE__LIFECYCLE_SUBSCRIBER_HPP_ +#define RCLCPP_LIFECYCLE__LIFECYCLE_SUBSCRIBER_HPP_ -// #include -// #include +#include +#include #include "rclcpp/subscription.hpp" @@ -41,7 +41,6 @@ template< class LifecycleSubscription : public SimpleManagedEntity, public rclcpp::Subscription { - private: using SubscriptionTopicStatisticsSharedPtr = std::shared_ptr>; @@ -69,7 +68,7 @@ class LifecycleSubscription : public SimpleManagedEntity, } /// Check if we need to handle the message, and execute the callback if we do. - virtual void handle_message( + void handle_message( std::shared_ptr & message, const rclcpp::MessageInfo & message_info) override { if (!this->is_activated()) { @@ -81,4 +80,4 @@ class LifecycleSubscription : public SimpleManagedEntity, } // namespace rclcpp_lifecycle -#endif // RCLCPP_LIFECYCLE__LIFECYCLE_SUBSCRIPTION_HPP_ +#endif // RCLCPP_LIFECYCLE__LIFECYCLE_SUBSCRIBER_HPP_