diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 686c25933f..ece49f44f2 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -458,12 +458,13 @@ class ControllerManager : public rclcpp::Node * * \param[in] controllers list with controllers. * \param[in] activation_list list with controllers to activate. + * \param[in] deactivation_list list with controllers to deactivate. * \param[out] message describing the result of the check. * \return return_type::OK if all interfaces are available, otherwise return_type::ERROR. */ controller_interface::return_type check_for_interfaces_availability_to_activate( const std::vector & controllers, const std::vector activation_list, - std::string & message); + const std::vector deactivation_list, std::string & message); /** * @brief Inserts a controller into an ordered list based on dependencies to compute the diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index f10cb9c893..a7d3af2478 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1980,7 +1980,7 @@ controller_interface::return_type ControllerManager::switch_controller_cb( if ( check_for_interfaces_availability_to_activate( - controllers, switch_params_.activate_request, message) != + controllers, switch_params_.activate_request, switch_params_.deactivate_request, message) != controller_interface::return_type::OK) { clear_requests(); @@ -2456,7 +2456,7 @@ void ControllerManager::activate_controllers( } // Now prepare and perform the stop interface switching as this is needed for exclusive // interfaces - if ( + else if ( !failed_controllers_command_interfaces.empty() && (!resource_manager_->prepare_command_mode_switch({}, failed_controllers_command_interfaces) || !resource_manager_->perform_command_mode_switch({}, failed_controllers_command_interfaces))) @@ -2963,6 +2963,8 @@ void ControllerManager::manage_switch() switch_params_.deactivate_command_interface_request)) { RCLCPP_ERROR(get_logger(), "Error while performing mode switch."); + // If the hardware switching fails, there is no point in continuing to switch controllers + return; } execution_time_.switch_perform_mode_time = std::chrono::duration(std::chrono::steady_clock::now() - start_time) @@ -3926,10 +3928,37 @@ void ControllerManager::publish_activity() controller_interface::return_type ControllerManager::check_for_interfaces_availability_to_activate( const std::vector & controllers, const std::vector activation_list, - std::string & message) + const std::vector deactivation_list, std::string & message) { + std::vector future_unavailable_cmd_interfaces = {}; + std::vector future_available_cmd_interfaces = {}; + for (const auto & controller_name : deactivation_list) + { + auto controller_it = std::find_if( + controllers.begin(), controllers.end(), + std::bind(controller_name_compare, std::placeholders::_1, controller_name)); + if (controller_it == controllers.end()) + { + message = fmt::format( + FMT_COMPILE("Unable to find the deactivation controller : '{}' within the controller list"), + controller_name); + RCLCPP_ERROR(get_logger(), "%s", message.c_str()); + return controller_interface::return_type::ERROR; + } + const auto controller_cmd_interfaces = + controller_it->c->command_interface_configuration().names; + for (const auto & cmd_itf : controller_cmd_interfaces) + { + future_available_cmd_interfaces.push_back(cmd_itf); + } + } for (const auto & controller_name : activation_list) { + if (ros2_control::has_item(deactivation_list, controller_name)) + { + // skip controllers that are being deactivated and activated in the same request + continue; + } auto controller_it = std::find_if( controllers.begin(), controllers.end(), std::bind(controller_name_compare, std::placeholders::_1, controller_name)); @@ -3959,6 +3988,29 @@ controller_interface::return_type ControllerManager::check_for_interfaces_availa RCLCPP_WARN(get_logger(), "%s", message.c_str()); return controller_interface::return_type::ERROR; } + if ( + resource_manager_->command_interface_is_claimed(cmd_itf) && + !ros2_control::has_item(future_available_cmd_interfaces, cmd_itf)) + { + message = fmt::format( + FMT_COMPILE( + "Unable to activate controller '{}' since the " + "command interface '{}' is currently claimed by another controller."), + controller_it->info.name, cmd_itf); + RCLCPP_WARN(get_logger(), "%s", message.c_str()); + return controller_interface::return_type::ERROR; + } + if (ros2_control::has_item(future_unavailable_cmd_interfaces, cmd_itf)) + { + message = fmt::format( + FMT_COMPILE( + "Unable to activate controller '{}' since the " + "command interface '{}' will be used by another controller that is being activated."), + controller_it->info.name, cmd_itf); + RCLCPP_WARN(get_logger(), "%s", message.c_str()); + return controller_interface::return_type::ERROR; + } + future_unavailable_cmd_interfaces.push_back(cmd_itf); } for (const auto & state_itf : controller_state_interfaces) { diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index ae3f138bcc..30fb8ec992 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -2551,3 +2551,130 @@ TEST_F(TestControllerManagerSrvs, switch_controller_controllers_taking_long_time EXPECT_EQ(old_counter_2 + 1, test_controller_2->internal_counter); EXPECT_EQ(old_counter_3 + 1, test_controller_3->internal_counter); } + +TEST_F(TestControllerManagerSrvs, switch_controller_test_two_controllers_using_same_interface) +{ + // create server client and request + rclcpp::executors::SingleThreadedExecutor srv_executor; + rclcpp::Node::SharedPtr srv_node = std::make_shared("srv_client"); + srv_executor.add_node(srv_node); + rclcpp::Client::SharedPtr client = + srv_node->create_client("test_controller_manager/list_controllers"); + auto request = std::make_shared(); + + // create set of controllers + static constexpr char TEST_CONTROLLER_1[] = "test_controller_1"; + static constexpr char TEST_CONTROLLER_2[] = "test_controller_2"; + + // create non-chained controller + auto test_controller_1 = std::make_shared(); + controller_interface::InterfaceConfiguration cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint1/position", "joint1/max_velocity"}}; + controller_interface::InterfaceConfiguration state_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint1/position", "joint1/velocity"}}; + test_controller_1->set_command_interface_configuration(cmd_cfg); + test_controller_1->set_state_interface_configuration(state_cfg); + + auto test_controller_2 = std::make_shared(); + cmd_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint1/position", "joint2/velocity"}}; + state_cfg = { + controller_interface::interface_configuration_type::INDIVIDUAL, + {"joint1/position", "joint2/velocity"}}; + test_controller_2->set_command_interface_configuration(cmd_cfg); + test_controller_2->set_state_interface_configuration(state_cfg); + + // add controllers + cm_->add_controller( + test_controller_1, TEST_CONTROLLER_1, test_controller::TEST_CONTROLLER_CLASS_NAME); + cm_->add_controller( + test_controller_2, TEST_CONTROLLER_2, test_controller::TEST_CONTROLLER_CLASS_NAME); + + // get controller list before configure + auto result = call_service_and_wait(*client, request, srv_executor); + // check chainable controller + ASSERT_EQ(2u, result->controller.size()); + ASSERT_EQ(result->controller[0].name, TEST_CONTROLLER_1); + ASSERT_EQ(result->controller[1].name, TEST_CONTROLLER_2); + + // configure controllers + for (const auto & controller : {TEST_CONTROLLER_1, TEST_CONTROLLER_2}) + { + cm_->configure_controller(controller); + } + + // get controller list after configure + result = call_service_and_wait(*client, request, srv_executor); + ASSERT_EQ(2u, result->controller.size()); + + // reordered controllers + ASSERT_EQ(result->controller[0].name, TEST_CONTROLLER_2); + ASSERT_EQ(result->controller[1].name, TEST_CONTROLLER_1); + + // Check individual activation and they should work fine + auto res = cm_->switch_controller( + {TEST_CONTROLLER_1}, {}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true, + rclcpp::Duration(0, 0)); + ASSERT_EQ(res, controller_interface::return_type::OK); + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + + res = cm_->switch_controller( + {}, {TEST_CONTROLLER_1}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true, + rclcpp::Duration(0, 0)); + ASSERT_EQ(res, controller_interface::return_type::OK); + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + + // Now test activating controller_2 + res = cm_->switch_controller( + {TEST_CONTROLLER_2}, {}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true, + rclcpp::Duration(0, 0)); + ASSERT_EQ(res, controller_interface::return_type::OK); + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + test_controller_2->get_lifecycle_state().id()); + + res = cm_->switch_controller( + {}, {TEST_CONTROLLER_2}, controller_manager_msgs::srv::SwitchController::Request::STRICT, true, + rclcpp::Duration(0, 0)); + ASSERT_EQ(res, controller_interface::return_type::OK); + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); + + // Now try activating both the controllers at once, it should fail as they are using same + // interface + res = cm_->switch_controller( + {TEST_CONTROLLER_1, TEST_CONTROLLER_2}, {}, + controller_manager_msgs::srv::SwitchController::Request::STRICT, false, rclcpp::Duration(0, 0)); + ASSERT_EQ(res, controller_interface::return_type::ERROR); + + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_1->get_lifecycle_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + test_controller_2->get_lifecycle_state().id()); +} diff --git a/controller_manager/test/test_release_interfaces.cpp b/controller_manager/test/test_release_interfaces.cpp index 87646167d4..122c022813 100644 --- a/controller_manager/test/test_release_interfaces.cpp +++ b/controller_manager/test/test_release_interfaces.cpp @@ -81,7 +81,7 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface) 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))) + ASSERT_EQ(std::future_status::ready, 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::ERROR, switch_future.get()); @@ -185,7 +185,7 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface) 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))) + ASSERT_EQ(std::future_status::ready, 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::ERROR, switch_future.get()); @@ -206,12 +206,12 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface) auto switch_future = std::async( std::launch::async, &controller_manager::ControllerManager::switch_controller, cm_, start_controllers, stop_controllers, BEST_EFFORT, true, rclcpp::Duration(0, 0)); - ASSERT_EQ(std::future_status::timeout, switch_future.wait_for(std::chrono::milliseconds(100))) + ASSERT_EQ(std::future_status::ready, 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::ERROR, switch_future.get()); ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, abstract_test_controller1.c->get_lifecycle_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE,