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

destogl · web-flow · commit cf4448d92d66 · 2023-06-23T19:23:46.000+01:00
diff --git a/controller_manager/doc/userdoc.rst b/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.
diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp
@@ -245,46 +245,102 @@ 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>({});
-  if (get_parameter("configure_components_on_start", configure_components_on_start))
+  get_parameter("configure_components_on_start", configure_components_on_start);
+  if (!configure_components_on_start.empty())
   {
     RCLCPP_WARN(
       get_logger(),
-      "[Deprecated]: Usage of parameter \"configure_components_on_start\" is deprecated. Use "
-      "hardware_spawner instead.");
-    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);
-    }
+      "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));
   }
 
+  // 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>({});
-  if (get_parameter("activate_components_on_start", activate_components_on_start))
+  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);
+  if (!activate_components_on_start.empty())
   {
-    RCLCPP_WARN_STREAM(
+    RCLCPP_WARN(
       get_logger(),
-      "[Deprecated]: Usage of parameter \"activate_components_on_start\" is deprecated. Use "
-      "hardware_spawner instead.");
+      "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);
     }
   }
-  // if both parameter are empty or non-existing preserve behavior where all components are
-  // activated per default
-  if (configure_components_on_start.empty() && activate_components_on_start.empty())
+  // END: Keep old functionality on humble for backwards compatibility (Remove at the end of 2023)
+  else
   {
-    RCLCPP_WARN_STREAM(
-      get_logger(),
-      "[Deprecated]: Automatic activation of all hardware components will not be supported in the "
-      "future anymore. Use hardware_spawner instead.");
-    resource_manager_->activate_all_components();
+    // 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);
+    }
   }
 }
 
diff --git a/controller_manager/test/test_hardware_management_srvs.cpp b/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_COMMAND_INTERFACES;
 using ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_NAME;
@@ -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
diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp
@@ -66,8 +66,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,
@@ -374,7 +373,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.
    */
   HardwareReadWriteStatus read(const rclcpp::Time & time, const rclcpp::Duration & period);
 
@@ -383,18 +382,10 @@ 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.
    */
   HardwareReadWriteStatus write(const rclcpp::Time & time, const rclcpp::Duration & period);
 
-  /// 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.
diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp
@@ -1356,25 +1356,4 @@ void ResourceManager::validate_storage(
 
 // END: private methods
 
-// Temporary method to keep old interface and reduce framework changes in the PRs
-void ResourceManager::activate_all_components()
-{
-  using lifecycle_msgs::msg::State;
-  rclcpp_lifecycle::State active_state(
-    State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE);
-
-  for (auto & component : resource_storage_->actuators_)
-  {
-    set_component_state(component.get_name(), active_state);
-  }
-  for (auto & component : resource_storage_->sensors_)
-  {
-    set_component_state(component.get_name(), active_state);
-  }
-  for (auto & component : resource_storage_->systems_)
-  {
-    set_component_state(component.get_name(), active_state);
-  }
-}
-
 }  // namespace hardware_interface
