diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 0a2b103fbc..9bb3186f2e 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -176,6 +176,20 @@ if(BUILD_TESTING) DESTINATION lib ) + add_library(test_controller_failed_activate SHARED + test/test_controller_failed_activate/test_controller_failed_activate.cpp + ) + target_link_libraries(test_controller_failed_activate PUBLIC + controller_manager + ) + target_compile_definitions(test_controller_failed_activate PRIVATE "CONTROLLER_MANAGER_BUILDING_DLL") + pluginlib_export_plugin_description_file( + controller_interface test/test_controller_failed_activate/test_controller_failed_activate.xml) + install( + TARGETS test_controller_failed_activate + DESTINATION lib + ) + ament_add_gmock(test_release_interfaces test/test_release_interfaces.cpp APPEND_ENV AMENT_PREFIX_PATH=${ament_index_build_path}_$ @@ -184,6 +198,7 @@ if(BUILD_TESTING) controller_manager test_controller test_controller_with_interfaces + test_controller_failed_activate ros2_control_test_assets::ros2_control_test_assets ) diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index 9e720fc681..111a1a2731 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -411,6 +411,12 @@ Hardware and Controller Errors If the hardware during it's ``read`` or ``write`` method returns ``return_type::ERROR``, the controller manager will stop all controllers that are using the hardware's command and state interfaces. Likewise, if a controller returns ``return_type::ERROR`` from its ``update`` method, the controller manager will deactivate the respective controller. In future, the controller manager will try to start any fallback controllers if available. +Factors that affect Determinism +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +When run under the conditions determined in the above section, the determinism is assured up to the limitations of the hardware and the real-time kernel. However, there are some situations that can affect determinism: + +* When a controller fails to activate, the controller_manager will call the methods ``prepare_command_mode_switch`` and ``perform_command_mode_switch`` to stop the started interfaces. These calls can cause jitter in the main control loop. + Support for Asynchronous Updates ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ For some applications, it is desirable to run a controller at a lower frequency than the controller manager's update rate. For instance, if the ``update_rate`` for the controller manager is 100Hz, the sum of the execution times of all controllers' ``update`` calls and hardware components ``read`` and ``write`` calls must be below 10ms. If one controller requires 15ms of execution time, it cannot be executed synchronously without affecting the overall system update rate. Running a controller asynchronously can be beneficial in this scenario. diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 3f2898aac8..495433d47b 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1992,6 +1992,7 @@ void ControllerManager::activate_controllers( const std::vector & rt_controller_list, const std::vector controllers_to_activate) { + std::vector failed_controllers_command_interfaces; for (const auto & controller_name : controllers_to_activate) { auto found_it = std::find_if( @@ -2100,35 +2101,38 @@ void ControllerManager::activate_controllers( } controller->assign_interfaces(std::move(command_loans), std::move(state_loans)); + auto new_state = controller->get_lifecycle_state(); try { found_it->periodicity_statistics->Reset(); found_it->execution_time_statistics->Reset(); - const auto new_state = controller->get_node()->activate(); - if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) - { - RCLCPP_ERROR( - 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); - } + new_state = controller->get_node()->activate(); } catch (const std::exception & e) { 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 (...) { RCLCPP_ERROR( get_logger(), "Caught unknown exception while activating the controller '%s'", controller_name.c_str()); + } + if (new_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + RCLCPP_ERROR( + get_logger(), + "After activation, controller '%s' is in state '%s' (%d), expected '%s' (%d). Releasing " + "interfaces!", + 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); controller->release_interfaces(); + failed_controllers_command_interfaces.insert( + failed_controllers_command_interfaces.end(), command_interface_names.begin(), + command_interface_names.end()); continue; } @@ -2140,6 +2144,18 @@ void ControllerManager::activate_controllers( resource_manager_->make_controller_reference_interfaces_available(controller_name); } } + // Now prepare and perform the stop interface switching as this is needed for exclusive + // interfaces + 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))) + { + RCLCPP_ERROR( + get_logger(), + "Error switching back the interfaces in the hardware when the controller activation " + "failed."); + } } void ControllerManager::activate_controllers_asap( diff --git a/controller_manager/test/test_controller_failed_activate/test_controller_failed_activate.cpp b/controller_manager/test/test_controller_failed_activate/test_controller_failed_activate.cpp new file mode 100644 index 0000000000..c13df1a713 --- /dev/null +++ b/controller_manager/test/test_controller_failed_activate/test_controller_failed_activate.cpp @@ -0,0 +1,66 @@ +// Copyright 2021 Department of Engineering Cybernetics, NTNU. +// +// 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 "test_controller_failed_activate.hpp" + +#include +#include + +#include "lifecycle_msgs/msg/transition.hpp" + +namespace test_controller_failed_activate +{ +TestControllerFailedActivate::TestControllerFailedActivate() +: controller_interface::ControllerInterface() +{ +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +TestControllerFailedActivate::on_init() +{ + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +controller_interface::return_type TestControllerFailedActivate::update( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +{ + return controller_interface::return_type::OK; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +TestControllerFailedActivate::on_configure(const rclcpp_lifecycle::State & /*previous_state&*/) +{ + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +TestControllerFailedActivate::on_activate(const rclcpp_lifecycle::State & /*previous_state&*/) +{ + // Simply simulate a controller that can not be activated + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +TestControllerFailedActivate::on_cleanup(const rclcpp_lifecycle::State & /*previous_state*/) +{ + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +} // namespace test_controller_failed_activate + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + test_controller_failed_activate::TestControllerFailedActivate, + controller_interface::ControllerInterface) diff --git a/controller_manager/test/test_controller_failed_activate/test_controller_failed_activate.hpp b/controller_manager/test/test_controller_failed_activate/test_controller_failed_activate.hpp new file mode 100644 index 0000000000..e2f55dcc9f --- /dev/null +++ b/controller_manager/test/test_controller_failed_activate/test_controller_failed_activate.hpp @@ -0,0 +1,67 @@ +// Copyright 2020 Department of Engineering Cybernetics, NTNU +// +// 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. + +#ifndef TEST_CONTROLLER_FAILED_ACTIVATE__TEST_CONTROLLER_FAILED_ACTIVATE_HPP_ +#define TEST_CONTROLLER_FAILED_ACTIVATE__TEST_CONTROLLER_FAILED_ACTIVATE_HPP_ + +#include +#include + +#include "controller_manager/controller_manager.hpp" + +namespace test_controller_failed_activate +{ +// Corresponds to the name listed within the pluginglib xml +constexpr char TEST_CONTROLLER_WITH_INTERFACES_CLASS_NAME[] = + "controller_manager/test_controller_failed_activate"; +// Corresponds to the command interface to claim +constexpr char TEST_CONTROLLER_COMMAND_INTERFACE[] = "joint2/velocity"; +class TestControllerFailedActivate : public controller_interface::ControllerInterface +{ +public: + TestControllerFailedActivate(); + + virtual ~TestControllerFailedActivate() = default; + + controller_interface::InterfaceConfiguration command_interface_configuration() const override + { + return controller_interface::InterfaceConfiguration{ + controller_interface::interface_configuration_type::INDIVIDUAL, + {TEST_CONTROLLER_COMMAND_INTERFACE}}; + } + + controller_interface::InterfaceConfiguration state_interface_configuration() const override + { + return controller_interface::InterfaceConfiguration{ + controller_interface::interface_configuration_type::NONE}; + } + + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_init() override; + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State & previous_state) override; +}; + +} // namespace test_controller_failed_activate + +#endif // TEST_CONTROLLER_FAILED_ACTIVATE__TEST_CONTROLLER_FAILED_ACTIVATE_HPP_ diff --git a/controller_manager/test/test_controller_failed_activate/test_controller_failed_activate.xml b/controller_manager/test/test_controller_failed_activate/test_controller_failed_activate.xml new file mode 100644 index 0000000000..80c4e6bef5 --- /dev/null +++ b/controller_manager/test/test_controller_failed_activate/test_controller_failed_activate.xml @@ -0,0 +1,9 @@ + + + + + Controller used for testing + + + + diff --git a/controller_manager/test/test_release_interfaces.cpp b/controller_manager/test/test_release_interfaces.cpp index 67b7cb5b68..c931502c7b 100644 --- a/controller_manager/test/test_release_interfaces.cpp +++ b/controller_manager/test/test_release_interfaces.cpp @@ -19,6 +19,7 @@ #include "gmock/gmock.h" #include "lifecycle_msgs/msg/state.hpp" #include "test_controller/test_controller.hpp" +#include "test_controller_failed_activate/test_controller_failed_activate.hpp" #include "test_controller_with_interfaces/test_controller_with_interfaces.hpp" using ::testing::_; @@ -330,3 +331,72 @@ TEST_F(TestReleaseExclusiveInterfaces, test_exclusive_interface_deactivation_on_ abstract_test_controller2.c->get_lifecycle_state().id()); } } + +TEST_F(TestReleaseExclusiveInterfaces, test_exclusive_interface_switching_failure) +{ + std::string controller_type = + test_controller_failed_activate::TEST_CONTROLLER_WITH_INTERFACES_CLASS_NAME; + + // Load two controllers of different names + std::string controller_name1 = "test_controller1"; + std::string controller_name2 = "test_controller2"; + ASSERT_NO_THROW(cm_->load_controller(controller_name1, controller_type)); + ASSERT_NO_THROW(cm_->load_controller( + controller_name2, test_controller_with_interfaces::TEST_CONTROLLER_WITH_INTERFACES_CLASS_NAME)); + 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 controller + // test_controller1 activation always fails + 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::ERROR, switch_future.get()); + 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 second controller, interfaces should have been released + // test_controller2 always successfully activates + RCLCPP_INFO(cm_->get_logger(), "Starting controller #2"); + std::vector start_controllers = {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_INACTIVE, + 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 2345f0af88..48cb94e3d5 100644 --- a/hardware_interface_testing/CMakeLists.txt +++ b/hardware_interface_testing/CMakeLists.txt @@ -20,13 +20,14 @@ foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) 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_actuator_exclusive_interfaces.cpp) + test/test_components/test_actuator.cpp + test/test_components/test_sensor.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 + DESTINATION lib ) pluginlib_export_plugin_description_file( hardware_interface test/test_components/test_components.xml) diff --git a/hardware_interface_testing/test/test_components/test_components.xml b/hardware_interface_testing/test/test_components/test_components.xml index b3ae421601..31a3dd3e9b 100644 --- a/hardware_interface_testing/test/test_components/test_components.xml +++ b/hardware_interface_testing/test/test_components/test_components.xml @@ -5,6 +5,12 @@ Test Actuator + + + Test Actuator + + +