From 4c58cda427b4e4f32a153fd8f11c39a91001941b Mon Sep 17 00:00:00 2001 From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com> Date: Sun, 22 Dec 2024 16:14:32 +0100 Subject: [PATCH] Fix spawner behaviour on failing controller activation or deactivation (#1941) (#1968) --------- Co-authored-by: Sai Kishor Kothakota --- controller_manager/CMakeLists.txt | 18 ++-- .../controller_manager/spawner.py | 4 +- controller_manager/src/controller_manager.cpp | 35 +++++- .../test/test_controller/test_controller.cpp | 32 +++++- ...st_controller_spawner_with_interfaces.yaml | 25 +++++ .../test/test_release_interfaces.cpp | 4 +- .../test/test_spawner_unspawner.cpp | 100 ++++++++++++++++++ 7 files changed, 200 insertions(+), 18 deletions(-) create mode 100644 controller_manager/test/test_controller_spawner_with_interfaces.yaml diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 72b63962da..689e7c7244 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -183,17 +183,13 @@ if(BUILD_TESTING) target_link_libraries(test_hardware_spawner ${PROJECT_NAME}) ament_target_dependencies(test_hardware_spawner ros2_control_test_assets) - install(FILES test/test_controller_spawner_with_type.yaml - DESTINATION test) - - install(FILES test/test_controller_spawner_with_basic_controllers.yaml - DESTINATION test) - - install(FILES test/test_controller_overriding_parameters.yaml - DESTINATION test) - - install(FILES test/test_controller_spawner_wildcard_entries.yaml - DESTINATION test) + install(FILES + test/test_controller_spawner_with_type.yaml + test/test_controller_spawner_with_basic_controllers.yaml + test/test_controller_overriding_parameters.yaml + test/test_controller_spawner_wildcard_entries.yaml + test/test_controller_spawner_with_interfaces.yaml + DESTINATION test) ament_add_gmock( test_hardware_management_srvs diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index c7fab71403..9f52fae2d3 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -265,7 +265,7 @@ def main(args=None): ) if not ret.ok: node.get_logger().error( - bcolors.FAIL + "Failed to activate controller" + bcolors.ENDC + f"{bcolors.FAIL}Failed to activate controller : {controller_name}{bcolors.ENDC}" ) return 1 @@ -290,7 +290,7 @@ def main(args=None): ) if not ret.ok: node.get_logger().error( - bcolors.FAIL + "Failed to activate the parsed controllers list" + bcolors.ENDC + f"{bcolors.FAIL}Failed to activate the parsed controllers list : {controller_names}{bcolors.ENDC}" ) return 1 diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 1f387b89ee..29b8ac2e54 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1264,6 +1264,7 @@ controller_interface::return_type ControllerManager::switch_controller( to = controllers; // update the claimed interface controller info + auto switch_result = controller_interface::return_type::OK; for (auto & controller : to) { if (is_controller_active(controller.c)) @@ -1284,6 +1285,32 @@ controller_interface::return_type ControllerManager::switch_controller( { controller.info.claimed_interfaces.clear(); } + if ( + std::find(activate_request_.begin(), activate_request_.end(), controller.info.name) != + activate_request_.end()) + { + if (!is_controller_active(controller.c)) + { + RCLCPP_ERROR( + get_logger(), "Could not activate controller : '%s'", controller.info.name.c_str()); + switch_result = controller_interface::return_type::ERROR; + } + } + /// @note The following is the case of the real controllers that are deactivated and doesn't + /// include the chained controllers that are deactivated and activated + if ( + std::find(deactivate_request_.begin(), deactivate_request_.end(), controller.info.name) != + deactivate_request_.end() && + std::find(activate_request_.begin(), activate_request_.end(), controller.info.name) == + activate_request_.end()) + { + if (is_controller_active(controller.c)) + { + RCLCPP_ERROR( + get_logger(), "Could not deactivate controller : '%s'", controller.info.name.c_str()); + switch_result = controller_interface::return_type::ERROR; + } + } } // switch lists @@ -1293,8 +1320,10 @@ controller_interface::return_type ControllerManager::switch_controller( clear_requests(); - RCLCPP_DEBUG(get_logger(), "Successfully switched controllers"); - return controller_interface::return_type::OK; + RCLCPP_DEBUG_EXPRESSION( + get_logger(), switch_result == controller_interface::return_type::OK, + "Successfully switched controllers"); + return switch_result; } controller_interface::ControllerInterfaceBaseSharedPtr ControllerManager::add_controller_impl( @@ -1518,6 +1547,7 @@ void ControllerManager::activate_controllers() get_logger(), "Resource conflict for controller '%s'. Command interface '%s' is already claimed.", controller_name.c_str(), command_interface.c_str()); + command_loans.clear(); assignment_successful = false; break; } @@ -1529,6 +1559,7 @@ void ControllerManager::activate_controllers() { RCLCPP_ERROR( get_logger(), "Can't activate controller '%s': %s", controller_name.c_str(), e.what()); + command_loans.clear(); assignment_successful = false; break; } diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index b6eac9b689..e86fd17b07 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -64,7 +64,7 @@ controller_interface::return_type TestController::update( for (size_t i = 0; i < command_interfaces_.size(); ++i) { - RCLCPP_INFO( + RCLCPP_DEBUG( get_node()->get_logger(), "Setting value of command interface '%s' to %f", command_interfaces_[i].get_name().c_str(), external_commands_for_testing_[i]); command_interfaces_[i].set_value(external_commands_for_testing_[i]); @@ -77,6 +77,36 @@ CallbackReturn TestController::on_init() { return CallbackReturn::SUCCESS; } CallbackReturn TestController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { + auto ctrl_node = get_node(); + if (!ctrl_node->has_parameter("command_interfaces")) + { + ctrl_node->declare_parameter("command_interfaces", std::vector({})); + } + if (!ctrl_node->has_parameter("state_interfaces")) + { + ctrl_node->declare_parameter("state_interfaces", std::vector({})); + } + const std::vector command_interfaces = + ctrl_node->get_parameter("command_interfaces").as_string_array(); + const std::vector state_interfaces = + ctrl_node->get_parameter("state_interfaces").as_string_array(); + if (!command_interfaces.empty() || !state_interfaces.empty()) + { + cmd_iface_cfg_.names.clear(); + state_iface_cfg_.names.clear(); + for (const auto & cmd_itf : command_interfaces) + { + cmd_iface_cfg_.names.push_back(cmd_itf); + } + cmd_iface_cfg_.type = controller_interface::interface_configuration_type::INDIVIDUAL; + external_commands_for_testing_.resize(command_interfaces.size(), 0.0); + for (const auto & state_itf : state_interfaces) + { + state_iface_cfg_.names.push_back(state_itf); + } + state_iface_cfg_.type = controller_interface::interface_configuration_type::INDIVIDUAL; + } + return CallbackReturn::SUCCESS; } diff --git a/controller_manager/test/test_controller_spawner_with_interfaces.yaml b/controller_manager/test/test_controller_spawner_with_interfaces.yaml new file mode 100644 index 0000000000..05b650c232 --- /dev/null +++ b/controller_manager/test/test_controller_spawner_with_interfaces.yaml @@ -0,0 +1,25 @@ +ctrl_with_joint2_command_interface: + ros__parameters: + type: "controller_manager/test_controller" + command_interfaces: + - "joint2/velocity" + +ctrl_with_joint1_command_interface: + ros__parameters: + type: "controller_manager/test_controller" + command_interfaces: + - "joint1/position" + +ctrl_with_joint1_and_joint2_command_interfaces: + ros__parameters: + type: "controller_manager/test_controller" + command_interfaces: + - "joint1/position" + - "joint2/velocity" + +ctrl_with_state_interfaces: + ros__parameters: + type: "controller_manager/test_controller" + state_interfaces: + - "joint1/position" + - "joint2/position" diff --git a/controller_manager/test/test_release_interfaces.cpp b/controller_manager/test/test_release_interfaces.cpp index 3ee141edaa..b72384b7fd 100644 --- a/controller_manager/test/test_release_interfaces.cpp +++ b/controller_manager/test/test_release_interfaces.cpp @@ -86,7 +86,7 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface) 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()); + EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, abstract_test_controller1.c->get_state().id()); @@ -190,7 +190,7 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface) 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()); + EXPECT_EQ(controller_interface::return_type::ERROR, switch_future.get()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, abstract_test_controller1.c->get_state().id()); diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 8a0c35a6a3..7184428c5a 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -411,6 +411,106 @@ TEST_F(TestLoadController, spawner_test_with_wildcard_entries_with_no_ctrl_name) verify_ctrl_parameter(wildcard_ctrl_3.c->get_node(), true); } +TEST_F(TestLoadController, spawner_test_failed_activation_of_controllers) +{ + const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + + "/test/test_controller_spawner_with_interfaces.yaml"; + + ControllerManagerRunner cm_runner(this); + // Provide controller type via the parsed file + EXPECT_EQ( + call_spawner( + "ctrl_with_joint1_command_interface ctrl_with_joint2_command_interface -c " + "test_controller_manager " + "--controller-manager-timeout 1.0 " + "-p " + + test_file_path), + 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); + + auto ctrl_with_joint2_command_interface = cm_->get_loaded_controllers()[1]; + ASSERT_EQ(ctrl_with_joint2_command_interface.info.name, "ctrl_with_joint2_command_interface"); + ASSERT_EQ( + ctrl_with_joint2_command_interface.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + EXPECT_EQ( + ctrl_with_joint2_command_interface.c->get_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ( + ctrl_with_joint2_command_interface.c->command_interface_configuration().names.size(), 1ul); + ASSERT_THAT( + ctrl_with_joint2_command_interface.c->command_interface_configuration().names, + std::vector({"joint2/velocity"})); + + auto ctrl_with_joint1_command_interface = cm_->get_loaded_controllers()[0]; + ASSERT_EQ(ctrl_with_joint1_command_interface.info.name, "ctrl_with_joint1_command_interface"); + ASSERT_EQ( + ctrl_with_joint1_command_interface.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); + EXPECT_EQ( + ctrl_with_joint1_command_interface.c->get_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ASSERT_EQ( + ctrl_with_joint1_command_interface.c->command_interface_configuration().names.size(), 1ul); + ASSERT_THAT( + ctrl_with_joint1_command_interface.c->command_interface_configuration().names, + std::vector({"joint1/position"})); + + EXPECT_EQ( + call_spawner( + "ctrl_with_joint1_and_joint2_command_interfaces -c test_controller_manager " + "--controller-manager-timeout 1.0 " + "-p " + + test_file_path), + 256) + << "Should fail as the ctrl_with_joint1_command_interface and " + "ctrl_with_joint2_command_interface are active"; + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul); + + auto ctrl_with_joint1_and_joint2_command_interfaces = cm_->get_loaded_controllers()[2]; + ASSERT_EQ( + ctrl_with_joint1_and_joint2_command_interfaces.info.name, + "ctrl_with_joint1_and_joint2_command_interfaces"); + ASSERT_EQ( + ctrl_with_joint1_and_joint2_command_interfaces.info.type, + test_controller::TEST_CONTROLLER_CLASS_NAME); + ASSERT_EQ( + ctrl_with_joint1_and_joint2_command_interfaces.c->get_state().id(), + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + ASSERT_EQ( + ctrl_with_joint1_and_joint2_command_interfaces.c->command_interface_configuration() + .names.size(), + 2ul); + ASSERT_THAT( + ctrl_with_joint1_and_joint2_command_interfaces.c->command_interface_configuration().names, + std::vector({"joint1/position", "joint2/velocity"})); + + EXPECT_EQ(call_unspawner("ctrl_with_joint1_command_interface -c test_controller_manager"), 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); + EXPECT_EQ( + call_spawner( + "ctrl_with_joint1_and_joint2_command_interfaces -c test_controller_manager " + "--controller-manager-timeout 1.0 " + "-p " + + test_file_path), + 256) + << "Should fail as the ctrl_with_joint2_command_interface is still active"; + + EXPECT_EQ(call_unspawner("ctrl_with_joint2_command_interface -c test_controller_manager"), 0); + + ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul); + EXPECT_EQ( + call_spawner( + "ctrl_with_joint1_and_joint2_command_interfaces -c test_controller_manager " + "--controller-manager-timeout 1.0 " + "-p " + + test_file_path), + 0) + << "Should pass as the ctrl_with_joint1_command_interface and " + "ctrl_with_joint2_command_interface are inactive"; +} + TEST_F(TestLoadController, unload_on_kill) { // Launch spawner with unload on kill