diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 1bb84eb32c..9544a7ceab 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -181,6 +181,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/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index d7e924a863..71ca554116 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -205,6 +205,44 @@ void get_active_controllers_using_command_interfaces_of_controller( } } +void extract_command_interfaces_for_controller( + const controller_manager::ControllerSpec & ctrl, + const hardware_interface::ResourceManager & resource_manager, + std::vector & request_interface_list) +{ + auto command_interface_config = ctrl.c->command_interface_configuration(); + std::vector command_interface_names = {}; + if (command_interface_config.type == controller_interface::interface_configuration_type::ALL) + { + command_interface_names = resource_manager.available_command_interfaces(); + } + if ( + command_interface_config.type == controller_interface::interface_configuration_type::INDIVIDUAL) + { + command_interface_names = command_interface_config.names; + } + request_interface_list.insert( + request_interface_list.end(), command_interface_names.begin(), command_interface_names.end()); +} + +void get_controller_list_command_interfaces( + const std::vector & controllers_list, + const std::vector & controllers_spec, + const hardware_interface::ResourceManager & resource_manager, + std::vector & request_interface_list) +{ + for (const auto & controller_name : controllers_list) + { + auto found_it = std::find_if( + controllers_spec.begin(), controllers_spec.end(), + std::bind(controller_name_compare, std::placeholders::_1, controller_name)); + if (found_it != controllers_spec.end()) + { + extract_command_interfaces_for_controller( + *found_it, resource_manager, request_interface_list); + } + } +} } // namespace namespace controller_manager @@ -1291,33 +1329,15 @@ controller_interface::return_type ControllerManager::switch_controller( activate_request_.erase(activate_list_it); } - const auto extract_interfaces_for_controller = - [this](const ControllerSpec ctrl, std::vector & request_interface_list) - { - auto command_interface_config = ctrl.c->command_interface_configuration(); - std::vector command_interface_names = {}; - if (command_interface_config.type == controller_interface::interface_configuration_type::ALL) - { - command_interface_names = resource_manager_->available_command_interfaces(); - } - if ( - command_interface_config.type == - controller_interface::interface_configuration_type::INDIVIDUAL) - { - command_interface_names = command_interface_config.names; - } - request_interface_list.insert( - request_interface_list.end(), command_interface_names.begin(), - command_interface_names.end()); - }; - if (in_activate_list) { - extract_interfaces_for_controller(controller, activate_command_interface_request_); + extract_command_interfaces_for_controller( + controller, *resource_manager_, activate_command_interface_request_); } if (in_deactivate_list) { - extract_interfaces_for_controller(controller, deactivate_command_interface_request_); + extract_command_interfaces_for_controller( + controller, *resource_manager_, deactivate_command_interface_request_); } // cache mapping between hardware and controllers for stopping when read/write error happens @@ -2479,6 +2499,26 @@ controller_interface::return_type ControllerManager::update( RCLCPP_ERROR( get_logger(), "Activating fallback controllers : [ %s]", controllers_string.c_str()); } + std::vector failed_controller_interfaces, fallback_controller_interfaces; + failed_controller_interfaces.reserve(500); + get_controller_list_command_interfaces( + failed_controllers_list, rt_controller_list, *resource_manager_, + failed_controller_interfaces); + get_controller_list_command_interfaces( + cumulative_fallback_controllers, rt_controller_list, *resource_manager_, + fallback_controller_interfaces); + if (!failed_controller_interfaces.empty()) + { + if (!(resource_manager_->prepare_command_mode_switch( + fallback_controller_interfaces, failed_controller_interfaces) && + resource_manager_->perform_command_mode_switch( + fallback_controller_interfaces, failed_controller_interfaces))) + { + RCLCPP_ERROR( + get_logger(), + "Error while attempting mode switch when deactivating controllers in update cycle!"); + } + } deactivate_controllers(rt_controller_list, active_controllers_using_interfaces); if (!cumulative_fallback_controllers.empty()) { 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()); + } +} diff --git a/hardware_interface_testing/CMakeLists.txt b/hardware_interface_testing/CMakeLists.txt index cd8e30028a..f37238835c 100644 --- a/hardware_interface_testing/CMakeLists.txt +++ b/hardware_interface_testing/CMakeLists.txt @@ -23,7 +23,8 @@ endforeach() add_library(test_components SHARED test/test_components/test_actuator.cpp test/test_components/test_sensor.cpp -test/test_components/test_system.cpp) +test/test_components/test_system.cpp +test/test_components/test_actuator_exclusive_interfaces.cpp) ament_target_dependencies(test_components hardware_interface pluginlib ros2_control_test_assets) install(TARGETS test_components DESTINATION lib diff --git a/hardware_interface_testing/test/test_components/test_actuator_exclusive_interfaces.cpp b/hardware_interface_testing/test/test_components/test_actuator_exclusive_interfaces.cpp new file mode 100644 index 0000000000..c2c52d72db --- /dev/null +++ b/hardware_interface_testing/test/test_components/test_actuator_exclusive_interfaces.cpp @@ -0,0 +1,180 @@ +// Copyright 2024 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include "hardware_interface/actuator_interface.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" +#include "ros2_control_test_assets/test_hardware_interface_constants.hpp" + +using hardware_interface::ActuatorInterface; +using hardware_interface::CommandInterface; +using hardware_interface::return_type; +using hardware_interface::StateInterface; + +static std::pair extract_joint_and_interface( + const std::string & full_name) +{ + // Signature is: interface/joint + const auto joint_name = full_name.substr(0, full_name.find_last_of('/')); + const auto interface_name = full_name.substr(full_name.find_last_of('/') + 1); + + return {joint_name, interface_name}; +} +struct JointState +{ + double pos; + double vel; + double effort; +}; + +class TestActuatorExclusiveInterfaces : public ActuatorInterface +{ + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + { + if (ActuatorInterface::on_init(info) != CallbackReturn::SUCCESS) + { + return CallbackReturn::ERROR; + } + + for (const auto & j : info.joints) + { + (void)j; // Suppress unused warning + current_states_.emplace_back(JointState{}); + } + + return CallbackReturn::SUCCESS; + } + + std::vector export_state_interfaces() override + { + std::vector state_interfaces; + for (std::size_t i = 0; i < info_.joints.size(); ++i) + { + const auto & joint = info_.joints[i]; + + state_interfaces.emplace_back(hardware_interface::StateInterface( + joint.name, hardware_interface::HW_IF_POSITION, ¤t_states_.at(i).pos)); + state_interfaces.emplace_back(hardware_interface::StateInterface( + joint.name, hardware_interface::HW_IF_VELOCITY, ¤t_states_.at(i).vel)); + state_interfaces.emplace_back(hardware_interface::StateInterface( + joint.name, hardware_interface::HW_IF_EFFORT, ¤t_states_.at(i).effort)); + } + return state_interfaces; + } + + std::vector export_command_interfaces() override + { + std::vector command_interfaces; + for (std::size_t i = 0; i < info_.joints.size(); ++i) + { + const auto & joint = info_.joints[i]; + + command_interfaces.emplace_back(hardware_interface::CommandInterface( + joint.name, hardware_interface::HW_IF_POSITION, ¤t_states_.at(i).pos)); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + joint.name, hardware_interface::HW_IF_VELOCITY, ¤t_states_.at(i).vel)); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + joint.name, hardware_interface::HW_IF_EFFORT, ¤t_states_.at(i).effort)); + } + return command_interfaces; + } + + hardware_interface::return_type prepare_command_mode_switch( + const std::vector & start_interfaces, + const std::vector & stop_interfaces) override + { + std::vector claimed_joint_copy = currently_claimed_joints_; + + for (const auto & interface : stop_interfaces) + { + const auto && [joint_name, interface_name] = extract_joint_and_interface(interface); + if ( + interface_name == hardware_interface::HW_IF_POSITION || + interface_name == hardware_interface::HW_IF_VELOCITY || + interface_name == hardware_interface::HW_IF_EFFORT) + { + claimed_joint_copy.erase( + std::remove(claimed_joint_copy.begin(), claimed_joint_copy.end(), joint_name), + claimed_joint_copy.end()); + } + } + + for (const auto & interface : start_interfaces) + { + const auto && [joint_name, interface_name] = extract_joint_and_interface(interface); + if ( + interface_name == hardware_interface::HW_IF_POSITION || + interface_name == hardware_interface::HW_IF_VELOCITY || + interface_name == hardware_interface::HW_IF_EFFORT) + { + if ( + std::find(claimed_joint_copy.begin(), claimed_joint_copy.end(), joint_name) != + claimed_joint_copy.end()) + { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("TestActuatorExclusiveInterfaces"), + "Joint : " << joint_name + << " is already claimed. This cannot happen as the hardware " + "interface is exclusive."); + throw std::runtime_error( + "Joint : " + joint_name + + " is already claimed. This cannot happen as the hardware interface is exclusive."); + } + } + } + return hardware_interface::return_type::OK; + } + + hardware_interface::return_type perform_command_mode_switch( + const std::vector & start_interfaces, + const std::vector & stop_interfaces) override + { + for (const auto & interface : stop_interfaces) + { + const auto && [joint_name, interface_name] = extract_joint_and_interface(interface); + currently_claimed_joints_.erase( + std::remove(currently_claimed_joints_.begin(), currently_claimed_joints_.end(), joint_name), + currently_claimed_joints_.end()); + } + + for (const auto & interface : start_interfaces) + { + const auto && [joint_name, interface_name] = extract_joint_and_interface(interface); + + currently_claimed_joints_.push_back(joint_name); + } + + return hardware_interface::return_type::OK; + } + + return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + return return_type::OK; + } + + return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override + { + return return_type::OK; + } + +private: + std::vector currently_claimed_joints_; + std::vector current_states_; +}; + +#include "pluginlib/class_list_macros.hpp" // NOLINT +PLUGINLIB_EXPORT_CLASS(TestActuatorExclusiveInterfaces, hardware_interface::ActuatorInterface) diff --git a/hardware_interface_testing/test/test_components/test_components.xml b/hardware_interface_testing/test/test_components/test_components.xml index e24ee28317..b3ae421601 100644 --- a/hardware_interface_testing/test/test_components/test_components.xml +++ b/hardware_interface_testing/test/test_components/test_components.xml @@ -35,4 +35,10 @@ Test Uninitializable System + + + + Test Actuator with Exclusive Interfaces + + diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp index e94d4e6736..e5a4de8cab 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp @@ -1163,6 +1163,59 @@ const auto hardware_resources_missing_command_keys = )"; +const auto hardware_resources_with_exclusive_interface = + R"( + + + test_actuator_exclusive_interfaces + + + + + + + + + + + + + test_actuator_exclusive_interfaces + + + + + + + + + + + + + test_actuator_exclusive_interfaces + + + + + + + + + + + + + test_sensor + 2 + 2 + + + + + +)"; + const auto diffbot_urdf = R"(