Use lifecycle nodes in controllers again

controller_interface/include/controller_interface/controller_interface.hpp

@@ -23,10 +23,9 @@
 
 #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/node_interfaces/lifecycle_node_interface.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
 
 namespace controller_interface
 {
@@ -80,16 +79,30 @@ class ControllerInterface : public rclcpp_lifecycle::node_interfaces::LifecycleN
   void release_interfaces();
 
   CONTROLLER_INTERFACE_PUBLIC
-  virtual LifecycleNodeInterface::CallbackReturn on_init() = 0;
+  virtual return_type init(const std::string & controller_name);
 
+  /// Custom configure method to read additional parameters for controller-nodes
+  /*
+   * Override default implementation for configure of LifecycleNode to get parameters.
+   */
   CONTROLLER_INTERFACE_PUBLIC
-  virtual return_type init(const std::string & controller_name);
+  const rclcpp_lifecycle::State & configure();
+
+  /// Extending interface with initialization method which is individual for each controller
+  CONTROLLER_INTERFACE_PUBLIC
+  virtual LifecycleNodeInterface::CallbackReturn on_init() = 0;
 
   CONTROLLER_INTERFACE_PUBLIC
   virtual return_type update(const rclcpp::Time & time, const rclcpp::Duration & period) = 0;
 
   CONTROLLER_INTERFACE_PUBLIC
-  std::shared_ptr<rclcpp::Node> get_node();
+  std::shared_ptr<rclcpp_lifecycle::LifecycleNode> get_node();
+
+  CONTROLLER_INTERFACE_PUBLIC
+  const rclcpp_lifecycle::State & get_state() const;
+
+  CONTROLLER_INTERFACE_PUBLIC
+  unsigned int get_update_rate() const;
 
   /// Declare and initialize a parameter with a type.
   /**
@@ -111,42 +124,10 @@ class ControllerInterface : public rclcpp_lifecycle::node_interfaces::LifecycleN
     }
   }
 
-  /**
-   * The methods below are a substitute to the LifecycleNode methods with the same name.
-   * The Life cycle is shown in ROS2 design document:
-   * https://design.ros2.org/articles/node_lifecycle.html
-   * 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 a 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_state() const;
-
-  CONTROLLER_INTERFACE_PUBLIC
-  unsigned int get_update_rate() const;
-
 protected:
   std::vector<hardware_interface::LoanedCommandInterface> command_interfaces_;
   std::vector<hardware_interface::LoanedStateInterface> state_interfaces_;
-  std::shared_ptr<rclcpp::Node> node_;
-  rclcpp_lifecycle::State lifecycle_state_;
+  std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_;
   unsigned int update_rate_ = 0;
 };
 

controller_interface/src/controller_interface.cpp

@@ -20,148 +20,63 @@
 #include <vector>
 
 #include "hardware_interface/types/lifecycle_state_names.hpp"
-#include "lifecycle_msgs/msg/state.hpp"
 
 namespace controller_interface
 {
 return_type ControllerInterface::init(const std::string & controller_name)
 {
-  node_ = std::make_shared<rclcpp::Node>(
-    controller_name, rclcpp::NodeOptions()
-                       .allow_undeclared_parameters(true)
-                       .automatically_declare_parameters_from_overrides(true));
+  node_ = std::make_shared<rclcpp_lifecycle::LifecycleNode>(
+    controller_name,
+    rclcpp::NodeOptions()
+      .allow_undeclared_parameters(true)
+      .automatically_declare_parameters_from_overrides(true),
+    false);  // disable LifecycleNode service interfaces
 
-  return_type result = return_type::OK;
-  switch (on_init())
+  try
   {
-    case LifecycleNodeInterface::CallbackReturn::SUCCESS:
-      lifecycle_state_ = rclcpp_lifecycle::State(
-        lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
-        hardware_interface::lifecycle_state_names::UNCONFIGURED);
-      break;
-    case LifecycleNodeInterface::CallbackReturn::ERROR:
-    case LifecycleNodeInterface::CallbackReturn::FAILURE:
-      result = return_type::ERROR;
-      break;
+    auto_declare<int>("update_rate", 0);
   }
-  return result;
-}
-
-const rclcpp_lifecycle::State & ControllerInterface::configure()
-{
-  if (lifecycle_state_.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
+  catch (const std::exception & e)
   {
-    switch (on_configure(lifecycle_state_))
-    {
-      case LifecycleNodeInterface::CallbackReturn::SUCCESS:
-        lifecycle_state_ = rclcpp_lifecycle::State(
-          lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
-          hardware_interface::lifecycle_state_names::INACTIVE);
-        break;
-      case LifecycleNodeInterface::CallbackReturn::ERROR:
-        on_error(lifecycle_state_);
-        lifecycle_state_ = rclcpp_lifecycle::State(
-          lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,
-          hardware_interface::lifecycle_state_names::FINALIZED);
-        break;
-      case LifecycleNodeInterface::CallbackReturn::FAILURE:
-        break;
-    }
-
-    if (node_->has_parameter("update_rate"))
-    {
-      update_rate_ = node_->get_parameter("update_rate").as_int();
-    }
+    fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
+    return return_type::ERROR;
   }
-  return lifecycle_state_;
-}
 
-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,
-        hardware_interface::lifecycle_state_names::UNCONFIGURED);
-      break;
-    case LifecycleNodeInterface::CallbackReturn::ERROR:
-      on_error(lifecycle_state_);
-      lifecycle_state_ = rclcpp_lifecycle::State(
-        lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,
-        hardware_interface::lifecycle_state_names::FINALIZED);
-      break;
-    case LifecycleNodeInterface::CallbackReturn::FAILURE:
-      break;
-  }
-  return lifecycle_state_;
-}
-const rclcpp_lifecycle::State & ControllerInterface::deactivate()
-{
-  switch (on_deactivate(lifecycle_state_))
+  switch (on_init())
   {
     case LifecycleNodeInterface::CallbackReturn::SUCCESS:
-      lifecycle_state_ = rclcpp_lifecycle::State(
-        lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
-        hardware_interface::lifecycle_state_names::INACTIVE);
       break;
     case LifecycleNodeInterface::CallbackReturn::ERROR:
-      on_error(lifecycle_state_);
-      lifecycle_state_ = rclcpp_lifecycle::State(
-        lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,
-        hardware_interface::lifecycle_state_names::FINALIZED);
-      break;
     case LifecycleNodeInterface::CallbackReturn::FAILURE:
-      break;
+      return return_type::ERROR;
   }
-  return lifecycle_state_;
-}
-const rclcpp_lifecycle::State & ControllerInterface::activate()
-{
-  if (lifecycle_state_.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
-  {
-    switch (on_activate(lifecycle_state_))
-    {
-      case LifecycleNodeInterface::CallbackReturn::SUCCESS:
-        lifecycle_state_ = rclcpp_lifecycle::State(
-          lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
-          hardware_interface::lifecycle_state_names::ACTIVE);
-        break;
-      case LifecycleNodeInterface::CallbackReturn::ERROR:
-        on_error(lifecycle_state_);
-        lifecycle_state_ = rclcpp_lifecycle::State(
-          lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,
-          hardware_interface::lifecycle_state_names::FINALIZED);
-        break;
-      case LifecycleNodeInterface::CallbackReturn::FAILURE:
-        break;
-    }
-  }
-  return lifecycle_state_;
+
+  node_->register_on_configure(
+    std::bind(&ControllerInterface::on_configure, this, std::placeholders::_1));
+
+  node_->register_on_cleanup(
+    std::bind(&ControllerInterface::on_cleanup, this, std::placeholders::_1));
+
+  node_->register_on_activate(
+    std::bind(&ControllerInterface::on_activate, this, std::placeholders::_1));
+
+  node_->register_on_deactivate(
+    std::bind(&ControllerInterface::on_deactivate, this, std::placeholders::_1));
+
+  node_->register_on_shutdown(
+    std::bind(&ControllerInterface::on_shutdown, this, std::placeholders::_1));
+
+  node_->register_on_error(std::bind(&ControllerInterface::on_error, this, std::placeholders::_1));
+
+  return return_type::OK;
 }
 
-const rclcpp_lifecycle::State & ControllerInterface::shutdown()
+const rclcpp_lifecycle::State & ControllerInterface::configure()
 {
-  switch (on_shutdown(lifecycle_state_))
-  {
-    case LifecycleNodeInterface::CallbackReturn::SUCCESS:
-      lifecycle_state_ = rclcpp_lifecycle::State(
-        lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,
-        hardware_interface::lifecycle_state_names::FINALIZED);
-      break;
-    case LifecycleNodeInterface::CallbackReturn::ERROR:
-      on_error(lifecycle_state_);
-      lifecycle_state_ = rclcpp_lifecycle::State(
-        lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED,
-        hardware_interface::lifecycle_state_names::FINALIZED);
-      break;
-    case LifecycleNodeInterface::CallbackReturn::FAILURE:
-      break;
-  }
-  return lifecycle_state_;
-}
+  update_rate_ = node_->get_parameter("update_rate").as_int();
 
-const rclcpp_lifecycle::State & ControllerInterface::get_state() const { return lifecycle_state_; }
+  return node_->configure();
+}
 
 void ControllerInterface::assign_interfaces(
   std::vector<hardware_interface::LoanedCommandInterface> && command_interfaces,
@@ -177,11 +92,16 @@ void ControllerInterface::release_interfaces()
   state_interfaces_.clear();
 }
 
-std::shared_ptr<rclcpp::Node> ControllerInterface::get_node()
+const rclcpp_lifecycle::State & ControllerInterface::get_state() const
+{
+  return node_->get_current_state();
+}
+
+std::shared_ptr<rclcpp_lifecycle::LifecycleNode> ControllerInterface::get_node()
 {
   if (!node_.get())
   {
-    throw std::runtime_error("Node hasn't been initialized yet!");
+    throw std::runtime_error("Lifecycle node hasn't been initialized yet!");
   }
   return node_;
 }

controller_interface/test/test_controller_with_options.hpp

@@ -44,14 +44,11 @@ class ControllerWithOptions : public controller_interface::ControllerInterface
   {
     rclcpp::NodeOptions options;
     options.allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true);
-    node_ = std::make_shared<rclcpp::Node>(controller_name, options);
+    node_ = std::make_shared<rclcpp_lifecycle::LifecycleNode>(controller_name, options);
 
     switch (on_init())
     {
       case LifecycleNodeInterface::CallbackReturn::SUCCESS:
-        lifecycle_state_ = rclcpp_lifecycle::State(
-          lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED,
-          hardware_interface::lifecycle_state_names::UNCONFIGURED);
         break;
       case LifecycleNodeInterface::CallbackReturn::ERROR:
       case LifecycleNodeInterface::CallbackReturn::FAILURE:

controller_manager/src/controller_manager.cpp

@@ -56,7 +56,7 @@ inline bool is_controller_inactive(
   return is_controller_inactive(*controller);
 }
 
-inline bool is_controller_active(controller_interface::ControllerInterface & controller)
+inline bool is_controller_active(const controller_interface::ControllerInterface & controller)
 {
   return controller.get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE;
 }
@@ -312,8 +312,8 @@ controller_interface::return_type ControllerManager::unload_controller(
   }
 
   RCLCPP_DEBUG(get_logger(), "Cleanup controller");
-  controller.c->cleanup();
-  executor_->remove_node(controller.c->get_node());
+  controller.c->get_node()->cleanup();
+  executor_->remove_node(controller.c->get_node()->get_node_base_interface());
   to.erase(found_it);
 
   // Destroys the old controllers list when the realtime thread is finished with it.
@@ -371,7 +371,7 @@ controller_interface::return_type ControllerManager::configure_controller(
   {
     RCLCPP_DEBUG(
       get_logger(), "Controller '%s' is cleaned-up before configuring", controller_name.c_str());
-    new_state = controller->cleanup();
+    new_state = controller->get_node()->cleanup();
     if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED)
     {
       RCLCPP_ERROR(
@@ -724,7 +724,7 @@ controller_interface::ControllerInterfaceSharedPtr ControllerManager::add_contro
       controller.info.name.c_str());
     controller.c->get_node()->set_parameter(use_sim_time);
   }
-  executor_->add_node(controller.c->get_node());
+  executor_->add_node(controller.c->get_node()->get_node_base_interface());
   to.emplace_back(controller);
 
   // Destroys the old controllers list when the realtime thread is finished with it.
@@ -782,7 +782,7 @@ void ControllerManager::stop_controllers()
     auto controller = found_it->c;
     if (is_controller_active(*controller))
     {
-      const auto new_state = controller->deactivate();
+      const auto new_state = controller->get_node()->deactivate();
       controller->release_interfaces();
       if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
       {
@@ -893,7 +893,7 @@ void ControllerManager::start_controllers()
     }
     controller->assign_interfaces(std::move(command_loans), std::move(state_loans));
 
-    const auto new_state = controller->activate();
+    const auto new_state = controller->get_node()->activate();
     if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
     {
       RCLCPP_ERROR(
@@ -1355,7 +1355,7 @@ controller_interface::return_type ControllerManager::update(
     // https://github.com/ros-controls/ros2_control/issues/153
     if (is_controller_active(*loaded_controller.c))
     {
-      auto controller_update_rate = loaded_controller.c->get_update_rate();
+      const auto controller_update_rate = loaded_controller.c->get_update_rate();
 
       bool controller_go =
         controller_update_rate == 0 || ((update_loop_counter_ % controller_update_rate) == 0);

controller_manager/test/test_controller/test_controller.cpp

@@ -30,8 +30,8 @@ TestController::TestController()
 controller_interface::InterfaceConfiguration TestController::command_interface_configuration() const
 {
   if (
-    lifecycle_state_.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
-    lifecycle_state_.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
+    get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
+    get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
   {
     return cmd_iface_cfg_;
   }
@@ -45,8 +45,8 @@ controller_interface::InterfaceConfiguration TestController::command_interface_c
 controller_interface::InterfaceConfiguration TestController::state_interface_configuration() const
 {
   if (
-    lifecycle_state_.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
-    lifecycle_state_.id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
+    get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
+    get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
   {
     return state_iface_cfg_;
   }