From f2ac6d76ab2d3ef14ae20729f0173aaad2b305dd Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Fri, 23 Jun 2023 20:23:46 +0200 Subject: [PATCH] Enable setting of initial state in HW compoments (#1046) (cherry picked from commit cf4448d92d6644c04f060452e309d362ebbd118b) # 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 | 28 ++-- controller_manager/src/controller_manager.cpp | 92 +++++++++++++ .../test/test_hardware_management_srvs.cpp | 126 +++++++++++++----- .../hardware_interface/resource_manager.hpp | 22 ++- hardware_interface/src/resource_manager.cpp | 4 + 5 files changed, 225 insertions(+), 47 deletions(-) diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index f8ee2a6ec5..b9c057f571 100644 --- 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; 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 ````-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; default: empty) - Define which hardware components should be configured when controller manager is started. - The names of the components are defined as attribute of ````-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; default: empty) + Defines which hardware components will be only loaded immediately when controller manager is started. +hardware_components_initial_state.inactive (optional; list; 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 index 9f038d4e08..1eaac69aee 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/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 components_to_set = std::vector({}); + 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 configure_components_on_start = std::vector({}); 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 activate_components_on_start = std::vector({}); 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)) } } diff --git a/controller_manager/test/test_hardware_management_srvs.cpp b/controller_manager/test/test_hardware_management_srvs.cpp index b0be2eddf5..c4c9b8bb67 100644 --- 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_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({TEST_ACTUATOR_HARDWARE_NAME}))); + "hardware_components_initial_state.unconfigured", + std::vector({TEST_SYSTEM_HARDWARE_NAME}))); cm_->set_parameter(rclcpp::Parameter( - "configure_components_on_start", std::vector({TEST_SENSOR_HARDWARE_NAME}))); + "hardware_components_initial_state.inactive", + std::vector({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(); - cm_ = std::make_shared( - std::make_unique(), 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(); + cm_ = std::make_shared( + std::make_unique(), 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(); + cm_ = std::make_shared( + std::make_unique(), 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({TEST_ACTUATOR_HARDWARE_NAME}))); + cm_->set_parameter(rclcpp::Parameter( + "configure_components_on_start", std::vector({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( + {LFC_STATE::PRIMARY_STATE_ACTIVE, LFC_STATE::PRIMARY_STATE_INACTIVE, + LFC_STATE::PRIMARY_STATE_UNCONFIGURED}), + std::vector({ACTIVE, INACTIVE, UNCONFIGURED}), + std::vector>>({ + // is available + {{true, true}, {true, true, true}}, // actuator + {{}, {true}}, // sensor + {{false, false, false, false}, {false, false, false, false, false, false, false}}, // system + }), + std::vector>>({ + // 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 index 5a919289b8..061281d8b3 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/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,10 +351,11 @@ 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. @@ -363,6 +363,20 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * 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_info) const; diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 7082b7ea23..e3a3ba37d9 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/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