Add service-skeletons for controlling hardware lifecycle. (ros-contro…

…ls#585)

* Towards selective starting and stoping of hardware components. Cleaning and renaming.

* Move Lifecycle of hardware componnet to the bottom for better overview.

* Use the same nomenclature as for controllers. 'start' -> 'activate'; 'stop' -> 'deactivate'

* Add selective starting and stopping of hardware resources.

Add HardwareComponentInfo structure in resource manager.

Use constants for HW parameters in tests of resource_manager.

Add list hardware components in CM to get details about them and check their status.

Use clear name for 'guard' and move release cmd itfs for better readability.

RM: Add lock for accesing maps with stored interfaces.

Separate hardware components-related services after controllers-related services.

Add service for activate/deactive hardware components.

Add activation and deactivation through ResourceStorage. This helps to manage available command interfaces.

* Use lifecycle_msgs/State in ListHardwareCompoents for state representation.

* Simplify repeatable code in methods.

* Add HW shutdown structure into ResouceManager.

* Fill out service callback in CM and add parameter for auto-configure.

* Move claimed_command_itf_map to ResourceStorage from ResourceManager.

* Do not automatically configure hardware in RM.

* Lifecycle and claiming in Resource Manager is working.

* Extend controller manger to support HW lifecycle.

* Add also available and claimed status into list compoentns service output.

* Add SetHardwareComponentState service.

* Make all output in services debug-output.

* Remove specific services for hardware lifecycle management and leave only 'set_hardware_component_state' service.

* fix typo

* Fix formatting.

* Make init_resource_manager less stateful.

* Supress compile warning on unused test constants.

* Fix formatting.

* Address comments from review.

* Revert full-file chnages.

* Add empty services into CM.

* Remove some more files.

* Correct formatting.

controller_manager/include/controller_manager/controller_manager.hpp

@@ -28,11 +28,13 @@
 #include "controller_manager_msgs/srv/configure_start_controller.hpp"
 #include "controller_manager_msgs/srv/list_controller_types.hpp"
 #include "controller_manager_msgs/srv/list_controllers.hpp"
+#include "controller_manager_msgs/srv/list_hardware_components.hpp"
 #include "controller_manager_msgs/srv/list_hardware_interfaces.hpp"
 #include "controller_manager_msgs/srv/load_configure_controller.hpp"
 #include "controller_manager_msgs/srv/load_controller.hpp"
 #include "controller_manager_msgs/srv/load_start_controller.hpp"
 #include "controller_manager_msgs/srv/reload_controller_libraries.hpp"
+#include "controller_manager_msgs/srv/set_hardware_component_state.hpp"
 #include "controller_manager_msgs/srv/switch_controller.hpp"
 #include "controller_manager_msgs/srv/unload_controller.hpp"
 
@@ -67,6 +69,9 @@ class ControllerManager : public rclcpp::Node
   CONTROLLER_MANAGER_PUBLIC
   virtual ~ControllerManager() = default;
 
+  CONTROLLER_MANAGER_PUBLIC
+  void init_resource_manager(const std::string & robot_description);
+
   CONTROLLER_MANAGER_PUBLIC
   controller_interface::ControllerInterfaceSharedPtr load_controller(
     const std::string & controller_name, const std::string & controller_type);
@@ -173,11 +178,6 @@ class ControllerManager : public rclcpp::Node
     const std::shared_ptr<controller_manager_msgs::srv::ListControllers::Request> request,
     std::shared_ptr<controller_manager_msgs::srv::ListControllers::Response> response);
 
-  CONTROLLER_MANAGER_PUBLIC
-  void list_controller_types_srv_cb(
-    const std::shared_ptr<controller_manager_msgs::srv::ListControllerTypes::Request> request,
-    std::shared_ptr<controller_manager_msgs::srv::ListControllerTypes::Response> response);
-
   CONTROLLER_MANAGER_PUBLIC
   void list_hardware_interfaces_srv_cb(
     const std::shared_ptr<controller_manager_msgs::srv::ListHardwareInterfaces::Request> request,
@@ -223,6 +223,21 @@ class ControllerManager : public rclcpp::Node
     const std::shared_ptr<controller_manager_msgs::srv::UnloadController::Request> request,
     std::shared_ptr<controller_manager_msgs::srv::UnloadController::Response> response);
 
+  CONTROLLER_MANAGER_PUBLIC
+  void list_controller_types_srv_cb(
+    const std::shared_ptr<controller_manager_msgs::srv::ListControllerTypes::Request> request,
+    std::shared_ptr<controller_manager_msgs::srv::ListControllerTypes::Response> response);
+
+  CONTROLLER_MANAGER_PUBLIC
+  void list_hardware_components_srv_cb(
+    const std::shared_ptr<controller_manager_msgs::srv::ListHardwareComponents::Request> request,
+    std::shared_ptr<controller_manager_msgs::srv::ListHardwareComponents::Response> response);
+
+  CONTROLLER_MANAGER_PUBLIC
+  void set_hardware_component_state_srv_cb(
+    const std::shared_ptr<controller_manager_msgs::srv::SetHardwareComponentState::Request> request,
+    std::shared_ptr<controller_manager_msgs::srv::SetHardwareComponentState::Response> response);
+
   // Per controller update rate support
   unsigned int update_loop_counter_ = 0;
   unsigned int update_rate_ = 100;
@@ -323,8 +338,6 @@ class ControllerManager : public rclcpp::Node
     list_controllers_service_;
   rclcpp::Service<controller_manager_msgs::srv::ListControllerTypes>::SharedPtr
     list_controller_types_service_;
-  rclcpp::Service<controller_manager_msgs::srv::ListHardwareInterfaces>::SharedPtr
-    list_hardware_interfaces_service_;
   rclcpp::Service<controller_manager_msgs::srv::LoadController>::SharedPtr load_controller_service_;
   rclcpp::Service<controller_manager_msgs::srv::ConfigureController>::SharedPtr
     configure_controller_service_;
@@ -341,6 +354,13 @@ class ControllerManager : public rclcpp::Node
   rclcpp::Service<controller_manager_msgs::srv::UnloadController>::SharedPtr
     unload_controller_service_;
 
+  rclcpp::Service<controller_manager_msgs::srv::ListHardwareComponents>::SharedPtr
+    list_hardware_components_service_;
+  rclcpp::Service<controller_manager_msgs::srv::ListHardwareInterfaces>::SharedPtr
+    list_hardware_interfaces_service_;
+  rclcpp::Service<controller_manager_msgs::srv::SetHardwareComponentState>::SharedPtr
+    set_hardware_component_state_service_;
+
   std::vector<std::string> start_request_, stop_request_;
   std::vector<std::string> start_command_interface_request_, stop_command_interface_request_;
 

controller_manager/src/controller_manager.cpp

@@ -112,11 +112,6 @@ void ControllerManager::init_services()
       "~/list_controller_types",
       std::bind(&ControllerManager::list_controller_types_srv_cb, this, _1, _2),
       rmw_qos_profile_services_default, best_effort_callback_group_);
-  list_hardware_interfaces_service_ =
-    create_service<controller_manager_msgs::srv::ListHardwareInterfaces>(
-      "~/list_hardware_interfaces",
-      std::bind(&ControllerManager::list_hardware_interfaces_srv_cb, this, _1, _2),
-      rmw_qos_profile_services_default, best_effort_callback_group_);
   load_controller_service_ = create_service<controller_manager_msgs::srv::LoadController>(
     "~/load_controller", std::bind(&ControllerManager::load_controller_service_cb, this, _1, _2),
     rmw_qos_profile_services_default, best_effort_callback_group_);
@@ -152,6 +147,21 @@ void ControllerManager::init_services()
     "~/unload_controller",
     std::bind(&ControllerManager::unload_controller_service_cb, this, _1, _2),
     rmw_qos_profile_services_default, best_effort_callback_group_);
+  list_hardware_components_service_ =
+    create_service<controller_manager_msgs::srv::ListHardwareComponents>(
+      "~/list_hardware_components",
+      std::bind(&ControllerManager::list_hardware_components_srv_cb, this, _1, _2),
+      rmw_qos_profile_services_default, best_effort_callback_group_);
+  list_hardware_interfaces_service_ =
+    create_service<controller_manager_msgs::srv::ListHardwareInterfaces>(
+      "~/list_hardware_interfaces",
+      std::bind(&ControllerManager::list_hardware_interfaces_srv_cb, this, _1, _2),
+      rmw_qos_profile_services_default, best_effort_callback_group_);
+  set_hardware_component_state_service_ =
+    create_service<controller_manager_msgs::srv::SetHardwareComponentState>(
+      "~/set_hardware_component_state",
+      std::bind(&ControllerManager::set_hardware_component_state_srv_cb, this, _1, _2),
+      rmw_qos_profile_services_default, best_effort_callback_group_);
 }
 
 controller_interface::ControllerInterfaceSharedPtr ControllerManager::load_controller(
@@ -909,28 +919,6 @@ void ControllerManager::list_controller_types_srv_cb(
   RCLCPP_DEBUG(get_logger(), "list types service finished");
 }
 
-void ControllerManager::list_hardware_interfaces_srv_cb(
-  const std::shared_ptr<controller_manager_msgs::srv::ListHardwareInterfaces::Request>,
-  std::shared_ptr<controller_manager_msgs::srv::ListHardwareInterfaces::Response> response)
-{
-  auto state_interface_names = resource_manager_->state_interface_keys();
-  for (const auto & state_interface_name : state_interface_names)
-  {
-    controller_manager_msgs::msg::HardwareInterface hwi;
-    hwi.name = state_interface_name;
-    hwi.is_claimed = false;
-    response->state_interfaces.push_back(hwi);
-  }
-  auto command_interface_names = resource_manager_->command_interface_keys();
-  for (const auto & command_interface_name : command_interface_names)
-  {
-    controller_manager_msgs::msg::HardwareInterface hwi;
-    hwi.name = command_interface_name;
-    hwi.is_claimed = resource_manager_->command_interface_is_claimed(command_interface_name);
-    response->command_interfaces.push_back(hwi);
-  }
-}
-
 void ControllerManager::load_controller_service_cb(
   const std::shared_ptr<controller_manager_msgs::srv::LoadController::Request> request,
   std::shared_ptr<controller_manager_msgs::srv::LoadController::Response> response)
@@ -1159,6 +1147,58 @@ void ControllerManager::unload_controller_service_cb(
     get_logger(), "unloading service finished for controller '%s' ", request->name.c_str());
 }
 
+void ControllerManager::list_hardware_components_srv_cb(
+  const std::shared_ptr<controller_manager_msgs::srv::ListHardwareComponents::Request>,
+  std::shared_ptr<controller_manager_msgs::srv::ListHardwareComponents::Response> response)
+{
+  RCLCPP_DEBUG(get_logger(), "list hardware components service called");
+  std::lock_guard<std::mutex> guard(services_lock_);
+  RCLCPP_DEBUG(get_logger(), "list hardware components service locked");
+
+  RCLCPP_DEBUG(get_logger(), "list hardware components service finished");
+}
+
+void ControllerManager::list_hardware_interfaces_srv_cb(
+  const std::shared_ptr<controller_manager_msgs::srv::ListHardwareInterfaces::Request>,
+  std::shared_ptr<controller_manager_msgs::srv::ListHardwareInterfaces::Response> response)
+{
+  RCLCPP_DEBUG(get_logger(), "list hardware interfaces service called");
+  std::lock_guard<std::mutex> guard(services_lock_);
+  RCLCPP_DEBUG(get_logger(), "list hardware interfaces service locked");
+
+  auto state_interface_names = resource_manager_->state_interface_keys();
+  for (const auto & state_interface_name : state_interface_names)
+  {
+    controller_manager_msgs::msg::HardwareInterface hwi;
+    hwi.name = state_interface_name;
+    hwi.is_claimed = false;
+    response->state_interfaces.push_back(hwi);
+  }
+  auto command_interface_names = resource_manager_->command_interface_keys();
+  for (const auto & command_interface_name : command_interface_names)
+  {
+    controller_manager_msgs::msg::HardwareInterface hwi;
+    hwi.name = command_interface_name;
+    hwi.is_claimed = resource_manager_->command_interface_is_claimed(command_interface_name);
+    response->command_interfaces.push_back(hwi);
+  }
+
+  RCLCPP_DEBUG(get_logger(), "list hardware interfaces service finished");
+}
+
+void ControllerManager::set_hardware_component_state_srv_cb(
+  const std::shared_ptr<controller_manager_msgs::srv::SetHardwareComponentState::Request> request,
+  std::shared_ptr<controller_manager_msgs::srv::SetHardwareComponentState::Response> response)
+{
+  RCLCPP_DEBUG(get_logger(), "set hardware component state service called");
+  std::lock_guard<std::mutex> guard(services_lock_);
+  RCLCPP_DEBUG(get_logger(), "set hardware component state service locked");
+
+  RCLCPP_DEBUG(get_logger(), "set hardware component state '%s'", request->name.c_str());
+
+  RCLCPP_DEBUG(get_logger(), "set hardware component state service finished");
+}
+
 std::vector<std::string> ControllerManager::get_controller_names()
 {
   std::vector<std::string> names;

controller_manager_msgs/CMakeLists.txt

@@ -7,22 +7,26 @@ endif()
 
 find_package(ament_cmake REQUIRED)
 find_package(builtin_interfaces REQUIRED)
+find_package(lifecycle_msgs REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 
 set(msg_files
   msg/ControllerState.msg
+  msg/HardwareComponentState.msg
   msg/HardwareInterface.msg
 )
 set(srv_files
   srv/ConfigureController.srv
   srv/ConfigureStartController.srv
   srv/ListControllers.srv
   srv/ListControllerTypes.srv
+  srv/ListHardwareComponents.srv
   srv/ListHardwareInterfaces.srv
   srv/LoadConfigureController.srv
   srv/LoadController.srv
   srv/LoadStartController.srv
   srv/ReloadControllerLibraries.srv
+  srv/SetHardwareComponentState.srv
   srv/SwitchController.srv
   srv/UnloadController.srv
 )
@@ -35,7 +39,7 @@ endif()
 rosidl_generate_interfaces(${PROJECT_NAME}
   ${msg_files}
   ${srv_files}
-  DEPENDENCIES builtin_interfaces
+  DEPENDENCIES builtin_interfaces lifecycle_msgs
   ADD_LINTER_TESTS
 )
 ament_export_dependencies(rosidl_default_runtime)

controller_manager_msgs/msg/HardwareComponentState.msg

@@ -0,0 +1,6 @@
+string name
+string type
+string class_type
+lifecycle_msgs/State state
+HardwareInterface[] command_interfaces
+HardwareInterface[] state_interfaces

controller_manager_msgs/msg/HardwareInterface.msg

@@ -1,2 +1,3 @@
 string name
+bool is_available
 bool is_claimed

controller_manager_msgs/package.xml

@@ -13,8 +13,10 @@
   <buildtool_depend>rosidl_default_generators</buildtool_depend>
 
   <build_depend>builtin_interfaces</build_depend>
+  <build_depend>lifecycle_msgs</build_depend>
 
   <exec_depend>builtin_interfaces</exec_depend>
+  <exec_depend>lifecycle_msgs</exec_depend>
   <exec_depend>rosidl_default_runtime</exec_depend>
 
   <test_depend>ament_lint_auto</test_depend>

controller_manager_msgs/srv/ListHardwareComponents.srv

@@ -0,0 +1,6 @@
+# The ListHardwareComponents service returns a list of hardware HardwareComponentsState.
+# This will convey name, component_type, state and type of the components
+# that are loaded inside the resource_manager.
+
+---
+HardwareComponentState[] component

controller_manager_msgs/srv/SetHardwareComponentState.srv

@@ -0,0 +1,14 @@
+# The SetHardwareComponentState service allows to control life-cycle of a single hardware component.
+# Supported states are defined in the design document of LifecycleNodes available at:
+# https://design.ros2.org/articles/node_lifecycle.html
+# To control life-cycle of a hardware component, specify its "name" and "target_state".
+# Target state may be defined by "id" using a constant from `lifecycle_msgs/msg/State` or a label
+# using definitions from `hardware_interface/types/lifecycle_state_names.hpp` file.
+# The return value "ok" indicates if the component has successfully changed its state to "target_state".
+# The return value "state" returns current state of the hardware component.
+
+string name
+lifecycle_msgs/State target_state
+---
+bool ok
+lifecycle_msgs/State state