diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 72b63962da..2c740661e8 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -179,6 +179,7 @@ if(BUILD_TESTING) test/test_hardware_spawner.cpp TIMEOUT 120 ) +<<<<<<< HEAD target_include_directories(test_hardware_spawner PRIVATE include) target_link_libraries(test_hardware_spawner ${PROJECT_NAME}) ament_target_dependencies(test_hardware_spawner ros2_control_test_assets) @@ -194,6 +195,21 @@ if(BUILD_TESTING) install(FILES test/test_controller_spawner_wildcard_entries.yaml DESTINATION test) +======= + target_link_libraries(test_hardware_spawner + controller_manager + test_controller + ros2_control_test_assets::ros2_control_test_assets + ) + + 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) +>>>>>>> 038a05f (Fix spawner behaviour on failing controller activation or deactivation (#1941)) 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..34acb02737 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; } @@ -1528,7 +1558,15 @@ void ControllerManager::activate_controllers() catch (const std::exception & e) { RCLCPP_ERROR( +<<<<<<< HEAD get_logger(), "Can't activate controller '%s': %s", controller_name.c_str(), e.what()); +======= + get_logger(), + "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(); +>>>>>>> 038a05f (Fix spawner behaviour on failing controller activation or deactivation (#1941)) assignment_successful = false; break; } @@ -1579,11 +1617,26 @@ void ControllerManager::activate_controllers() if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { RCLCPP_ERROR( +<<<<<<< HEAD get_logger(), "After activation, controller '%s' is in state '%s' (%d), expected '%s' (%d).", controller->get_node()->get_name(), new_state.label().c_str(), new_state.id(), hardware_interface::lifecycle_state_names::ACTIVE, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); +======= + 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 (...) + { + RCLCPP_ERROR( + get_logger(), "Caught unknown exception while activating the controller '%s'", + controller_name.c_str()); + controller->release_interfaces(); + continue; +>>>>>>> 038a05f (Fix spawner behaviour on failing controller activation or deactivation (#1941)) } // if it is a chainable controller, make the reference interfaces available on activation diff --git a/controller_manager/test/test_controller/test_controller.cpp b/controller_manager/test/test_controller/test_controller.cpp index b6eac9b689..17dd46b330 100644 --- a/controller_manager/test/test_controller/test_controller.cpp +++ b/controller_manager/test/test_controller/test_controller.cpp @@ -64,10 +64,34 @@ controller_interface::return_type TestController::update( for (size_t i = 0; i < command_interfaces_.size(); ++i) { +<<<<<<< HEAD RCLCPP_INFO( 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]); +======= + command_interfaces_[0].set_value(set_first_command_interface_value_to); + // reset to be easier to test + set_first_command_interface_value_to = std::numeric_limits::quiet_NaN(); + } + else + { + for (size_t i = 0; i < command_interfaces_.size(); ++i) + { + if (!std::isfinite(external_commands_for_testing_[i])) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "External command value for command interface '%s' is not finite", + command_interfaces_[i].get_name().c_str()); + return controller_interface::return_type::ERROR; + } + 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]); + } +>>>>>>> 038a05f (Fix spawner behaviour on failing controller activation or deactivation (#1941)) } return controller_interface::return_type::OK; @@ -77,6 +101,51 @@ CallbackReturn TestController::on_init() { return CallbackReturn::SUCCESS; } CallbackReturn TestController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { +<<<<<<< HEAD +======= + 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, + [this]( + const std::shared_ptr request, + std::shared_ptr response) + { + RCLCPP_INFO_STREAM( + get_node()->get_logger(), "Setting response to " << std::boolalpha << request->data); + response->success = request->data; + }); + +>>>>>>> 038a05f (Fix spawner behaviour on failing controller activation or deactivation (#1941)) 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..6a91531357 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()[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