diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index a97f171dd0..69f6ef24df 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -216,17 +216,13 @@ if(BUILD_TESTING) ros2_control_test_assets::ros2_control_test_assets ) - install(FILES test/test_controller_spawner_with_fallback_controllers.yaml - DESTINATION test) - - install(FILES test/test_controller_spawner_with_type.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_overriding_parameters.yaml + test/test_controller_spawner_with_fallback_controllers.yaml + test/test_controller_spawner_wildcard_entries.yaml + test/test_controller_spawner_with_interfaces.yaml + DESTINATION test) ament_add_gmock(test_hardware_management_srvs test/test_hardware_management_srvs.cpp diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index 1a55f17e6d..58eb7be198 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -270,7 +270,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 @@ -295,7 +295,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 61b9477e08..34566f95b3 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1479,6 +1479,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)) @@ -1499,6 +1500,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 @@ -1508,8 +1535,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( @@ -1741,6 +1770,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; } @@ -1755,6 +1785,7 @@ void ControllerManager::activate_controllers( "Caught exception of type : %s while claiming the command interfaces. Can't activate " "controller '%s': %s", typeid(e).name(), controller_name.c_str(), e.what()); + command_loans.clear(); assignment_successful = false; break; } @@ -1824,6 +1855,7 @@ void ControllerManager::activate_controllers( RCLCPP_ERROR( get_logger(), "Caught exception of type : %s while activating the controller '%s': %s", typeid(e).name(), controller_name.c_str(), e.what()); + controller->release_interfaces(); continue; } catch (...) @@ -1831,6 +1863,7 @@ void ControllerManager::activate_controllers( RCLCPP_ERROR( get_logger(), "Caught unknown exception while activating the controller '%s'", controller_name.c_str()); + controller->release_interfaces(); continue; } diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index d20e7598a9..ee8377f2a3 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -87,7 +87,7 @@ controller_interface::return_type TestController::update( command_interfaces_[i].get_name().c_str()); return controller_interface::return_type::ERROR; } - 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]); @@ -101,6 +101,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; + } + const std::string service_name = get_node()->get_name() + std::string("/set_bool"); service_ = get_node()->create_service( service_name, @@ -112,6 +142,7 @@ CallbackReturn TestController::on_configure(const rclcpp_lifecycle::State & /*pr get_node()->get_logger(), "Setting response to " << std::boolalpha << request->data); response->success = request->data; }); + 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 9caef761ab..4dedb47241 100644 --- a/controller_manager/test/test_release_interfaces.cpp +++ b/controller_manager/test/test_release_interfaces.cpp @@ -84,7 +84,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_lifecycle_state().id()); @@ -188,7 +188,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_lifecycle_state().id()); diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index a4159d7fbe..ff39b69e8c 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -437,6 +437,106 @@ TEST_F(TestLoadController, spawner_test_with_wildcard_entries_with_no_ctrl_name) verify_ctrl_parameter(wildcard_ctrl_1.c->get_node(), false); } +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()[0]; + 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_lifecycle_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()[1]; + 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_lifecycle_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()[0]; + 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_lifecycle_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