Enable setting of initial state in HW compoments (#1046)

(cherry picked from commit cf4448d)

# Conflicts:
#	controller_manager/src/controller_manager.cpp
#	hardware_interface/include/hardware_interface/resource_manager.hpp
#	hardware_interface/src/resource_manager.cpp

controller_manager/doc/userdoc.rst

@@ -39,21 +39,27 @@ The limits will be applied after you log out and in again.
 Parameters
 -----------
 
-activate_components_on_start (optional; list<string>; default: empty)
-  Define which hardware components should be activated when controller manager is started.
+hardware_components_initial_state
+  Map of parameters for controlled lifecycle management of hardware components.
   The names of the components are defined as attribute of ``<ros2_control>``-tag in ``robot_description``.
-  All other components will stay ``UNCONFIGURED``.
-  If this and ``configure_components_on_start`` are empty, all available components will be activated.
-  If this or ``configure_components_on_start`` are not empty, any component not in either list will be in unconfigured state.
+  Hardware components found in ``robot_description``, but without explicit state definition will be immediately activated.
+  Detailed explanation of each parameter is given below.
+  The full structure of the map is given in the following example:
 
+.. code-block:: yaml
 
-configure_components_on_start (optional; list<string>; default: empty)
-  Define which hardware components should be configured when controller manager is started.
-  The names of the components are defined as attribute of ``<ros2_control>``-tag in ``robot_description``.
-  All other components will stay ``UNCONFIGURED``.
-  If this and ``activate_components_on_start`` are empty, all available components will be activated.
-  If this or ``activate_components_on_start`` are not empty, any component not in either list will be in unconfigured state.
+    hardware_components_initial_state:
+      unconfigured:
+        - "arm1"
+        - "arm2"
+      inactive:
+        - "base3"
+
+hardware_components_initial_state.unconfigured (optional; list<string>; default: empty)
+  Defines which hardware components will be only loaded immediately when controller manager is started.
 
+hardware_components_initial_state.inactive (optional; list<string>; default: empty)
+  Defines which hardware components will be configured immediately when controller manager is started.
 
 robot_description (mandatory; string)
   String with the URDF string as robot description.

controller_manager/src/controller_manager.cpp

@@ -191,21 +191,87 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript
   // TODO(destogl): manage this when there is an error - CM should not die because URDF is wrong...
   resource_manager_->load_urdf(robot_description);
 
+  // Get all components and if they are not defined in parameters activate them automatically
+  auto components_to_activate = resource_manager_->get_components_status();
+
   using lifecycle_msgs::msg::State;
 
+  auto set_components_to_state =
+    [&](const std::string & parameter_name, rclcpp_lifecycle::State state)
+  {
+    std::vector<std::string> components_to_set = std::vector<std::string>({});
+    if (get_parameter(parameter_name, components_to_set))
+    {
+      for (const auto & component : components_to_set)
+      {
+        if (component.empty())
+        {
+          continue;
+        }
+        if (components_to_activate.find(component) == components_to_activate.end())
+        {
+          RCLCPP_WARN(
+            get_logger(), "Hardware component '%s' is unknown, therefore not set in '%s' state.",
+            component.c_str(), state.label().c_str());
+        }
+        else
+        {
+          RCLCPP_INFO(
+            get_logger(), "Setting component '%s' to '%s' state.", component.c_str(),
+            state.label().c_str());
+          resource_manager_->set_component_state(component, state);
+          components_to_activate.erase(component);
+        }
+      }
+    }
+  };
+
+  // unconfigured (loaded only)
+  set_components_to_state(
+    "hardware_components_initial_state.unconfigured",
+    rclcpp_lifecycle::State(
+      State::PRIMARY_STATE_UNCONFIGURED, hardware_interface::lifecycle_state_names::UNCONFIGURED));
+
+  // inactive (configured)
+  // BEGIN: Keep old functionality on for backwards compatibility (Remove at the end of 2023)
   std::vector<std::string> configure_components_on_start = std::vector<std::string>({});
   get_parameter("configure_components_on_start", configure_components_on_start);
+<<<<<<< HEAD
   rclcpp_lifecycle::State inactive_state(
     State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE);
   for (const auto & component : configure_components_on_start)
   {
     resource_manager_->set_component_state(component, inactive_state);
+=======
+  if (!configure_components_on_start.empty())
+  {
+    RCLCPP_WARN(
+      get_logger(),
+      "Parameter 'configure_components_on_start' is deprecated. "
+      "Use 'hardware_interface_state_after_start.inactive' instead, to set component's initial "
+      "state to 'inactive'. Don't use this parameters in combination with the new "
+      "'hardware_interface_state_after_start' parameter structure.");
+    set_components_to_state(
+      "configure_components_on_start",
+      rclcpp_lifecycle::State(
+        State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE));
+  }
+  // END: Keep old functionality on humble backwards compatibility (Remove at the end of 2023)
+  else
+  {
+    set_components_to_state(
+      "hardware_components_initial_state.inactive",
+      rclcpp_lifecycle::State(
+        State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE));
+>>>>>>> cf4448d (Enable setting of initial state in HW compoments (#1046))
   }
 
+  // BEGIN: Keep old functionality on for backwards compatibility (Remove at the end of 2023)
   std::vector<std::string> activate_components_on_start = std::vector<std::string>({});
   get_parameter("activate_components_on_start", activate_components_on_start);
   rclcpp_lifecycle::State active_state(
     State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE);
+<<<<<<< HEAD
   for (const auto & component : activate_components_on_start)
   {
     resource_manager_->set_component_state(component, active_state);
@@ -216,6 +282,32 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript
   if (configure_components_on_start.empty() && activate_components_on_start.empty())
   {
     resource_manager_->activate_all_components();
+=======
+  if (!activate_components_on_start.empty())
+  {
+    RCLCPP_WARN(
+      get_logger(),
+      "Parameter 'activate_components_on_start' is deprecated. "
+      "Components are activated per default. Don't use this parameters in combination with the new "
+      "'hardware_interface_state_after_start' parameter structure.");
+    rclcpp_lifecycle::State active_state(
+      State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE);
+    for (const auto & component : activate_components_on_start)
+    {
+      resource_manager_->set_component_state(component, active_state);
+    }
+  }
+  // END: Keep old functionality on humble for backwards compatibility (Remove at the end of 2023)
+  else
+  {
+    // activate all other components
+    for (const auto & [component, state] : components_to_activate)
+    {
+      rclcpp_lifecycle::State active_state(
+        State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE);
+      resource_manager_->set_component_state(component, active_state);
+    }
+>>>>>>> cf4448d (Enable setting of initial state in HW compoments (#1046))
   }
 }
 

controller_manager/test/test_hardware_management_srvs.cpp

@@ -35,6 +35,7 @@ using hardware_interface::lifecycle_state_names::ACTIVE;
 using hardware_interface::lifecycle_state_names::FINALIZED;
 using hardware_interface::lifecycle_state_names::INACTIVE;
 using hardware_interface::lifecycle_state_names::UNCONFIGURED;
+using hardware_interface::lifecycle_state_names::UNKNOWN;
 
 using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_CLASS_TYPE;
 using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES;
@@ -69,9 +70,11 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs
     cm_->set_parameter(
       rclcpp::Parameter("robot_description", ros2_control_test_assets::minimal_robot_urdf));
     cm_->set_parameter(rclcpp::Parameter(
-      "activate_components_on_start", std::vector<std::string>({TEST_ACTUATOR_HARDWARE_NAME})));
+      "hardware_components_initial_state.unconfigured",
+      std::vector<std::string>({TEST_SYSTEM_HARDWARE_NAME})));
     cm_->set_parameter(rclcpp::Parameter(
-      "configure_components_on_start", std::vector<std::string>({TEST_SENSOR_HARDWARE_NAME})));
+      "hardware_components_initial_state.inactive",
+      std::vector<std::string>({TEST_SENSOR_HARDWARE_NAME})));
 
     std::string robot_description = "";
     cm_->get_parameter("robot_description", robot_description);
@@ -199,36 +202,6 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs
   }
 };
 
-class TestControllerManagerHWManagementSrvsWithoutParams
-: public TestControllerManagerHWManagementSrvs
-{
-public:
-  void SetUp() override
-  {
-    executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
-    cm_ = std::make_shared<controller_manager::ControllerManager>(
-      std::make_unique<hardware_interface::ResourceManager>(), executor_, TEST_CM_NAME);
-    run_updater_ = false;
-
-    // TODO(destogl): separate this to init_tests method where parameter can be set for each test
-    // separately
-    cm_->set_parameter(
-      rclcpp::Parameter("robot_description", ros2_control_test_assets::minimal_robot_urdf));
-
-    std::string robot_description = "";
-    cm_->get_parameter("robot_description", robot_description);
-    if (robot_description.empty())
-    {
-      throw std::runtime_error(
-        "Unable to initialize resource manager, no robot description found.");
-    }
-
-    cm_->init_resource_manager(robot_description);
-
-    SetUpSrvsCMExecutor();
-  }
-};
-
 TEST_F(TestControllerManagerHWManagementSrvs, list_hardware_components)
 {
   // Default status after start:
@@ -386,6 +359,36 @@ TEST_F(TestControllerManagerHWManagementSrvs, selective_activate_deactivate_comp
     }));
 }
 
+class TestControllerManagerHWManagementSrvsWithoutParams
+: public TestControllerManagerHWManagementSrvs
+{
+public:
+  void SetUp() override
+  {
+    executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+    cm_ = std::make_shared<controller_manager::ControllerManager>(
+      std::make_unique<hardware_interface::ResourceManager>(), executor_, TEST_CM_NAME);
+    run_updater_ = false;
+
+    // TODO(destogl): separate this to init_tests method where parameter can be set for each test
+    // separately
+    cm_->set_parameter(
+      rclcpp::Parameter("robot_description", ros2_control_test_assets::minimal_robot_urdf));
+
+    std::string robot_description = "";
+    cm_->get_parameter("robot_description", robot_description);
+    if (robot_description.empty())
+    {
+      throw std::runtime_error(
+        "Unable to initialize resource manager, no robot description found.");
+    }
+
+    cm_->init_resource_manager(robot_description);
+
+    SetUpSrvsCMExecutor();
+  }
+};
+
 TEST_F(TestControllerManagerHWManagementSrvsWithoutParams, test_default_activation_of_all_hardware)
 {
   // "configure_components_on_start" and "activate_components_on_start" are not set (empty)
@@ -409,3 +412,62 @@ TEST_F(TestControllerManagerHWManagementSrvsWithoutParams, test_default_activati
       {{false, false, false, false}, {false, false, false, false, false, false, false}},  // system
     }));
 }
+
+// BEGIN: Remove at the end of 2023
+class TestControllerManagerHWManagementSrvsOldParameters
+: public TestControllerManagerHWManagementSrvs
+{
+public:
+  void SetUp() override
+  {
+    executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+    cm_ = std::make_shared<controller_manager::ControllerManager>(
+      std::make_unique<hardware_interface::ResourceManager>(), executor_, TEST_CM_NAME);
+    run_updater_ = false;
+
+    cm_->set_parameter(
+      rclcpp::Parameter("robot_description", ros2_control_test_assets::minimal_robot_urdf));
+    cm_->set_parameter(rclcpp::Parameter(
+      "activate_components_on_start", std::vector<std::string>({TEST_ACTUATOR_HARDWARE_NAME})));
+    cm_->set_parameter(rclcpp::Parameter(
+      "configure_components_on_start", std::vector<std::string>({TEST_SENSOR_HARDWARE_NAME})));
+
+    std::string robot_description = "";
+    cm_->get_parameter("robot_description", robot_description);
+    if (robot_description.empty())
+    {
+      throw std::runtime_error(
+        "Unable to initialize resource manager, no robot description found.");
+    }
+
+    cm_->init_resource_manager(robot_description);
+
+    SetUpSrvsCMExecutor();
+  }
+};
+
+TEST_F(TestControllerManagerHWManagementSrvsOldParameters, list_hardware_components)
+{
+  // Default status after start:
+  // checks if "configure_components_on_start" and "activate_components_on_start" are correctly read
+
+  list_hardware_components_and_check(
+    // actuator, sensor, system
+    std::vector<uint8_t>(
+      {LFC_STATE::PRIMARY_STATE_ACTIVE, LFC_STATE::PRIMARY_STATE_INACTIVE,
+       LFC_STATE::PRIMARY_STATE_UNCONFIGURED}),
+    std::vector<std::string>({ACTIVE, INACTIVE, UNCONFIGURED}),
+    std::vector<std::vector<std::vector<bool>>>({
+      // is available
+      {{true, true}, {true, true, true}},  // actuator
+      {{}, {true}},                        // sensor
+      {{false, false, false, false}, {false, false, false, false, false, false, false}},  // system
+    }),
+    std::vector<std::vector<std::vector<bool>>>({
+      // is claimed
+      {{false, false}, {false, false, false}},  // actuator
+      {{}, {false}},                            // sensor
+      {{false, false, false, false}, {false, false, false, false, false, false, false}},  // system
+    }));
+}
+// END: Remove at the end of 2023

hardware_interface/include/hardware_interface/resource_manager.hpp

@@ -55,8 +55,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
    * \param[in] validate_interfaces boolean argument indicating whether the exported
    * interfaces ought to be validated. Defaults to true.
    * \param[in] activate_all boolean argument indicating if all resources should be immediately
-   * activated. Currently used only in tests. In typical applications use parameters
-   * "autostart_components" and "autoconfigure_components" instead.
+   * activated. Currently used only in tests.
    */
   explicit ResourceManager(
     const std::string & urdf, bool validate_interfaces = true, bool activate_all = false);
@@ -343,7 +342,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
    * Reads from all active hardware components.
    *
    * Part of the real-time critical update loop.
-   * It is realtime-safe if used hadware interfaces are implemented adequately.
+   * It is realtime-safe if used hardware interfaces are implemented adequately.
    */
   void read(const rclcpp::Time & time, const rclcpp::Duration & period);
 
@@ -352,17 +351,32 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
    * Writes to all active hardware components.
    *
    * Part of the real-time critical update loop.
-   * It is realtime-safe if used hadware interfaces are implemented adequately.
+   * It is realtime-safe if used hardware interfaces are implemented adequately.
    */
   void write(const rclcpp::Time & time, const rclcpp::Duration & period);
 
+<<<<<<< HEAD
   /// Activates all available hardware components in the system.
   /**
    * All available hardware components int the ros2_control framework are activated.
    * This is used to preserve default behavior from previous versions where all hardware components
    * are activated per default.
    */
   void activate_all_components();
+=======
+  /// Checks whether a command interface is registered under the given key.
+  /**
+   * \param[in] key string identifying the interface to check.
+   * \return true if interface exist, false otherwise.
+   */
+  bool command_interface_exists(const std::string & key) const;
+
+  /// Checks whether a state interface is registered under the given key.
+  /**
+   * \return true if interface exist, false otherwise.
+   */
+  bool state_interface_exists(const std::string & key) const;
+>>>>>>> cf4448d (Enable setting of initial state in HW compoments (#1046))
 
 private:
   void validate_storage(const std::vector<hardware_interface::HardwareInfo> & hardware_info) const;

hardware_interface/src/resource_manager.cpp

@@ -1101,6 +1101,7 @@ void ResourceManager::validate_storage(
   }
 }
 
+<<<<<<< HEAD
 // Temporary method to keep old interface and reduce framework changes in PRs
 void ResourceManager::activate_all_components()
 {
@@ -1121,5 +1122,8 @@ void ResourceManager::activate_all_components()
     set_component_state(component.get_name(), active_state);
   }
 }
+=======
+// END: private methods
+>>>>>>> cf4448d (Enable setting of initial state in HW compoments (#1046))
 
 }  // namespace hardware_interface