Remove lifecycle node (ros-controls#261)

* Remove LifecycleNode from controller_interface
* Handle error on transitions
* Comment out unused arg instead of void cast
* Add missing state include

Co-authored-by: Denis Štogl <[email protected]>

controller_interface/include/controller_interface/controller_interface.hpp

@@ -23,9 +23,10 @@
 
 #include "hardware_interface/loaned_command_interface.hpp"
 #include "hardware_interface/loaned_state_interface.hpp"
+#include "lifecycle_msgs/msg/state.hpp"
 
 #include "rclcpp/rclcpp.hpp"
-#include "rclcpp_lifecycle/lifecycle_node.hpp"
+#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
 
 namespace controller_interface
 {
@@ -93,13 +94,40 @@ class ControllerInterface : public rclcpp_lifecycle::node_interfaces::LifecycleN
   update() = 0;
 
   CONTROLLER_INTERFACE_PUBLIC
-  std::shared_ptr<rclcpp_lifecycle::LifecycleNode>
-  get_lifecycle_node();
+  std::shared_ptr<rclcpp::Node>
+  get_node();
+
+  /**
+   * The methods below are a substitute to the LifecycleNode methods with the same name.
+   * We cannot use a LifecycleNode because it would expose change
+   * state services to the rest of the ROS system.
+   * Only the Controller Manager should have possibility to change state of s controller.
+   *
+   * Hopefully in the future we can use a LifecycleNode where we disable modifications from the outside.
+   */
+  CONTROLLER_INTERFACE_PUBLIC
+  const rclcpp_lifecycle::State & configure();
+
+  CONTROLLER_INTERFACE_PUBLIC
+  const rclcpp_lifecycle::State & cleanup();
+
+  CONTROLLER_INTERFACE_PUBLIC
+  const rclcpp_lifecycle::State & deactivate();
+
+  CONTROLLER_INTERFACE_PUBLIC
+  const rclcpp_lifecycle::State & activate();
+
+  CONTROLLER_INTERFACE_PUBLIC
+  const rclcpp_lifecycle::State & shutdown();
+
+  CONTROLLER_INTERFACE_PUBLIC
+  const rclcpp_lifecycle::State & get_current_state() const;
 
 protected:
   std::vector<hardware_interface::LoanedCommandInterface> command_interfaces_;
   std::vector<hardware_interface::LoanedStateInterface> state_interfaces_;
-  std::shared_ptr<rclcpp_lifecycle::LifecycleNode> lifecycle_node_;
+  std::shared_ptr<rclcpp::Node> node_;
+  rclcpp_lifecycle::State lifecycle_state_;
 };
 
 using ControllerInterfaceSharedPtr = std::shared_ptr<ControllerInterface>;

controller_interface/src/controller_interface.cpp

@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include "controller_interface/controller_interface.hpp"
+#include <lifecycle_msgs/msg/state.hpp>
 
 #include <memory>
 #include <string>
@@ -25,30 +26,105 @@ namespace controller_interface
 return_type
 ControllerInterface::init(const std::string & controller_name)
 {
-  lifecycle_node_ = std::make_shared<rclcpp_lifecycle::LifecycleNode>(
+  node_ = std::make_shared<rclcpp::Node>(
     controller_name,
     rclcpp::NodeOptions().allow_undeclared_parameters(true).
     automatically_declare_parameters_from_overrides(true));
+  return return_type::SUCCESS;
+}
 
-  lifecycle_node_->register_on_configure(
-    std::bind(&ControllerInterface::on_configure, this, std::placeholders::_1));
-
-  lifecycle_node_->register_on_cleanup(
-    std::bind(&ControllerInterface::on_cleanup, this, std::placeholders::_1));
-
-  lifecycle_node_->register_on_activate(
-    std::bind(&ControllerInterface::on_activate, this, std::placeholders::_1));
-
-  lifecycle_node_->register_on_deactivate(
-    std::bind(&ControllerInterface::on_deactivate, this, std::placeholders::_1));
+const rclcpp_lifecycle::State & ControllerInterface::configure()
+{
+  switch (on_configure(lifecycle_state_)) {
+    case LifecycleNodeInterface::CallbackReturn::SUCCESS:
+      lifecycle_state_ = rclcpp_lifecycle::State(
+        lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
+        "inactive");
+      break;
+    case LifecycleNodeInterface::CallbackReturn::ERROR:
+      on_error(lifecycle_state_);
+      lifecycle_state_ = rclcpp_lifecycle::State(
+        lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, "finalized");
+      break;
+    case LifecycleNodeInterface::CallbackReturn::FAILURE:
+      break;
+  }
+  return lifecycle_state_;
+}
 
-  lifecycle_node_->register_on_shutdown(
-    std::bind(&ControllerInterface::on_shutdown, this, std::placeholders::_1));
+const rclcpp_lifecycle::State & ControllerInterface::cleanup()
+{
+  switch (on_cleanup(lifecycle_state_)) {
+    case LifecycleNodeInterface::CallbackReturn::SUCCESS:
+      lifecycle_state_ = rclcpp_lifecycle::State(
+        lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, "unconfigured");
+      break;
+    case LifecycleNodeInterface::CallbackReturn::ERROR:
+      on_error(lifecycle_state_);
+      lifecycle_state_ = rclcpp_lifecycle::State(
+        lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, "finalized");
+      break;
+    case LifecycleNodeInterface::CallbackReturn::FAILURE:
+      break;
+  }
+  return lifecycle_state_;
+}
+const rclcpp_lifecycle::State & ControllerInterface::deactivate()
+{
+  switch (on_deactivate(lifecycle_state_)) {
+    case LifecycleNodeInterface::CallbackReturn::SUCCESS:
+      lifecycle_state_ = rclcpp_lifecycle::State(
+        lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, "inactive");
+      break;
+    case LifecycleNodeInterface::CallbackReturn::ERROR:
+      on_error(lifecycle_state_);
+      lifecycle_state_ = rclcpp_lifecycle::State(
+        lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, "finalized");
+      break;
+    case LifecycleNodeInterface::CallbackReturn::FAILURE:
+      break;
+  }
+  return lifecycle_state_;
+}
+const rclcpp_lifecycle::State & ControllerInterface::activate()
+{
+  switch (on_activate(lifecycle_state_)) {
+    case LifecycleNodeInterface::CallbackReturn::SUCCESS:
+      lifecycle_state_ = rclcpp_lifecycle::State(
+        lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "active");
+      break;
+    case LifecycleNodeInterface::CallbackReturn::ERROR:
+      on_error(lifecycle_state_);
+      lifecycle_state_ = rclcpp_lifecycle::State(
+        lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, "finalized");
+      break;
+    case LifecycleNodeInterface::CallbackReturn::FAILURE:
+      break;
+  }
+  return lifecycle_state_;
+}
 
-  lifecycle_node_->register_on_error(
-    std::bind(&ControllerInterface::on_error, this, std::placeholders::_1));
+const rclcpp_lifecycle::State & ControllerInterface::shutdown()
+{
+  switch (on_activate(lifecycle_state_)) {
+    case LifecycleNodeInterface::CallbackReturn::SUCCESS:
+      lifecycle_state_ = rclcpp_lifecycle::State(
+        lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, "finalized");
+      break;
+    case LifecycleNodeInterface::CallbackReturn::ERROR:
+      on_error(lifecycle_state_);
+      lifecycle_state_ = rclcpp_lifecycle::State(
+        lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED, "finalized");
+      break;
+    case LifecycleNodeInterface::CallbackReturn::FAILURE:
+      break;
+  }
+  return lifecycle_state_;
+}
 
-  return return_type::SUCCESS;
+const rclcpp_lifecycle::State & ControllerInterface::get_current_state() const
+{
+  return lifecycle_state_;
 }
 
 void ControllerInterface::assign_interfaces(
@@ -65,10 +141,10 @@ void ControllerInterface::release_interfaces()
   state_interfaces_.clear();
 }
 
-std::shared_ptr<rclcpp_lifecycle::LifecycleNode>
-ControllerInterface::get_lifecycle_node()
+std::shared_ptr<rclcpp::Node>
+ControllerInterface::get_node()
 {
-  return lifecycle_node_;
+  return node_;
 }
 
 }  // namespace controller_interface

controller_manager/src/controller_manager.cpp

@@ -36,7 +36,7 @@ static constexpr const char * kControllerInterface = "controller_interface::Cont
 
 inline bool is_controller_running(controller_interface::ControllerInterface & controller)
 {
-  return controller.get_lifecycle_node()->get_current_state().id() ==
+  return controller.get_current_state().id() ==
          lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE;
 }
 
@@ -220,8 +220,8 @@ controller_interface::return_type ControllerManager::unload_controller(
   }
 
   RCLCPP_DEBUG(get_logger(), "Cleanup controller");
-  controller.c->get_lifecycle_node()->cleanup();
-  executor_->remove_node(controller.c->get_lifecycle_node()->get_node_base_interface());
+  controller.c->cleanup();
+  executor_->remove_node(controller.c->get_node());
   to.erase(found_it);
 
   // Destroys the old controllers list when the realtime thread is finished with it.
@@ -494,13 +494,8 @@ ControllerManager::add_controller_impl(
   }
 
   controller.c->init(controller.info.name);
-
-  // TODO(v-lopez) this should only be done if controller_manager is configured.
-  // Probably the whole load_controller part should fail if the controller_manager
-  // is not configured, should it implement a LifecycleNodeInterface
-  // https://github.com/ros-controls/ros2_control/issues/152
-  controller.c->get_lifecycle_node()->configure();
-  executor_->add_node(controller.c->get_lifecycle_node()->get_node_base_interface());
+  controller.c->configure();
+  executor_->add_node(controller.c->get_node());
   to.emplace_back(controller);
 
   // Destroys the old controllers list when the realtime thread is finished with it.
@@ -554,7 +549,7 @@ void ControllerManager::stop_controllers()
     }
     auto controller = found_it->c;
     if (is_controller_running(*controller)) {
-      const auto new_state = controller->get_lifecycle_node()->deactivate();
+      const auto new_state = controller->deactivate();
       if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) {
         RCLCPP_ERROR(
           get_logger(),
@@ -656,12 +651,12 @@ void ControllerManager::start_controllers()
     }
     controller->assign_interfaces(std::move(command_loans), std::move(state_loans));
 
-    const auto new_state = controller->get_lifecycle_node()->activate();
+    const auto new_state = controller->activate();
     if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) {
       RCLCPP_ERROR(
         get_logger(),
         "After activating, controller %s is in state %s, expected Active",
-        controller->get_lifecycle_node()->get_name(),
+        controller->get_node()->get_name(),
         new_state.label().c_str());
     }
   }
@@ -734,7 +729,7 @@ void ControllerManager::list_controllers_srv_cb(
     controller_manager_msgs::msg::ControllerState & cs = response->controller[i];
     cs.name = controllers[i].info.name;
     cs.type = controllers[i].info.type;
-    cs.state = controllers[i].c->get_lifecycle_node()->get_current_state().label();
+    cs.state = controllers[i].c->get_current_state().label();
 
 #ifdef TODO_IMPLEMENT_RESOURCE_CHECKING
     cs.claimed_resources.clear();

controller_manager/test/test_controller/test_controller.cpp

@@ -34,9 +34,18 @@ TestController::update()
 }
 
 rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
-TestController::on_configure(const rclcpp_lifecycle::State & previous_state)
+TestController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/)
 {
-  (void) previous_state;
+  return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
+}
+
+rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+TestController::on_cleanup(
+  const rclcpp_lifecycle::State & /*previous_state*/)
+{
+  if (cleanup_calls) {
+    (*cleanup_calls)++;
+  }
   return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
 }
 

controller_manager/test/test_controller/test_controller.hpp

@@ -59,7 +59,14 @@ class TestController : public controller_interface::ControllerInterface
   rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
   on_configure(const rclcpp_lifecycle::State & previous_state) override;
 
+  CONTROLLER_MANAGER_PUBLIC
+  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
+  on_cleanup(const rclcpp_lifecycle::State & previous_state) override;
+
   size_t internal_counter = 0;
+  // Variable where we store when cleanup was called, pointer because the controller
+  // is usually destroyed after cleanup
+  size_t * cleanup_calls = nullptr;
 };
 
 }  // namespace test_controller

controller_manager/test/test_controller_manager.cpp

@@ -45,7 +45,7 @@ TEST_F(TestControllerManager, controller_lifecycle) {
 
   EXPECT_EQ(
     lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
-    test_controller->get_lifecycle_node()->get_current_state().id());
+    test_controller->get_current_state().id());
 
   EXPECT_EQ(controller_interface::return_type::SUCCESS, cm_->update());
   EXPECT_EQ(0u, test_controller->internal_counter) << "Controller is not started";
@@ -72,7 +72,7 @@ TEST_F(TestControllerManager, controller_lifecycle) {
   );
   EXPECT_EQ(
     lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
-    test_controller->get_lifecycle_node()->get_current_state().id());
+    test_controller->get_current_state().id());
 
   EXPECT_EQ(controller_interface::return_type::SUCCESS, cm_->update());
   EXPECT_EQ(1u, test_controller->internal_counter);
@@ -103,7 +103,7 @@ TEST_F(TestControllerManager, controller_lifecycle) {
 
   EXPECT_EQ(
     lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
-    test_controller->get_lifecycle_node()->get_current_state().id());
+    test_controller->get_current_state().id());
   auto unload_future = std::async(
     std::launch::async,
     &controller_manager::ControllerManager::unload_controller, cm_,
@@ -121,6 +121,6 @@ TEST_F(TestControllerManager, controller_lifecycle) {
 
   EXPECT_EQ(
     lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
-    test_controller->get_lifecycle_node()->get_current_state().id());
+    test_controller->get_current_state().id());
   EXPECT_EQ(1, test_controller.use_count());
 }