Sq: Lifecycle Separation of Concerns

Signed-off-by: Tom Groechel <tgroeche@umich.edu>

Author: Thomas Groechel

rclcpp_lifecycle/CMakeLists.txt

@@ -23,7 +23,10 @@ find_package(rosidl_typesupport_cpp REQUIRED)
 ### CPP High level library
 add_library(rclcpp_lifecycle
   src/lifecycle_node.cpp
+  src/lifecycle_node_entities_manager.cpp
   src/lifecycle_node_interface_impl.cpp
+  src/lifecycle_node_state_manager.cpp
+  src/lifecycle_node_state_services_manager.cpp
   src/managed_entity.cpp
   src/node_interfaces/lifecycle_node_interface.cpp
   src/state.cpp

rclcpp_lifecycle/include/rclcpp_lifecycle/lifecycle_node.hpp

@@ -1004,7 +1004,8 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface,
    */
   RCLCPP_LIFECYCLE_PUBLIC
   bool
-  register_on_configure(std::function<LifecycleNodeInterface::CallbackReturn(const State &)> fcn);
+  register_on_configure(
+    std::function<LifecycleNodeInterface::CallbackReturn(const State &)> fcn);
 
   /// Register the cleanup callback
   /**
@@ -1014,7 +1015,8 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface,
    */
   RCLCPP_LIFECYCLE_PUBLIC
   bool
-  register_on_cleanup(std::function<LifecycleNodeInterface::CallbackReturn(const State &)> fcn);
+  register_on_cleanup(
+    std::function<LifecycleNodeInterface::CallbackReturn(const State &)> fcn);
 
   /// Register the shutdown callback
   /**
@@ -1024,7 +1026,8 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface,
    */
   RCLCPP_LIFECYCLE_PUBLIC
   bool
-  register_on_shutdown(std::function<LifecycleNodeInterface::CallbackReturn(const State &)> fcn);
+  register_on_shutdown(
+    std::function<LifecycleNodeInterface::CallbackReturn(const State &)> fcn);
 
   /// Register the activate callback
   /**
@@ -1034,7 +1037,8 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface,
    */
   RCLCPP_LIFECYCLE_PUBLIC
   bool
-  register_on_activate(std::function<LifecycleNodeInterface::CallbackReturn(const State &)> fcn);
+  register_on_activate(
+    std::function<LifecycleNodeInterface::CallbackReturn(const State &)> fcn);
 
   /// Register the deactivate callback
   /**
@@ -1044,7 +1048,8 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface,
    */
   RCLCPP_LIFECYCLE_PUBLIC
   bool
-  register_on_deactivate(std::function<LifecycleNodeInterface::CallbackReturn(const State &)> fcn);
+  register_on_deactivate(
+    std::function<LifecycleNodeInterface::CallbackReturn(const State &)> fcn);
 
   /// Register the error callback
   /**
@@ -1054,7 +1059,8 @@ class LifecycleNode : public node_interfaces::LifecycleNodeInterface,
    */
   RCLCPP_LIFECYCLE_PUBLIC
   bool
-  register_on_error(std::function<LifecycleNodeInterface::CallbackReturn(const State &)> fcn);
+  register_on_error(
+    std::function<LifecycleNodeInterface::CallbackReturn(const State &)> fcn);
 
   RCLCPP_LIFECYCLE_PUBLIC
   CallbackReturn

rclcpp_lifecycle/src/lifecycle_node.cpp

@@ -541,6 +541,7 @@ LifecycleNode::register_on_error(
     lifecycle_msgs::msg::State::TRANSITION_STATE_ERRORPROCESSING, fcn);
 }
 
+
 const State &
 LifecycleNode::get_current_state() const
 {

rclcpp_lifecycle/src/lifecycle_node_entities_manager.cpp

@@ -0,0 +1,55 @@
+// Copyright 2016 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 "lifecycle_node_entities_manager.hpp"
+
+namespace rclcpp_lifecycle
+{
+
+void
+LifecycleNodeEntitiesManager::on_activate() const
+{
+  for (const auto & weak_entity : weak_managed_entities_) {
+    auto entity = weak_entity.lock();
+    if (entity) {
+      entity->on_activate();
+    }
+  }
+}
+
+void
+LifecycleNodeEntitiesManager::on_deactivate() const
+{
+  for (const auto & weak_entity : weak_managed_entities_) {
+    auto entity = weak_entity.lock();
+    if (entity) {
+      entity->on_deactivate();
+    }
+  }
+}
+
+void
+LifecycleNodeEntitiesManager::add_managed_entity(
+  std::weak_ptr<rclcpp_lifecycle::ManagedEntityInterface> managed_entity)
+{
+  weak_managed_entities_.push_back(managed_entity);
+}
+
+void
+LifecycleNodeEntitiesManager::add_timer_handle(
+  std::shared_ptr<rclcpp::TimerBase> timer)
+{
+  weak_timers_.push_back(timer);
+}
+
+}  // namespace rclcpp_lifecycle

rclcpp_lifecycle/src/lifecycle_node_entities_manager.hpp

@@ -0,0 +1,47 @@
+// Copyright 2023 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 LIFECYCLE_NODE_ENTITIES_MANAGER_HPP_
+#define LIFECYCLE_NODE_ENTITIES_MANAGER_HPP_
+
+#include <vector>
+#include <memory>
+#include "rclcpp_lifecycle/managed_entity.hpp"
+#include <rclcpp/timer.hpp>
+
+namespace rclcpp_lifecycle
+{
+
+class LifecycleNodeEntitiesManager
+{
+public:
+  void
+  on_activate() const;
+
+  void
+  on_deactivate() const;
+
+  void
+  add_managed_entity(std::weak_ptr<rclcpp_lifecycle::ManagedEntityInterface> managed_entity);
+
+  void
+  add_timer_handle(std::shared_ptr<rclcpp::TimerBase> timer);
+
+private:
+  std::vector<std::weak_ptr<rclcpp_lifecycle::ManagedEntityInterface>> weak_managed_entities_;
+  std::vector<std::weak_ptr<rclcpp::TimerBase>> weak_timers_;
+};
+
+}  // namespace rclcpp_lifecycle
+#endif  // LIFECYCLE_NODE_ENTITIES_MANAGER_HPP_