Automatically transition lifecycle entities when node transitions

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

rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp

@@ -968,10 +968,18 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface,
  bool
  register_on_error(std::function<LifecycleNodeInterface::CallbackReturn(const State &)> 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<rclcpp_lifecycle::LifecyclePublisherInterface> pub);
+ add_managed_entity(std::weak_ptr<rclcpp_lifecycle::ManagedEntityInterface> managed_entity);
 
  RCLCPP_LIFECYCLE_PUBLIC
  void

rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node_impl.hpp

@@ -49,11 +49,13 @@ LifecycleNode::create_publisher(
  const rclcpp::PublisherOptionsWithAllocator<AllocatorT> & options)
 {
  using PublisherT = rclcpp_lifecycle::LifecyclePublisher<MessageT, AllocatorT>;
- return rclcpp::create_publisher<MessageT, AllocatorT, PublisherT>(
+ auto pub = rclcpp::create_publisher<MessageT, AllocatorT, PublisherT>(
  *this,
  topic_name,
  qos,
  options);
+ this->add_managed_entity(pub);
+ return pub;
 }
 
 // TODO(karsten1987): Create LifecycleSubscriber

rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_publisher.hpp

@@ -19,35 +19,20 @@
 #include <string>
 #include <utility>
 
+#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<typename MessageT, typename Alloc = std::allocator<void>>
-class LifecyclePublisher : public LifecyclePublisherInterface,
+class LifecyclePublisher : public SimpleManagedEntity,
  public rclcpp::Publisher<MessageT, Alloc>
 {
 public:
@@ -64,7 +49,6 @@ class LifecyclePublisher : public LifecyclePublisherInterface,
  const rclcpp::QoS & qos,
  const rclcpp::PublisherOptionsWithAllocator<Alloc> & options)
  : rclcpp::Publisher<MessageT, Alloc>(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<MessageT, MessageDeleter> 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<MessageT, Alloc>::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_;
 };

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 <atomic>
+
+#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<bool> activated_ = false;
+};
+
+} // namespace rclcpp_lifecycle
+#endif // RCLCPP_LIFECYCLE__MANAGED_ENTITY_HPP_

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<rclcpp_lifecycle::LifecyclePublisherInterface> pub)
+LifecycleNode::add_managed_entity(
+ std::weak_ptr<rclcpp_lifecycle::ManagedEntityInterface> managed_entity)
 {
- impl_->add_publisher_handle(pub);
+ impl_->add_managed_entity(managed_entity);
 }
 
 void

rclcpp_lifecycle/src/lifecycle_node_interface_impl.hpp

@@ -501,9 +501,9 @@ class LifecycleNode::LifecycleNodeInterfaceImpl
  }
 
  void
- add_publisher_handle(std::shared_ptr<rclcpp_lifecycle::LifecyclePublisherInterface> pub)
+ add_managed_entity(std::weak_ptr<rclcpp_lifecycle::ManagedEntityInterface> 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<std::weak_ptr<rclcpp_lifecycle::LifecyclePublisherInterface>> weak_pubs_;
+ std::vector<std::weak_ptr<rclcpp_lifecycle::ManagedEntityInterface>> weak_managed_entities_;
  std::vector<std::weak_ptr<rclcpp::TimerBase>> weak_timers_;
 };
 

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

rclcpp_lifecycle/test/test_lifecycle_publisher.cpp

@@ -46,7 +46,7 @@ class EmptyLifecycleNode : public rclcpp_lifecycle::LifecycleNode
  publisher_ =
  std::make_shared<rclcpp_lifecycle::LifecyclePublisher<test_msgs::msg::Empty>>(
  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), []() {});