Automatically transition lifecycle entities when node transitions (#1863)

* Automatically transition LifecyclePublisher(s) between activated and inactive

Signed-off-by: Ivan Santiago Paunovic <[email protected]>

* Fix: Add created publishers to the managed entities vector

Signed-off-by: Ivan Santiago Paunovic <[email protected]>

* enabled_ -> activated_

Signed-off-by: Ivan Santiago Paunovic <[email protected]>

* Continue setting should_log_ as before

Signed-off-by: Ivan Santiago Paunovic <[email protected]>

* Fix visibility attributes so it works on Windows

Signed-off-by: Ivan Santiago Paunovic <[email protected]>

Author: ivanpauno

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), []() {});