diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index e44fb5fb9b..3defaead53 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -180,6 +180,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 ) diff --git a/controller_manager/test/test_release_interfaces.cpp b/controller_manager/test/test_release_interfaces.cpp index 9caef761ab..62f9f5acb9 100644 --- a/controller_manager/test/test_release_interfaces.cpp +++ b/controller_manager/test/test_release_interfaces.cpp @@ -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::_; @@ -197,3 +198,137 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface) abstract_test_controller2.c->get_lifecycle_state().id()); } } + +class TestReleaseExclusiveInterfaces +: public ControllerManagerFixture +{ +public: + TestReleaseExclusiveInterfaces() + : ControllerManagerFixture( + 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(); + 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(); + 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 start_controllers = {controller_name1, controller_name2}; + std::vector 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::quiet_NaN()}); + test_controller2->set_external_commands_for_testing({std::numeric_limits::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 start_controllers = {controller_name1, controller_name2}; + std::vector 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 start_controllers = {controller_name1}; + std::vector 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()); + } +}