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..c00b2cf833 --- /dev/null +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_subscriber.hpp @@ -0,0 +1,83 @@ +// 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_SUBSCRIBER_HPP_ +#define RCLCPP_LIFECYCLE__LIFECYCLE_SUBSCRIBER_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. + 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_SUBSCRIBER_HPP_