Skip to content

Fix the prepare_command_mode_switch behaviour when HW is INACTIVE #2347

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

Merged
bmagyar merged 11 commits into from
Jul 28, 2025
Merged

Fix the prepare_command_mode_switch behaviour when HW is INACTIVE #2347

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
11 commits
Select commit Hold shift + click to select a range
1a543ce
Fix the failing prepare_command_mode_switch check when HW goes to error
saikishor Jun 28, 2025
6983b1b
Remove check as it is not needed for the ERROR state
saikishor Jun 29, 2025
ed06e58
Add test case to test the deactivation and error on return values
saikishor Jun 29, 2025
a316945
remove comments
saikishor Jun 29, 2025
04144dc
Merge branch 'master' into fix/prepare_command_mode_switch
saikishor Jul 1, 2025
7f40ef6
Disable starting interfaces when the component is INACTIVE
saikishor Jul 1, 2025
947614e
Merge branch 'master' into fix/prepare_command_mode_switch
saikishor Jul 2, 2025
e63804f
Merge branch 'master' into fix/prepare_command_mode_switch
saikishor Jul 16, 2025
2a803af
Merge branch 'master' into fix/prepare_command_mode_switch
saikishor Jul 22, 2025
b530bc9
Update test_resource_manager_prepare_perform_switch.cpp
bmagyar Jul 28, 2025
67d0169
Update test_resource_manager_prepare_perform_switch.cpp
bmagyar Jul 28, 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
9 changes: 5 additions & 4 deletions controller_manager/src/controller_manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2699,8 +2699,9 @@ void ControllerManager::read(const rclcpp::Time & time, const rclcpp::Duration &
rt_buffer_.get_concatenated_string(rt_buffer_.deactivate_controllers_list).c_str());
std::vector<ControllerSpec> & rt_controller_list =
rt_controllers_wrapper_.update_and_get_used_by_rt_list();
perform_hardware_command_mode_change(
rt_controller_list, {}, rt_buffer_.deactivate_controllers_list, "read");

// As the hardware is in UNCONFIGURED state with error call, no need to prepare or perform
// command mode switch
deactivate_controllers(rt_controller_list, rt_buffer_.deactivate_controllers_list);
// TODO(destogl): do auto-start of broadcasters
}
Expand Down Expand Up @@ -2970,8 +2971,8 @@ void ControllerManager::write(const rclcpp::Time & time, const rclcpp::Duration
std::vector<ControllerSpec> & rt_controller_list =
rt_controllers_wrapper_.update_and_get_used_by_rt_list();

perform_hardware_command_mode_change(
rt_controller_list, {}, rt_buffer_.deactivate_controllers_list, "write");
// As the hardware is in UNCONFIGURED state with error call, no need to prepare or perform
// command mode switch
deactivate_controllers(rt_controller_list, rt_buffer_.deactivate_controllers_list);
// TODO(destogl): do auto-start of broadcasters
}
Expand Down
21 changes: 21 additions & 0 deletions hardware_interface/src/resource_manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -657,6 +657,7 @@ class ResourceStorage
{
auto interfaces = hardware.export_state_interfaces();
const auto interface_names = add_state_interfaces(interfaces);
hardware_info_map_[hardware.get_name()].state_interfaces = interface_names;

RCLCPP_WARN_EXPRESSION(
get_logger(), interface_names.empty(),
Expand Down Expand Up @@ -2008,6 +2009,16 @@ bool ResourceManager::prepare_command_mode_switch(
component.get_name().c_str());
continue;
}
if (
!start_interfaces_buffer.empty() &&
component.get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
{
RCLCPP_WARN(
logger, "Component '%s' is in INACTIVE state, but has start interfaces to switch: \n%s",
component.get_name().c_str(),
interfaces_to_string(start_interfaces_buffer, stop_interfaces_buffer).c_str());
return false;
}
if (
component.get_lifecycle_state().id() ==
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
Expand Down Expand Up @@ -2098,6 +2109,16 @@ bool ResourceManager::perform_command_mode_switch(
component.get_name().c_str());
continue;
}
if (
!start_interfaces_buffer.empty() &&
component.get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
{
RCLCPP_WARN(
logger, "Component '%s' is in INACTIVE state, but has start interfaces to switch: \n%s",
component.get_name().c_str(),
interfaces_to_string(start_interfaces_buffer, stop_interfaces_buffer).c_str());
return false;
}
if (
component.get_lifecycle_state().id() ==
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE ||
Expand Down
193 changes: 164 additions & 29 deletions hardware_interface_testing/test/test_resource_manager_prepare_perform_switch.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@

// Authors: Dr. Denis

#include "ros2_control_test_assets/test_hardware_interface_constants.hpp"
#include "test_resource_manager.hpp"

#include <string>
Expand Down Expand Up @@ -209,28 +210,32 @@ TEST_F(
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0);
EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0);

// When TestActuatorHardware is INACTIVE expect OK
EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator));
// When TestActuatorHardware is INACTIVE expect not OK
EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator));
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0);
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.001, 1e-7);
EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator));
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.0, 1e-7)
<< "Start interfaces with inactive should result in no change";
EXPECT_FALSE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator));
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0);
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.101, 1e-7);
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.0, 1e-7)
<< "Start interfaces with inactive should result in no change";

EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys));
EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys));
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0);
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.102, 1e-7);
EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys));
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.0, 1e-7)
<< "Start interfaces with inactive should result in no change";
EXPECT_FALSE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys));
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0);
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.202, 1e-7);
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.0, 1e-7)
<< "Start interfaces with inactive should result in no change";

EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator));
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0);
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.203, 1e-7);
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.001, 1e-7);
EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator))
<< "Inactive is OK";
<< "Inactive with empty start interfaces is OK";
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0);
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.303, 1e-7);
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.101, 1e-7);
};

// System : INACTIVE
Expand All @@ -242,54 +247,58 @@ TEST_F(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, "inactive",
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "active");

// When TestSystemCommandModes is INACTIVE expect OK
EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_system, legal_keys_system));
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 1.0);
// When TestSystemCommandModes is INACTIVE expect not OK
EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_system, legal_keys_system));
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0)
<< "Start interfaces with inactive should result in no change";
EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0)
<< "System interfaces shouldn't affect the actuator";
EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, legal_keys_system));
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 101.0);
EXPECT_FALSE(rm_->perform_command_mode_switch(legal_keys_system, legal_keys_system));
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0)
<< "Start interfaces with inactive should result in no change";
EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0)
<< "System interfaces shouldn't affect the actuator";

EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_system, empty_keys));
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 102.0);
EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_system, empty_keys));
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0)
<< "Start interfaces with inactive should result in no change";
EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0)
<< "System interfaces shouldn't affect the actuator";
EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_system, empty_keys));
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 202.0);
EXPECT_FALSE(rm_->perform_command_mode_switch(legal_keys_system, empty_keys));
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0)
<< "Start interfaces with inactive should result in no change";
EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0)
<< "System interfaces shouldn't affect the actuator";

EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_system));
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 203.0);
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 1.0);
EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0)
<< "System interfaces shouldn't affect the actuator";
EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_system));
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0);
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 101.0);
EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0)
<< "System interfaces shouldn't affect the actuator";

// When TestActuatorHardware is ACTIVE expect OK
EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator));
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0);
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 101.0);
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.001, 1e-7);
EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator));
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0);
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 101.0);
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.101, 1e-7);

EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys));
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0);
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 101.0);
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.102, 1e-7);
EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys));
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0);
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 101.0);
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.202, 1e-7);

EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator));
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0);
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 101.0);
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.203, 1e-7);
EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator));
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 303.0);
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 101.0);
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.303, 1e-7);
};

Expand Down Expand Up @@ -351,6 +360,132 @@ TEST_F(
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.303, 1e-7);
};

// System : UNCONFIGURED
// Actuator: ERROR
TEST_F(
ResourceManagerPreparePerformTest,
when_system_unconfigured_and_actuator_active_and_then_error_expect_actuator_passing)
{
preconfigure_components(
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, "unconfigured",
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, "active");

// When TestSystemCommandModes is UNCONFIGURED expect error
EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_system, legal_keys_system));
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0);
EXPECT_FALSE(rm_->perform_command_mode_switch(legal_keys_system, legal_keys_system))
<< "The system HW component is unconfigured, so the perform should fail!";
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0);

EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_system, empty_keys));
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0);
EXPECT_FALSE(rm_->perform_command_mode_switch(legal_keys_system, empty_keys))
<< "The system HW component is unconfigured, so the perform should fail!";
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0);

EXPECT_FALSE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_system));
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0);
EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_system))
<< "The system HW component is unconfigured, so the perform should fail!";
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
EXPECT_EQ(claimed_actuator_position_state_->get_optional().value(), 0.0);

// When TestActuatorHardware is ACTIVE expect OK
EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, legal_keys_actuator));
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.001, 1e-7);
EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, legal_keys_actuator));
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.101, 1e-7);

EXPECT_TRUE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys));
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.102, 1e-7);
EXPECT_TRUE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys));
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.202, 1e-7);

EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator));
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.203, 1e-7);
EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator));
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.303, 1e-7);

std::unique_ptr<hardware_interface::LoanedCommandInterface> claimed_actuator_velocity_command_ =
std::make_unique<hardware_interface::LoanedCommandInterface>(
rm_->claim_command_interface("joint3/position"));

auto status_map = rm_->get_components_status();
EXPECT_EQ(
status_map["TestSystemCommandModes"].state.id(),
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
EXPECT_EQ(
status_map["TestActuatorHardware"].state.id(),
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE);

// Now deactivate with write deactivate value
claimed_actuator_velocity_command_->set_value(test_constants::WRITE_DEACTIVATE_VALUE);
rm_->write(node_.now(), rclcpp::Duration(0, 1000000));

status_map = rm_->get_components_status();
EXPECT_EQ(
status_map["TestSystemCommandModes"].state.id(),
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
EXPECT_EQ(
status_map["TestActuatorHardware"].state.id(),
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);

// Similar to deactivate callback from write cycle
EXPECT_TRUE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator));
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.304, 1e-7);
EXPECT_TRUE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator));
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.404, 1e-7);

// Similar to the proximal activation
EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys));
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.404, 1e-7);
EXPECT_FALSE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys))
<< "Start interfaces with inactive should result in no change";
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.404, 1e-7)
<< "Start interfaces with inactive should result in no change";

// Now return ERROR with write fail value
claimed_actuator_velocity_command_->set_value(test_constants::WRITE_FAIL_VALUE);
rm_->write(node_.now(), rclcpp::Duration(0, 1000000));

status_map = rm_->get_components_status();
EXPECT_EQ(
status_map["TestSystemCommandModes"].state.id(),
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);
EXPECT_EQ(
status_map["TestActuatorHardware"].state.id(),
lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED);

EXPECT_FALSE(rm_->prepare_command_mode_switch(empty_keys, legal_keys_actuator));
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.404, 1e-7);
EXPECT_FALSE(rm_->perform_command_mode_switch(empty_keys, legal_keys_actuator));
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.404, 1e-7);

EXPECT_FALSE(rm_->prepare_command_mode_switch(legal_keys_actuator, empty_keys));
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.404, 1e-7);
EXPECT_FALSE(rm_->perform_command_mode_switch(legal_keys_actuator, empty_keys));
EXPECT_EQ(claimed_system_acceleration_state_->get_optional().value(), 0.0);
EXPECT_NEAR(claimed_actuator_position_state_->get_optional().value(), 0.404, 1e-7);
};

// System : UNCONFIGURED
// Actuator: FINALIZED
TEST_F(
Expand Down
Loading