diff --git a/rclcpp_lifecycle/CMakeLists.txt b/rclcpp_lifecycle/CMakeLists.txt index e73a5a3ffd..d635d54133 100644 --- a/rclcpp_lifecycle/CMakeLists.txt +++ b/rclcpp_lifecycle/CMakeLists.txt @@ -19,6 +19,7 @@ find_package(lifecycle_msgs REQUIRED) ### CPP High level library add_library(rclcpp_lifecycle src/lifecycle_node.cpp + src/managed_entity.cpp src/node_interfaces/lifecycle_node_interface.cpp src/state.cpp src/transition.cpp diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp index 4442304c0c..b091ea9c00 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp @@ -968,10 +968,18 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface, bool register_on_error(std::function fcn); + RCLCPP_LIFECYCLE_PUBLIC + CallbackReturn + on_activate(const State & previous_state) override; + + RCLCPP_LIFECYCLE_PUBLIC + CallbackReturn + on_deactivate(const State & previous_state) override; + protected: RCLCPP_LIFECYCLE_PUBLIC void - add_publisher_handle(std::shared_ptr pub); + add_managed_entity(std::weak_ptr managed_entity); RCLCPP_LIFECYCLE_PUBLIC void diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp index a7f1c686bc..22fd7f9c08 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp @@ -49,11 +49,13 @@ LifecycleNode::create_publisher( const rclcpp::PublisherOptionsWithAllocator & options) { using PublisherT = rclcpp_lifecycle::LifecyclePublisher; - return rclcpp::create_publisher( + auto pub = rclcpp::create_publisher( *this, topic_name, qos, options); + this->add_managed_entity(pub); + return pub; } // TODO(karsten1987): Create LifecycleSubscriber diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp index d1b5f80975..8df08dcb41 100644 --- a/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp @@ -19,35 +19,20 @@ #include #include +#include "rclcpp/logging.hpp" #include "rclcpp/publisher.hpp" -#include "rclcpp/logging.hpp" +#include "rclcpp_lifecycle/managed_entity.hpp" + namespace rclcpp_lifecycle { -/// base class with only -/** - * pure virtual functions. A managed - * node can then deactivate or activate - * the publishing. - * It is more a convenient interface class - * than a necessary base class. - */ -class LifecyclePublisherInterface -{ -public: - virtual ~LifecyclePublisherInterface() {} - virtual void on_activate() = 0; - virtual void on_deactivate() = 0; - virtual bool is_activated() = 0; -}; - /// brief child class of rclcpp Publisher class. /** * Overrides all publisher functions to check for enabled/disabled state. */ template> -class LifecyclePublisher : public LifecyclePublisherInterface, +class LifecyclePublisher : public SimpleManagedEntity, public rclcpp::Publisher { public: @@ -64,7 +49,6 @@ class LifecyclePublisher : public LifecyclePublisherInterface, const rclcpp::QoS & qos, const rclcpp::PublisherOptionsWithAllocator & options) : rclcpp::Publisher(node_base, topic, qos, options), - enabled_(false), should_log_(true), logger_(rclcpp::get_logger("LifecyclePublisher")) { @@ -81,7 +65,7 @@ class LifecyclePublisher : public LifecyclePublisherInterface, virtual void publish(std::unique_ptr msg) { - if (!enabled_) { + if (!this->is_activated()) { log_publisher_not_enabled(); return; } @@ -97,32 +81,20 @@ class LifecyclePublisher : public LifecyclePublisherInterface, virtual void publish(const MessageT & msg) { - if (!enabled_) { + if (!this->is_activated()) { log_publisher_not_enabled(); return; } rclcpp::Publisher::publish(msg); } - virtual void - on_activate() - { - enabled_ = true; - } - - virtual void - on_deactivate() + void + on_activate() override { - enabled_ = false; + SimpleManagedEntity::on_activate(); should_log_ = true; } - virtual bool - is_activated() - { - return enabled_; - } - private: /// LifecyclePublisher log helper function /** @@ -146,7 +118,6 @@ class LifecyclePublisher : public LifecyclePublisherInterface, should_log_ = false; } - bool enabled_ = false; bool should_log_ = true; rclcpp::Logger logger_; }; diff --git a/rclcpp_lifecycle/include/rclcpp_lifecycle/managed_entity.hpp b/rclcpp_lifecycle/include/rclcpp_lifecycle/managed_entity.hpp new file mode 100644 index 0000000000..4f83411e80 --- /dev/null +++ b/rclcpp_lifecycle/include/rclcpp_lifecycle/managed_entity.hpp @@ -0,0 +1,68 @@ +// Copyright 2022 Open Source Robotics Foundation, 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__MANAGED_ENTITY_HPP_ +#define RCLCPP_LIFECYCLE__MANAGED_ENTITY_HPP_ + +#include + +#include "rclcpp_lifecycle/visibility_control.h" + +namespace rclcpp_lifecycle +{ + +/// Base class for lifecycle entities, like `LifecyclePublisher`. +class ManagedEntityInterface +{ +public: + RCLCPP_LIFECYCLE_PUBLIC + virtual + ~ManagedEntityInterface() {} + + RCLCPP_LIFECYCLE_PUBLIC + virtual + void + on_activate() = 0; + + RCLCPP_LIFECYCLE_PUBLIC + virtual + void + on_deactivate() = 0; +}; + +/// A simple implementation of `ManagedEntityInterface`, which toogles a flag. +class SimpleManagedEntity : public ManagedEntityInterface +{ +public: + RCLCPP_LIFECYCLE_PUBLIC + ~SimpleManagedEntity() override = default; + + RCLCPP_LIFECYCLE_PUBLIC + void + on_activate() override; + + RCLCPP_LIFECYCLE_PUBLIC + void + on_deactivate() override; + + RCLCPP_LIFECYCLE_PUBLIC + bool + is_activated() const; + +private: + std::atomic activated_ = false; +}; + +} // namespace rclcpp_lifecycle +#endif // RCLCPP_LIFECYCLE__MANAGED_ENTITY_HPP_ diff --git a/rclcpp_lifecycle/src/lifecycle_node.cpp b/rclcpp_lifecycle/src/lifecycle_node.cpp index d2bd49bcf7..c012e50804 100644 --- a/rclcpp_lifecycle/src/lifecycle_node.cpp +++ b/rclcpp_lifecycle/src/lifecycle_node.cpp @@ -639,11 +639,25 @@ LifecycleNode::shutdown(LifecycleNodeInterface::CallbackReturn & cb_return_code) rcl_lifecycle_shutdown_label, cb_return_code); } +node_interfaces::LifecycleNodeInterface::CallbackReturn +LifecycleNode::on_activate(const State &) +{ + impl_->on_activate(); + return LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +node_interfaces::LifecycleNodeInterface::CallbackReturn +LifecycleNode::on_deactivate(const State &) +{ + impl_->on_deactivate(); + return LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + void -LifecycleNode::add_publisher_handle( - std::shared_ptr pub) +LifecycleNode::add_managed_entity( + std::weak_ptr managed_entity) { - impl_->add_publisher_handle(pub); + impl_->add_managed_entity(managed_entity); } void diff --git a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp index 9f70d2c259..0fca4c19aa 100644 --- a/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp +++ b/rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp @@ -501,9 +501,9 @@ class LifecycleNode::LifecycleNodeInterfaceImpl } void - add_publisher_handle(std::shared_ptr pub) + add_managed_entity(std::weak_ptr managed_entity) { - weak_pubs_.push_back(pub); + weak_managed_entities_.push_back(managed_entity); } void @@ -512,6 +512,28 @@ class LifecycleNode::LifecycleNodeInterfaceImpl weak_timers_.push_back(timer); } + void + on_activate() + { + for (const auto & weak_entity : weak_managed_entities_) { + auto entity = weak_entity.lock(); + if (entity) { + entity->on_activate(); + } + } + } + + void + on_deactivate() + { + for (const auto & weak_entity : weak_managed_entities_) { + auto entity = weak_entity.lock(); + if (entity) { + entity->on_activate(); + } + } + } + rcl_lifecycle_state_machine_t state_machine_; State current_state_; std::map< @@ -538,7 +560,7 @@ class LifecycleNode::LifecycleNodeInterfaceImpl GetTransitionGraphSrvPtr srv_get_transition_graph_; // to controllable things - std::vector> weak_pubs_; + std::vector> weak_managed_entities_; std::vector> weak_timers_; }; diff --git a/rclcpp_lifecycle/src/managed_entity.cpp b/rclcpp_lifecycle/src/managed_entity.cpp new file mode 100644 index 0000000000..8cb8fb46ec --- /dev/null +++ b/rclcpp_lifecycle/src/managed_entity.cpp @@ -0,0 +1,35 @@ +// Copyright 2022 Open Source Robotics Foundation, 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. + +#include "rclcpp_lifecycle/managed_entity.hpp" + +namespace rclcpp_lifecycle +{ + +void SimpleManagedEntity::on_activate() +{ + activated_.store(true); +} + +void SimpleManagedEntity::on_deactivate() +{ + activated_.store(false); +} + +bool SimpleManagedEntity::is_activated() const +{ + return activated_.load(); +} + +} // namespace rclcpp_lifecycle diff --git a/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp b/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp index 555354a8c3..278b1f9026 100644 --- a/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp +++ b/rclcpp_lifecycle/test/test_lifecycle_publisher.cpp @@ -46,7 +46,7 @@ class EmptyLifecycleNode : public rclcpp_lifecycle::LifecycleNode publisher_ = std::make_shared>( get_node_base_interface().get(), std::string("topic"), rclcpp::QoS(10), options); - add_publisher_handle(publisher_); + add_managed_entity(publisher_); // For coverage this is being added here auto timer = create_wall_timer(std::chrono::seconds(1), []() {});