From 960010d1dd4a4a03a17b4bf36a4ce0ac1223462f Mon Sep 17 00:00:00 2001 From: Lennart Nachtigall Date: Fri, 19 Apr 2024 08:41:23 +0200 Subject: [PATCH 01/19] added an actuator interface with exclusive interface handling --- hardware_interface_testing/CMakeLists.txt | 10 +- .../test_actuator_exclusive.cpp | 183 ++++++++++++++++++ .../test/test_components/test_components.xml | 6 + 3 files changed, 195 insertions(+), 4 deletions(-) create mode 100644 hardware_interface_testing/test/test_components/test_actuator_exclusive.cpp diff --git a/hardware_interface_testing/CMakeLists.txt b/hardware_interface_testing/CMakeLists.txt index cd8e30028a..441327a14c 100644 --- a/hardware_interface_testing/CMakeLists.txt +++ b/hardware_interface_testing/CMakeLists.txt @@ -21,12 +21,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.cpp + test/test_components/test_sensor.cpp + test/test_components/test_system.cpp + test/test_components/test_actuator_exclusive.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_actuator_exclusive.cpp b/hardware_interface_testing/test/test_components/test_actuator_exclusive.cpp new file mode 100644 index 0000000000..2ca33150b9 --- /dev/null +++ b/hardware_interface_testing/test/test_components/test_actuator_exclusive.cpp @@ -0,0 +1,183 @@ +// Copyright 2020 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 "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 TestActuatorExclusive : 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()) + { + return hardware_interface::return_type::ERROR; + } + } + } + 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); + + // TODO make sure it is not possible to claim multiple interfaces of the same axis + 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_; +}; + +class TestUnitilizableActuatorExclusive : public TestActuatorExclusive +{ + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + { + ActuatorInterface::on_init(info); + return CallbackReturn::ERROR; + } +}; + +#include "pluginlib/class_list_macros.hpp" // NOLINT +PLUGINLIB_EXPORT_CLASS(TestActuatorExclusive, hardware_interface::ActuatorInterface) +PLUGINLIB_EXPORT_CLASS(TestUnitilizableActuatorExclusive, 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..aa307be190 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 + + + From 141887e0d8eb0db4d7c8fe698bc28f83a083c882 Mon Sep 17 00:00:00 2001 From: Lennart Nachtigall Date: Fri, 19 Apr 2024 08:50:15 +0200 Subject: [PATCH 02/19] ran precommit --- .../test/test_components/test_actuator_exclusive.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hardware_interface_testing/test/test_components/test_actuator_exclusive.cpp b/hardware_interface_testing/test/test_components/test_actuator_exclusive.cpp index 2ca33150b9..e5e1a20498 100644 --- a/hardware_interface_testing/test/test_components/test_actuator_exclusive.cpp +++ b/hardware_interface_testing/test/test_components/test_actuator_exclusive.cpp @@ -51,7 +51,7 @@ class TestActuatorExclusive : public ActuatorInterface for (const auto & j : info.joints) { - (void)j; //Suppress unused warning + (void)j; // Suppress unused warning current_states_.emplace_back(JointState{}); } From 2850e98d45d932d88cde0addf3c1fb9988488597 Mon Sep 17 00:00:00 2001 From: Lennart Nachtigall Date: Fri, 19 Apr 2024 09:51:32 +0200 Subject: [PATCH 03/19] renamed into test_actuator_exclusive_interfaces --- hardware_interface_testing/CMakeLists.txt | 2 +- ...e.cpp => test_actuator_exclusive_interfaces.cpp} | 13 ++----------- .../test/test_components/test_components.xml | 2 +- 3 files changed, 4 insertions(+), 13 deletions(-) rename hardware_interface_testing/test/test_components/{test_actuator_exclusive.cpp => test_actuator_exclusive_interfaces.cpp} (92%) diff --git a/hardware_interface_testing/CMakeLists.txt b/hardware_interface_testing/CMakeLists.txt index 441327a14c..838607a239 100644 --- a/hardware_interface_testing/CMakeLists.txt +++ b/hardware_interface_testing/CMakeLists.txt @@ -24,7 +24,7 @@ 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.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 diff --git a/hardware_interface_testing/test/test_components/test_actuator_exclusive.cpp b/hardware_interface_testing/test/test_components/test_actuator_exclusive_interfaces.cpp similarity index 92% rename from hardware_interface_testing/test/test_components/test_actuator_exclusive.cpp rename to hardware_interface_testing/test/test_components/test_actuator_exclusive_interfaces.cpp index e5e1a20498..73a062eaf5 100644 --- a/hardware_interface_testing/test/test_components/test_actuator_exclusive.cpp +++ b/hardware_interface_testing/test/test_components/test_actuator_exclusive_interfaces.cpp @@ -40,7 +40,7 @@ struct JointState double effort; }; -class TestActuatorExclusive : public ActuatorInterface +class TestActuatorExclusiveInterfaces : public ActuatorInterface { CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override { @@ -169,15 +169,6 @@ class TestActuatorExclusive : public ActuatorInterface std::vector current_states_; }; -class TestUnitilizableActuatorExclusive : public TestActuatorExclusive -{ - CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override - { - ActuatorInterface::on_init(info); - return CallbackReturn::ERROR; - } -}; #include "pluginlib/class_list_macros.hpp" // NOLINT -PLUGINLIB_EXPORT_CLASS(TestActuatorExclusive, hardware_interface::ActuatorInterface) -PLUGINLIB_EXPORT_CLASS(TestUnitilizableActuatorExclusive, hardware_interface::ActuatorInterface) +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 aa307be190..fdcd0ea5ee 100644 --- a/hardware_interface_testing/test/test_components/test_components.xml +++ b/hardware_interface_testing/test/test_components/test_components.xml @@ -5,7 +5,7 @@ Test Actuator - + Test Actuator From 567d5ff00fe372302483c3e689dedb25e01ce26b Mon Sep 17 00:00:00 2001 From: Lennart Nachtigall Date: Fri, 19 Apr 2024 10:20:57 +0200 Subject: [PATCH 04/19] formatting fix --- .../test/test_components/test_actuator_exclusive_interfaces.cpp | 1 - 1 file changed, 1 deletion(-) 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 index 73a062eaf5..acc7d214c6 100644 --- a/hardware_interface_testing/test/test_components/test_actuator_exclusive_interfaces.cpp +++ b/hardware_interface_testing/test/test_components/test_actuator_exclusive_interfaces.cpp @@ -169,6 +169,5 @@ class TestActuatorExclusiveInterfaces : public ActuatorInterface std::vector current_states_; }; - #include "pluginlib/class_list_macros.hpp" // NOLINT PLUGINLIB_EXPORT_CLASS(TestActuatorExclusiveInterfaces, hardware_interface::ActuatorInterface) From 201582044edc4a8ecaa162c870c9569b181959a3 Mon Sep 17 00:00:00 2001 From: Lennart Nachtigall Date: Fri, 19 Apr 2024 10:13:23 +0200 Subject: [PATCH 05/19] added a test controller that fails at activation --- controller_manager/CMakeLists.txt | 14 ++++ .../test_controller_failed_activate.cpp | 68 +++++++++++++++++ .../test_controller_failed_activate.hpp | 76 +++++++++++++++++++ .../test_controller_failed_activate.xml | 9 +++ 4 files changed, 167 insertions(+) create mode 100644 controller_manager/test/test_controller_failed_activate/test_controller_failed_activate.cpp create mode 100644 controller_manager/test/test_controller_failed_activate/test_controller_failed_activate.hpp create mode 100644 controller_manager/test/test_controller_failed_activate/test_controller_failed_activate.xml diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 5d91604cec..fd270fea46 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -182,6 +182,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}_$ 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..c46fea53d9 --- /dev/null +++ b/controller_manager/test/test_controller_failed_activate/test_controller_failed_activate.cpp @@ -0,0 +1,68 @@ +// 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_with_interfaces + +#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..672ed1eb3b --- /dev/null +++ b/controller_manager/test/test_controller_failed_activate/test_controller_failed_activate.hpp @@ -0,0 +1,76 @@ +// 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_WITH_INTERFACES__TEST_CONTROLLER_WITH_INTERFACES_HPP_ +#define TEST_CONTROLLER_WITH_INTERFACES__TEST_CONTROLLER_WITH_INTERFACES_HPP_ + +#include +#include + +#include "controller_interface/visibility_control.h" +#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: + CONTROLLER_MANAGER_PUBLIC + TestControllerFailedActivate(); + + CONTROLLER_MANAGER_PUBLIC + 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_MANAGER_PUBLIC + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + CONTROLLER_MANAGER_PUBLIC + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_init() override; + + CONTROLLER_MANAGER_PUBLIC + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + CONTROLLER_MANAGER_PUBLIC + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + + CONTROLLER_MANAGER_PUBLIC + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State & previous_state) override; +}; + +} // namespace test_controller_with_interfaces + +#endif // TEST_CONTROLLER_WITH_INTERFACES__TEST_CONTROLLER_WITH_INTERFACES_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 + + + + From bcd7bf8a03dabfec2910fd7fa66d020caf955cf3 Mon Sep 17 00:00:00 2001 From: Lennart Nachtigall Date: Fri, 19 Apr 2024 10:19:53 +0200 Subject: [PATCH 06/19] forgot to run pre-commit --- .../test_controller_failed_activate.cpp | 6 ++---- .../test_controller_failed_activate.hpp | 3 +-- 2 files changed, 3 insertions(+), 6 deletions(-) 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 index c46fea53d9..c13df1a713 100644 --- 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 @@ -44,22 +44,20 @@ TestControllerFailedActivate::on_configure(const rclcpp_lifecycle::State & /*pre 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 + // 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_with_interfaces +} // namespace test_controller_failed_activate #include "pluginlib/class_list_macros.hpp" 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 index 672ed1eb3b..483d2b0158 100644 --- 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 @@ -65,12 +65,11 @@ class TestControllerFailedActivate : public controller_interface::ControllerInte rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; - CONTROLLER_MANAGER_PUBLIC rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup( const rclcpp_lifecycle::State & previous_state) override; }; -} // namespace test_controller_with_interfaces +} // namespace test_controller_failed_activate #endif // TEST_CONTROLLER_WITH_INTERFACES__TEST_CONTROLLER_WITH_INTERFACES_HPP_ From 6f2b3614bf4521e1344267eb1fd97ad6aea089dc Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 29 Apr 2024 22:54:28 +0200 Subject: [PATCH 07/19] Fix issues in the setup of the test controller that fails on activate --- controller_manager/CMakeLists.txt | 1 + .../test_controller_failed_activate.hpp | 6 +++--- .../test_components/test_actuator_exclusive_interfaces.cpp | 1 - 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index fd270fea46..79a259630f 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -203,6 +203,7 @@ if(BUILD_TESTING) target_link_libraries(test_release_interfaces controller_manager test_controller_with_interfaces + test_controller_failed_activate ros2_control_test_assets::ros2_control_test_assets ) 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 index 483d2b0158..d592ca271b 100644 --- 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 @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TEST_CONTROLLER_WITH_INTERFACES__TEST_CONTROLLER_WITH_INTERFACES_HPP_ -#define TEST_CONTROLLER_WITH_INTERFACES__TEST_CONTROLLER_WITH_INTERFACES_HPP_ +#ifndef TEST_CONTROLLER_FAILED_ACTIVATE__TEST_CONTROLLER_FAILED_ACTIVATE_HPP_ +#define TEST_CONTROLLER_FAILED_ACTIVATE__TEST_CONTROLLER_FAILED_ACTIVATE_HPP_ #include #include @@ -72,4 +72,4 @@ class TestControllerFailedActivate : public controller_interface::ControllerInte } // namespace test_controller_failed_activate -#endif // TEST_CONTROLLER_WITH_INTERFACES__TEST_CONTROLLER_WITH_INTERFACES_HPP_ +#endif // TEST_CONTROLLER_FAILED_ACTIVATE__TEST_CONTROLLER_FAILED_ACTIVATE_HPP_ 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 index acc7d214c6..3e524a5b73 100644 --- a/hardware_interface_testing/test/test_components/test_actuator_exclusive_interfaces.cpp +++ b/hardware_interface_testing/test/test_components/test_actuator_exclusive_interfaces.cpp @@ -147,7 +147,6 @@ class TestActuatorExclusiveInterfaces : public ActuatorInterface { const auto && [joint_name, interface_name] = extract_joint_and_interface(interface); - // TODO make sure it is not possible to claim multiple interfaces of the same axis currently_claimed_joints_.push_back(joint_name); } From 3b10a7f11206025bb90c6391ec3d1e5c030c3e32 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 29 Apr 2024 23:56:36 +0200 Subject: [PATCH 08/19] Added test to reproduce the failure of switching with exclusive interfaces --- .../test/test_release_interfaces.cpp | 80 +++++++++++++++++++ .../ros2_control_test_assets/descriptions.hpp | 53 ++++++++++++ 2 files changed, 133 insertions(+) diff --git a/controller_manager/test/test_release_interfaces.cpp b/controller_manager/test/test_release_interfaces.cpp index 4dedb47241..158110fd0e 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_failed_activate/test_controller_failed_activate.hpp" #include "test_controller_with_interfaces/test_controller_with_interfaces.hpp" using ::testing::_; @@ -29,6 +30,19 @@ class TestReleaseInterfaces : 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(TestReleaseInterfaces, switch_controllers_same_interface) { std::string controller_type = @@ -197,3 +211,69 @@ TEST_F(TestReleaseInterfaces, switch_controllers_same_interface) 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_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + abstract_test_controller2.c->get_state().id()); + + { // Test starting the first controller + 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()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + abstract_test_controller1.c->get_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, + abstract_test_controller2.c->get_state().id()); + } + + { // Test starting the second controller when the first is running + // Fails as they have the same command interface + 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_state().id()); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, + abstract_test_controller2.c->get_state().id()); + } +} 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 1744a806bc..4ab1990800 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 @@ -709,6 +709,59 @@ const auto async_hardware_resources = )"; +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 hardware_resources_with_different_rw_rates = R"( From 925cc253afa4a3b38f8c9b1ff8a4a9743653915e Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 29 Apr 2024 23:57:14 +0200 Subject: [PATCH 09/19] Throw an exception when requesting already claimed exclusive interface --- .../test_actuator_exclusive_interfaces.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) 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 index 3e524a5b73..497ca81e0b 100644 --- a/hardware_interface_testing/test/test_components/test_actuator_exclusive_interfaces.cpp +++ b/hardware_interface_testing/test/test_components/test_actuator_exclusive_interfaces.cpp @@ -17,6 +17,7 @@ #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; @@ -124,7 +125,14 @@ class TestActuatorExclusiveInterfaces : public ActuatorInterface std::find(claimed_joint_copy.begin(), claimed_joint_copy.end(), joint_name) != claimed_joint_copy.end()) { - return hardware_interface::return_type::ERROR; + 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."); } } } From deaaebf0fa440cfbe8a42ca08352f75d2c91c87a Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 30 Apr 2024 00:08:23 +0200 Subject: [PATCH 10/19] add the perform switching logic to fix the issue with exclusive interfaces --- controller_manager/src/controller_manager.cpp | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 34566f95b3..781307b391 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1844,10 +1844,27 @@ void ControllerManager::activate_controllers( { RCLCPP_ERROR( get_logger(), - "After activation, controller '%s' is in state '%s' (%d), expected '%s' (%d).", + "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(); + { + // Now prepare and perform the stop interface switching as this is needed for exclusive + // interfaces + if ( + !command_interface_names.empty() && + (!resource_manager_->prepare_command_mode_switch({}, command_interface_names) || + !resource_manager_->perform_command_mode_switch({}, command_interface_names))) + { + RCLCPP_ERROR( + get_logger(), + "Error switching back the interfaces in the hardware as the controller activation " + "failed."); + } + } + return; } } catch (const std::exception & e) From a50fd8770b74810d4ef7b4bb1ffe0af918825c41 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 2 May 2024 20:50:48 +0200 Subject: [PATCH 11/19] remove the scoping --- controller_manager/src/controller_manager.cpp | 26 +++++++++---------- 1 file changed, 12 insertions(+), 14 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 781307b391..2d44b957a0 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1850,20 +1850,18 @@ void ControllerManager::activate_controllers( hardware_interface::lifecycle_state_names::ACTIVE, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); controller->release_interfaces(); - { - // Now prepare and perform the stop interface switching as this is needed for exclusive - // interfaces - if ( - !command_interface_names.empty() && - (!resource_manager_->prepare_command_mode_switch({}, command_interface_names) || - !resource_manager_->perform_command_mode_switch({}, command_interface_names))) - { - RCLCPP_ERROR( - get_logger(), - "Error switching back the interfaces in the hardware as the controller activation " - "failed."); - } - } + // Now prepare and perform the stop interface switching as this is needed for exclusive + // interfaces + if ( + !command_interface_names.empty() && + (!resource_manager_->prepare_command_mode_switch({}, command_interface_names) || + !resource_manager_->perform_command_mode_switch({}, command_interface_names))) + { + RCLCPP_ERROR( + get_logger(), + "Error switching back the interfaces in the hardware as the controller activation " + "failed."); + } return; } } From e48bdfdab9d2d9a272cf145262263a9fdb68afae Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 13 May 2024 15:58:16 +0200 Subject: [PATCH 12/19] Collect all failed activated controllers and perform command switch at once --- controller_manager/src/controller_manager.cpp | 55 ++++++++++--------- 1 file changed, 28 insertions(+), 27 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 2d44b957a0..6a34b820f4 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1727,6 +1727,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( @@ -1835,50 +1836,38 @@ void ControllerManager::activate_controllers( } controller->assign_interfaces(std::move(command_loans), std::move(state_loans)); + auto new_state = controller->get_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). 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(); - // Now prepare and perform the stop interface switching as this is needed for exclusive - // interfaces - if ( - !command_interface_names.empty() && - (!resource_manager_->prepare_command_mode_switch({}, command_interface_names) || - !resource_manager_->perform_command_mode_switch({}, command_interface_names))) - { - RCLCPP_ERROR( - get_logger(), - "Error switching back the interfaces in the hardware as the controller activation " - "failed."); - } - return; - } + 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; } @@ -1890,6 +1879,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( From 3ba484f57b34991e75510e13d6c688e8c8e2afee Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 13 May 2024 16:13:57 +0200 Subject: [PATCH 13/19] added some documentation about the determinism --- controller_manager/doc/userdoc.rst | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index f6967930eb..eab5165343 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -383,3 +383,9 @@ 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 upto the limitations of the hardware and the real-time kernel. However, there are some following situations that can affect the 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. From 4a0980084ce52dbf323939881ed255b71928f830 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 26 Sep 2024 10:51:43 +0200 Subject: [PATCH 14/19] add naming changes from state to lifecycle_state --- controller_manager/src/controller_manager.cpp | 2 +- controller_manager/test/test_release_interfaces.cpp | 12 ++++++------ 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 6a34b820f4..17fcca89e0 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -1836,7 +1836,7 @@ void ControllerManager::activate_controllers( } controller->assign_interfaces(std::move(command_loans), std::move(state_loans)); - auto new_state = controller->get_state(); + auto new_state = controller->get_lifecycle_state(); try { found_it->periodicity_statistics->Reset(); diff --git a/controller_manager/test/test_release_interfaces.cpp b/controller_manager/test/test_release_interfaces.cpp index 158110fd0e..89e215bef8 100644 --- a/controller_manager/test/test_release_interfaces.cpp +++ b/controller_manager/test/test_release_interfaces.cpp @@ -233,10 +233,10 @@ TEST_F(TestReleaseExclusiveInterfaces, test_exclusive_interface_switching_failur ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller1.c->get_state().id()); + abstract_test_controller1.c->get_lifecycle_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller2.c->get_state().id()); + abstract_test_controller2.c->get_lifecycle_state().id()); { // Test starting the first controller RCLCPP_INFO(cm_->get_logger(), "Starting controller #1"); @@ -251,10 +251,10 @@ TEST_F(TestReleaseExclusiveInterfaces, test_exclusive_interface_switching_failur EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller1.c->get_state().id()); + abstract_test_controller1.c->get_lifecycle_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller2.c->get_state().id()); + abstract_test_controller2.c->get_lifecycle_state().id()); } { // Test starting the second controller when the first is running @@ -271,9 +271,9 @@ TEST_F(TestReleaseExclusiveInterfaces, test_exclusive_interface_switching_failur EXPECT_EQ(controller_interface::return_type::OK, switch_future.get()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, - abstract_test_controller1.c->get_state().id()); + abstract_test_controller1.c->get_lifecycle_state().id()); ASSERT_EQ( lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - abstract_test_controller2.c->get_state().id()); + abstract_test_controller2.c->get_lifecycle_state().id()); } } From 01beeeadb526195c04fef90dbb94017b8a36f9ab Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sat, 4 Jan 2025 14:06:59 +0000 Subject: [PATCH 15/19] remove copy pasta remnants from comments MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- controller_manager/test/test_release_interfaces.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/controller_manager/test/test_release_interfaces.cpp b/controller_manager/test/test_release_interfaces.cpp index 89e215bef8..e6adc9aea3 100644 --- a/controller_manager/test/test_release_interfaces.cpp +++ b/controller_manager/test/test_release_interfaces.cpp @@ -238,7 +238,9 @@ TEST_F(TestReleaseExclusiveInterfaces, test_exclusive_interface_switching_failur lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, abstract_test_controller2.c->get_lifecycle_state().id()); - { // Test starting the first controller + { + // 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 = {}; @@ -257,8 +259,9 @@ TEST_F(TestReleaseExclusiveInterfaces, test_exclusive_interface_switching_failur abstract_test_controller2.c->get_lifecycle_state().id()); } - { // Test starting the second controller when the first is running - // Fails as they have the same command interface + { + // 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 = {}; From 9f56e52117b913431ba6a3c0e9ebbe106799e004 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 6 Jan 2025 17:21:59 +0100 Subject: [PATCH 16/19] Fix pre-commit --- controller_manager/test/test_release_interfaces.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/controller_manager/test/test_release_interfaces.cpp b/controller_manager/test/test_release_interfaces.cpp index e6adc9aea3..b0bc7502c2 100644 --- a/controller_manager/test/test_release_interfaces.cpp +++ b/controller_manager/test/test_release_interfaces.cpp @@ -239,8 +239,8 @@ TEST_F(TestReleaseExclusiveInterfaces, test_exclusive_interface_switching_failur abstract_test_controller2.c->get_lifecycle_state().id()); { - // Test starting the first controller - // test_controller1 activation always fails + // 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 = {}; From 5b1771de80333699396f7f6fb32ed361b95c4f28 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 6 Jan 2025 17:22:11 +0100 Subject: [PATCH 17/19] Remove visibility macros from the test controller --- .../test_controller_failed_activate.hpp | 8 -------- 1 file changed, 8 deletions(-) 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 index d592ca271b..e2f55dcc9f 100644 --- 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 @@ -18,7 +18,6 @@ #include #include -#include "controller_interface/visibility_control.h" #include "controller_manager/controller_manager.hpp" namespace test_controller_failed_activate @@ -31,10 +30,8 @@ constexpr char TEST_CONTROLLER_COMMAND_INTERFACE[] = "joint2/velocity"; class TestControllerFailedActivate : public controller_interface::ControllerInterface { public: - CONTROLLER_MANAGER_PUBLIC TestControllerFailedActivate(); - CONTROLLER_MANAGER_PUBLIC virtual ~TestControllerFailedActivate() = default; controller_interface::InterfaceConfiguration command_interface_configuration() const override @@ -50,22 +47,17 @@ class TestControllerFailedActivate : public controller_interface::ControllerInte controller_interface::interface_configuration_type::NONE}; } - CONTROLLER_MANAGER_PUBLIC controller_interface::return_type update( const rclcpp::Time & time, const rclcpp::Duration & period) override; - CONTROLLER_MANAGER_PUBLIC rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_init() override; - CONTROLLER_MANAGER_PUBLIC rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( const rclcpp_lifecycle::State & previous_state) override; - CONTROLLER_MANAGER_PUBLIC rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate( const rclcpp_lifecycle::State & previous_state) override; - CONTROLLER_MANAGER_PUBLIC rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup( const rclcpp_lifecycle::State & previous_state) override; }; From 381e27724c81f6f301ac314fc5707cc5a4f4d61f Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 6 Jan 2025 17:42:12 +0100 Subject: [PATCH 18/19] Fix the switch return to ERROR due to recent change in return state of switch_controllers --- controller_manager/test/test_release_interfaces.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager/test/test_release_interfaces.cpp b/controller_manager/test/test_release_interfaces.cpp index b0bc7502c2..edb5ae1cf7 100644 --- a/controller_manager/test/test_release_interfaces.cpp +++ b/controller_manager/test/test_release_interfaces.cpp @@ -250,7 +250,7 @@ TEST_F(TestReleaseExclusiveInterfaces, test_exclusive_interface_switching_failur 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_INACTIVE, abstract_test_controller1.c->get_lifecycle_state().id()); From 3fa67f31b81c28f69449cf7797908571e6ffd1f0 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Mon, 6 Jan 2025 19:38:33 +0100 Subject: [PATCH 19/19] Update controller_manager/doc/userdoc.rst Co-authored-by: Bence Magyar --- controller_manager/doc/userdoc.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index eab5165343..3df55b6390 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -386,6 +386,6 @@ Likewise, if a controller returns ``return_type::ERROR`` from its ``update`` met Factors that affect Determinism ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -When run under the conditions determined in the above section, the determinism is assured upto the limitations of the hardware and the real-time kernel. However, there are some following situations that can affect the 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.