Skip to content

Enable non-loaded hardware components after start of CM. #1049

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.

Sign up for GitHub

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

Open
wants to merge 18 commits into
base: master
Choose a base branch
from
Open

Enable non-loaded hardware components after start of CM. #1049

Show file tree
Hide file tree
Changes from 8 commits
Commits
Show all changes
18 commits
Select commit Hold shift + click to select a range
339dc73
Can compile.
destogl Jun 4, 2023
70d8c33
Prepare everything for unloaded components.
destogl Jun 5, 2023
9322373
Remove not-loaded functionality that needs more changes.
destogl Jun 5, 2023
57ae147
Revert "Remove not-loaded functionality that needs more changes."
destogl Jun 5, 2023
6f96ea7
Prepare for enabling not loaded components.
destogl Jun 5, 2023
9054324
Merge branch 'master' into enabling-not-loaded-hw-components
destogl Jun 7, 2023
7792ce3
Merge branch 'master' into enabling-not-loaded-hw-components
destogl Jan 2, 2025
61265e6
Added not-loaded parameter for HW plugins.
destogl Jan 2, 2025
e6cadf6
Update controller_manager/src/controller_manager.cpp
destogl Jan 9, 2025
74d376e
Update controller_manager/test/test_hardware_management_srvs.cpp
destogl Jan 9, 2025
8f890c7
Update controller_manager/test/test_hardware_management_srvs.cpp
destogl Jan 9, 2025
b21954e
Apply suggestions from code review
destogl Jan 9, 2025
6a3870c
Update controller_manager/src/controller_manager.cpp
destogl Jan 9, 2025
3ac185c
Merge branch 'master' into enabling-not-loaded-hw-components
destogl Jan 9, 2025
3c61830
Extend RM to support not-loading of compoentns and fix tests.
destogl Jan 10, 2025
416b4df
Fixed formatting and typo.
destogl Jan 10, 2025
4dfe63e
Merge branch 'master' into enabling-not-loaded-hw-components
destogl Jan 14, 2025
0894f5d
Merge branch 'master' into enabling-not-loaded-hw-components
bmagyar Jan 31, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
20 changes: 19 additions & 1 deletion controller_manager/src/controller_manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -419,6 +419,23 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript
{
*params_ = cm_param_listener_->get_params();
}

// not-loaded
for (const auto & component : params_->hardware_components_initial_state.not_loaded)
{
if (components_to_activate.find(component) == components_to_activate.end())
{
RCLCPP_WARN(
get_logger(), "Hardware component '%s' is unknown, therefore cannot be exclueded from loading.",
component.c_str());
}
else
{
RCLCPP_INFO(
get_logger(), "Excluding component '%s' from loading.", component.c_str());
components_to_activate.erase(component);
}
}

// unconfigured (loaded only)
set_components_to_state(
Expand All @@ -433,7 +450,8 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript
State::PRIMARY_STATE_INACTIVE, hardware_interface::lifecycle_state_names::INACTIVE));

// activate all other components
for (const auto & [component, state] : components_to_activate)
for (const auto & [component, state] :
)
{
rclcpp_lifecycle::State active_state(
State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE);
Expand Down
9 changes: 9 additions & 0 deletions controller_manager/src/controller_manager_parameters.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,15 @@ controller_manager:
}

hardware_components_initial_state:
not_loaded: {
Copy link
Member

@saikishor saikishor Jan 12, 2025 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
not_loaded: {
unloaded: {

Can we name this unloaded instead of not_loaded, in the different parts of code. What's your opinion?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I am open to it. Althrough for me personaly, not_loaded is clearer, unloaded is more for unload (verb) - but can change, no problem.

@bmagyar, @christophfroehlich what do you think about this?

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Sure. I don't have a strong opinion here. If they are fine, all good :)

type: string_array,
default_value: [],
description: "Defines which hardware components should not be loaded activated when controller manager is started. If later used, these hardware components will need to be loaded, configured and activated manually or via a hardware spawner.",
validation: {
unique<>: null,
}
}

unconfigured: {
type: string_array,
default_value: [],
Expand Down
114 changes: 114 additions & 0 deletions controller_manager/test/test_hardware_management_srvs.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -350,6 +350,61 @@ TEST_F(TestControllerManagerHWManagementSrvs, selective_activate_deactivate_comp
}));
}

class TestControllerManagerHWManagementSrvsNotLoaded : 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(
"hardware_components_initial_state.not_loaded",
std::vector<std::string>({TEST_SYSTEM_HARDWARE_NAME})));
cm_->set_parameter(rclcpp::Parameter(
"hardware_components_initial_state.inactive",
std::vector<std::string>({TEST_SENSOR_HARDWARE_NAME, TEST_ACTUATOR_HARDWARE_NAME})));

cm_ = std::make_shared<controller_manager::ControllerManager>(executor_, TEST_CM_NAME);
run_updater_ = false;

auto msg = std_msgs::msg::String();
msg.data = ros2_control_test_assets::minimal_robot_urdf;
cm_->robot_description_callback(msg);

SetUpSrvsCMExecutor();
}
};

TEST_F(TestControllerManagerHWManagementSrvsNotLoaded, test_component_not_loaded)
{
// Default status after start:
// checks if "configure_components_on_start" and "activate_components_on_start" are correctly read

// there is not system loaded therefore test should not break having only two members for checking
// results
list_hardware_components_and_check(
// actuator, sensor, system
std::vector<uint8_t>(
{LFC_STATE::PRIMARY_STATE_INACTIVE, LFC_STATE::PRIMARY_STATE_INACTIVE,
LFC_STATE::PRIMARY_STATE_UNKNOWN}),
std::vector<std::string>({INACTIVE, INACTIVE, UNKNOWN}),
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
}));
}

class TestControllerManagerHWManagementSrvsWithoutParams
: public TestControllerManagerHWManagementSrvs
{
Expand Down Expand Up @@ -392,3 +447,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
Loading