Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Enable setting of initial state in HW compoments #1046

Merged
merged 9 commits into from
Jun 23, 2023
28 changes: 17 additions & 11 deletions controller_manager/doc/userdoc.rst
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down
98 changes: 77 additions & 21 deletions controller_manager/src/controller_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
}
}

Expand Down
126 changes: 94 additions & 32 deletions controller_manager/test/test_hardware_management_srvs.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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",
bmagyar marked this conversation as resolved.
Show resolved Hide resolved
std::vector<std::string>({TEST_SYSTEM_HARDWARE_NAME})));
bmagyar marked this conversation as resolved.
Show resolved Hide resolved
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);
Expand Down Expand Up @@ -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:
Expand Down Expand Up @@ -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)
Expand All @@ -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
bmagyar marked this conversation as resolved.
Show resolved Hide resolved
15 changes: 3 additions & 12 deletions hardware_interface/include/hardware_interface/resource_manager.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down Expand Up @@ -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);

Expand All @@ -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.
Expand Down
21 changes: 0 additions & 21 deletions hardware_interface/src/resource_manager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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