Skip to content

Commit

Permalink
[CM] Fix the controller deactivation on the control cycles returning …
Browse files Browse the repository at this point in the history 
…ERROR (#1756)

* Added a method to get the failed controller command interfaces from the list
* prepare and perform mode switch in the read cycle on returning error on failed controllers
* Add test_actuator_exclusive_interfaces to avoid locking of resources if they interface stay claimed

---------

Co-authored-by: Christoph Fröhlich <[email protected]>
  • Loading branch information
@saikishor @christophfroehlich
saikishor and christophfroehlich authored Feb 12, 2025
1 parent 465e5d0 commit 675a7a9
Show file tree
Hide file tree
Showing 7 changed files with 439 additions and 23 deletions.
1 change: 1 addition & 0 deletions controller_manager/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -187,6 +187,7 @@ if(BUILD_TESTING)
)
target_link_libraries(test_release_interfaces
controller_manager
test_controller
test_controller_with_interfaces
ros2_control_test_assets::ros2_control_test_assets
)
Expand Down
84 changes: 62 additions & 22 deletions controller_manager/src/controller_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -214,6 +214,44 @@ void get_active_controllers_using_command_interfaces_of_controller(
}
}

void extract_command_interfaces_for_controller(
const controller_manager::ControllerSpec & ctrl,
const hardware_interface::ResourceManager & resource_manager,
std::vector<std::string> & request_interface_list)
{
auto command_interface_config = ctrl.c->command_interface_configuration();
std::vector<std::string> command_interface_names = {};
if (command_interface_config.type == controller_interface::interface_configuration_type::ALL)
{
command_interface_names = resource_manager.available_command_interfaces();
}
if (
command_interface_config.type == controller_interface::interface_configuration_type::INDIVIDUAL)
{
command_interface_names = command_interface_config.names;
}
request_interface_list.insert(
request_interface_list.end(), command_interface_names.begin(), command_interface_names.end());
}

void get_controller_list_command_interfaces(
const std::vector<std::string> & controllers_list,
const std::vector<controller_manager::ControllerSpec> & controllers_spec,
const hardware_interface::ResourceManager & resource_manager,
std::vector<std::string> & request_interface_list)
{
for (const auto & controller_name : controllers_list)
{
auto found_it = std::find_if(
controllers_spec.begin(), controllers_spec.end(),
std::bind(controller_name_compare, std::placeholders::_1, controller_name));
if (found_it != controllers_spec.end())
{
extract_command_interfaces_for_controller(
*found_it, resource_manager, request_interface_list);
}
}
}
} // namespace

namespace controller_manager
Expand Down Expand Up @@ -1411,33 +1449,15 @@ controller_interface::return_type ControllerManager::switch_controller(
activate_request_.erase(activate_list_it);
}

const auto extract_interfaces_for_controller =
[this](const ControllerSpec ctrl, std::vector<std::string> & request_interface_list)
{
auto command_interface_config = ctrl.c->command_interface_configuration();
std::vector<std::string> command_interface_names = {};
if (command_interface_config.type == controller_interface::interface_configuration_type::ALL)
{
command_interface_names = resource_manager_->available_command_interfaces();
}
if (
command_interface_config.type ==
controller_interface::interface_configuration_type::INDIVIDUAL)
{
command_interface_names = command_interface_config.names;
}
request_interface_list.insert(
request_interface_list.end(), command_interface_names.begin(),
command_interface_names.end());
};

if (in_activate_list)
{
extract_interfaces_for_controller(controller, activate_command_interface_request_);
extract_command_interfaces_for_controller(
controller, *resource_manager_, activate_command_interface_request_);
}
if (in_deactivate_list)
{
extract_interfaces_for_controller(controller, deactivate_command_interface_request_);
extract_command_interfaces_for_controller(
controller, *resource_manager_, deactivate_command_interface_request_);
}

// cache mapping between hardware and controllers for stopping when read/write error happens
Expand Down Expand Up @@ -2668,6 +2688,26 @@ controller_interface::return_type ControllerManager::update(
RCLCPP_ERROR(
get_logger(), "Activating fallback controllers : [ %s]", controllers_string.c_str());
}
std::vector<std::string> failed_controller_interfaces, fallback_controller_interfaces;
failed_controller_interfaces.reserve(500);
get_controller_list_command_interfaces(
failed_controllers_list, rt_controller_list, *resource_manager_,
failed_controller_interfaces);
get_controller_list_command_interfaces(
cumulative_fallback_controllers, rt_controller_list, *resource_manager_,
fallback_controller_interfaces);
if (!failed_controller_interfaces.empty())
{
if (!(resource_manager_->prepare_command_mode_switch(
fallback_controller_interfaces, failed_controller_interfaces) &&
resource_manager_->perform_command_mode_switch(
fallback_controller_interfaces, failed_controller_interfaces)))
{
RCLCPP_ERROR(
get_logger(),
"Error while attempting mode switch when deactivating controllers in update cycle!");
}
}
deactivate_controllers(rt_controller_list, active_controllers_using_interfaces);
if (!cumulative_fallback_controllers.empty())
{
Expand Down
135 changes: 135 additions & 0 deletions controller_manager/test/test_release_interfaces.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@
#include "controller_manager/controller_manager.hpp"
#include "controller_manager_test_common.hpp"
#include "lifecycle_msgs/msg/state.hpp"
#include "test_controller/test_controller.hpp"
#include "test_controller_with_interfaces/test_controller_with_interfaces.hpp"

using ::testing::_;
Expand Down Expand Up @@ -197,3 +198,137 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface)
abstract_test_controller2.c->get_lifecycle_state().id());
}
}

class TestReleaseExclusiveInterfaces
: public ControllerManagerFixture<controller_manager::ControllerManager>
{
public:
TestReleaseExclusiveInterfaces()
: ControllerManagerFixture<controller_manager::ControllerManager>(
std::string(ros2_control_test_assets::urdf_head) +
std::string(ros2_control_test_assets::hardware_resources_with_exclusive_interface) +
std::string(ros2_control_test_assets::urdf_tail))
{
}
};

TEST_F(TestReleaseExclusiveInterfaces, test_exclusive_interface_deactivation_on_update_error)
{
const std::string controller_type = test_controller::TEST_CONTROLLER_CLASS_NAME;

// Load two controllers of different names
const std::string controller_name1 = "test_controller1";
const std::string controller_name2 = "test_controller2";

auto test_controller1 = std::make_shared<test_controller::TestController>();
controller_interface::InterfaceConfiguration cmd_cfg = {
controller_interface::interface_configuration_type::INDIVIDUAL, {"joint1/position"}};
controller_interface::InterfaceConfiguration state_cfg = {
controller_interface::interface_configuration_type::INDIVIDUAL,
{"joint1/position", "joint1/velocity"}};
test_controller1->set_command_interface_configuration(cmd_cfg);
test_controller1->set_state_interface_configuration(state_cfg);
cm_->add_controller(test_controller1, controller_name1, controller_type);

auto test_controller2 = std::make_shared<test_controller::TestController>();
cmd_cfg = {controller_interface::interface_configuration_type::INDIVIDUAL, {"joint2/velocity"}};
state_cfg = {
controller_interface::interface_configuration_type::INDIVIDUAL,
{"joint2/position", "joint2/velocity"}};
test_controller2->set_command_interface_configuration(cmd_cfg);
test_controller2->set_state_interface_configuration(state_cfg);
cm_->add_controller(test_controller2, controller_name2, controller_type);
ASSERT_EQ(2u, cm_->get_loaded_controllers().size());
controller_manager::ControllerSpec abstract_test_controller1 = cm_->get_loaded_controllers()[0];
controller_manager::ControllerSpec abstract_test_controller2 = cm_->get_loaded_controllers()[1];

// Configure controllers
ASSERT_EQ(controller_interface::return_type::OK, cm_->configure_controller(controller_name1));
ASSERT_EQ(controller_interface::return_type::OK, cm_->configure_controller(controller_name2));

ASSERT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
abstract_test_controller1.c->get_lifecycle_state().id());
ASSERT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
abstract_test_controller2.c->get_lifecycle_state().id());

{ // Test starting the first and second controller
RCLCPP_INFO(cm_->get_logger(), "Starting controller #1 and #2");
std::vector<std::string> start_controllers = {controller_name1, controller_name2};
std::vector<std::string> stop_controllers = {};
auto switch_future = std::async(
std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
start_controllers, stop_controllers, STRICT, true, rclcpp::Duration(0, 0));
ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100)))
<< "switch_controller should be blocking until next update cycle";
ControllerManagerRunner cm_runner(this);
EXPECT_EQ(controller_interface::return_type::OK, switch_future.get());
ASSERT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
abstract_test_controller1.c->get_lifecycle_state().id());
ASSERT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
abstract_test_controller2.c->get_lifecycle_state().id());
}

// As the external command is Nan, the controller update cycle should return an error and
// deactivate the controllers
{
test_controller1->set_external_commands_for_testing({std::numeric_limits<double>::quiet_NaN()});
test_controller2->set_external_commands_for_testing({std::numeric_limits<double>::quiet_NaN()});
ControllerManagerRunner cm_runner(this);
std::this_thread::sleep_for(std::chrono::milliseconds(100));
ASSERT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
abstract_test_controller1.c->get_lifecycle_state().id());
ASSERT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
abstract_test_controller2.c->get_lifecycle_state().id());
}

{ // Test starting both controllers but only the second one will pass as the first one has the
// same external command
test_controller2->set_external_commands_for_testing({0.0});
RCLCPP_INFO(cm_->get_logger(), "Starting controller #1 and #2 again");
std::vector<std::string> start_controllers = {controller_name1, controller_name2};
std::vector<std::string> stop_controllers = {};
auto switch_future = std::async(
std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
start_controllers, stop_controllers, STRICT, true, rclcpp::Duration(0, 0));
ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100)))
<< "switch_controller should be blocking until next update cycle";
ControllerManagerRunner cm_runner(this);
EXPECT_EQ(controller_interface::return_type::OK, switch_future.get());
std::this_thread::sleep_for(std::chrono::milliseconds(50));
ASSERT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,
abstract_test_controller1.c->get_lifecycle_state().id())
<< "Controller 1 current state is "
<< abstract_test_controller1.c->get_lifecycle_state().label();
ASSERT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
abstract_test_controller2.c->get_lifecycle_state().id());
}

{ // Test starting controller 1 and it should work as external command is valid now
test_controller1->set_external_commands_for_testing({0.0});
RCLCPP_INFO(cm_->get_logger(), "Starting controller #1");
std::vector<std::string> start_controllers = {controller_name1};
std::vector<std::string> stop_controllers = {};
auto switch_future = std::async(
std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_,
start_controllers, stop_controllers, STRICT, true, rclcpp::Duration(0, 0));
ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100)))
<< "switch_controller should be blocking until next update cycle";
ControllerManagerRunner cm_runner(this);
EXPECT_EQ(controller_interface::return_type::OK, switch_future.get());
std::this_thread::sleep_for(std::chrono::milliseconds(50));
ASSERT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
abstract_test_controller1.c->get_lifecycle_state().id());
ASSERT_EQ(
lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE,
abstract_test_controller2.c->get_lifecycle_state().id());
}
}
3 changes: 2 additions & 1 deletion hardware_interface_testing/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,8 @@ endforeach()
add_library(test_components SHARED
test/test_components/test_actuator.cpp
test/test_components/test_sensor.cpp
test/test_components/test_system.cpp)
test/test_components/test_system.cpp
test/test_components/test_actuator_exclusive_interfaces.cpp)
ament_target_dependencies(test_components hardware_interface pluginlib ros2_control_test_assets)
install(TARGETS test_components
DESTINATION lib
Expand Down
Loading

0 comments on commit 675a7a9

Please sign in to comment.