From b1c45eb2d45e991ad288d0c94ad3d20058f9e5bb Mon Sep 17 00:00:00 2001 From: "Felix Exner (fexner)" Date: Sat, 9 Nov 2024 09:01:16 +0000 Subject: [PATCH 1/4] Fix Hardware spawner and add tests for it (#1759) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Tests for hardware spawner. * Fully support spawning a list of hardware components * Use python3 coverage instead of ros2 run in tests * Actually check component's state after changing it * Update hardware_spawner's user documentation --------- Co-authored-by: Dr. Denis Štogl Co-authored-by: Bence Magyar Co-authored-by: Christoph Fröhlich (cherry picked from commit 5dbd6d55c60dd6ad8d0244dca900e9a52f5b3c25) # Conflicts: # controller_manager/CMakeLists.txt --- controller_manager/CMakeLists.txt | 15 + .../controller_manager/hardware_spawner.py | 70 ++--- controller_manager/doc/userdoc.rst | 10 +- .../test/test_hardware_spawner.cpp | 282 ++++++++++++++++++ 4 files changed, 339 insertions(+), 38 deletions(-) create mode 100644 controller_manager/test/test_hardware_spawner.cpp diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 0586dd9709..1b37d1459b 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -191,6 +191,21 @@ if(BUILD_TESTING) ros2_control_test_assets::ros2_control_test_assets ) +<<<<<<< HEAD +======= + ament_add_gmock(test_hardware_spawner + test/test_hardware_spawner + TIMEOUT 120 + ) + target_link_libraries(test_hardware_spawner + controller_manager + ros2_control_test_assets::ros2_control_test_assets + ) + + install(FILES test/test_controller_spawner_with_fallback_controllers.yaml + DESTINATION test) + +>>>>>>> 5dbd6d5 (Fix Hardware spawner and add tests for it (#1759)) install(FILES test/test_controller_spawner_with_type.yaml DESTINATION test) diff --git a/controller_manager/controller_manager/hardware_spawner.py b/controller_manager/controller_manager/hardware_spawner.py index 29c0b5e97c..323e02584a 100644 --- a/controller_manager/controller_manager/hardware_spawner.py +++ b/controller_manager/controller_manager/hardware_spawner.py @@ -57,7 +57,7 @@ def has_service_names(node, node_name, node_namespace, service_names): def is_hardware_component_loaded( node, controller_manager, hardware_component, service_timeout=0.0 ): - components = list_hardware_components(node, hardware_component, service_timeout).component + components = list_hardware_components(node, controller_manager, service_timeout).component return any(c.name == hardware_component for c in components) @@ -82,34 +82,33 @@ def handle_set_component_state_service_call( ) -def activate_components(node, controller_manager_name, components_to_activate): +def activate_component(node, controller_manager_name, component_to_activate): active_state = State() active_state.id = State.PRIMARY_STATE_ACTIVE active_state.label = "active" - for component in components_to_activate: - handle_set_component_state_service_call( - node, controller_manager_name, component, active_state, "activated" - ) + handle_set_component_state_service_call( + node, controller_manager_name, component_to_activate, active_state, "activated" + ) -def configure_components(node, controller_manager_name, components_to_configure): +def configure_component(node, controller_manager_name, component_to_configure): inactive_state = State() inactive_state.id = State.PRIMARY_STATE_INACTIVE inactive_state.label = "inactive" - for component in components_to_configure: - handle_set_component_state_service_call( - node, controller_manager_name, component, inactive_state, "configured" - ) + handle_set_component_state_service_call( + node, controller_manager_name, component_to_configure, inactive_state, "configured" + ) def main(args=None): rclpy.init(args=args, signal_handler_options=SignalHandlerOptions.NO) parser = argparse.ArgumentParser() - activate_or_confiigure_grp = parser.add_mutually_exclusive_group(required=True) + activate_or_configure_grp = parser.add_mutually_exclusive_group(required=True) parser.add_argument( - "hardware_component_name", - help="The name of the hardware component which should be activated.", + "hardware_component_names", + help="The name of the hardware components which should be activated.", + nargs="+", ) parser.add_argument( "-c", @@ -126,13 +125,13 @@ def main(args=None): type=float, ) # add arguments which are mutually exclusive - activate_or_confiigure_grp.add_argument( + activate_or_configure_grp.add_argument( "--activate", help="Activates the given components. Note: Components are by default configured before activated. ", action="store_true", required=False, ) - activate_or_confiigure_grp.add_argument( + activate_or_configure_grp.add_argument( "--configure", help="Configures the given components.", action="store_true", @@ -141,9 +140,9 @@ def main(args=None): command_line_args = rclpy.utilities.remove_ros_args(args=sys.argv)[1:] args = parser.parse_args(command_line_args) + hardware_components = args.hardware_component_names controller_manager_name = args.controller_manager controller_manager_timeout = args.controller_manager_timeout - hardware_component = [args.hardware_component_name] activate = args.activate configure = args.configure @@ -156,24 +155,25 @@ def main(args=None): controller_manager_name = f"/{controller_manager_name}" try: - if not is_hardware_component_loaded( - node, controller_manager_name, hardware_component, controller_manager_timeout - ): - node.get_logger().warn( - bcolors.WARNING - + "Hardware Component is not loaded - state can not be changed." - + bcolors.ENDC - ) - elif activate: - activate_components(node, controller_manager_name, hardware_component) - elif configure: - configure_components(node, controller_manager_name, hardware_component) - else: - node.get_logger().error( - 'You need to either specify if the hardware component should be activated with the "--activate" flag or configured with the "--configure" flag' - ) - parser.print_help() - return 0 + for hardware_component in hardware_components: + if not is_hardware_component_loaded( + node, controller_manager_name, hardware_component, controller_manager_timeout + ): + node.get_logger().warn( + bcolors.WARNING + + "Hardware Component is not loaded - state can not be changed." + + bcolors.ENDC + ) + elif activate: + activate_component(node, controller_manager_name, hardware_component) + elif configure: + configure_component(node, controller_manager_name, hardware_component) + else: + node.get_logger().error( + 'You need to either specify if the hardware component should be activated with the "--activate" flag or configured with the "--configure" flag' + ) + parser.print_help() + return 0 except KeyboardInterrupt: pass except ServiceNotFoundError as err: diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index d3bc8ba497..18ee42e9e2 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -190,16 +190,20 @@ The parsed controller config file can follow the same conventions as the typical .. code-block:: console $ ros2 run controller_manager hardware_spawner -h - usage: hardware_spawner [-h] [-c CONTROLLER_MANAGER] (--activate | --configure) hardware_component_name + usage: hardware_spawner [-h] [-c CONTROLLER_MANAGER] [--controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT] + (--activate | --configure) + hardware_component_names [hardware_component_names ...] positional arguments: - hardware_component_name - The name of the hardware component which should be activated. + hardware_component_names + The name of the hardware components which should be activated. options: -h, --help show this help message and exit -c CONTROLLER_MANAGER, --controller-manager CONTROLLER_MANAGER Name of the controller manager ROS node + --controller-manager-timeout CONTROLLER_MANAGER_TIMEOUT + Time to wait for the controller manager --activate Activates the given components. Note: Components are by default configured before activated. --configure Configures the given components. diff --git a/controller_manager/test/test_hardware_spawner.cpp b/controller_manager/test/test_hardware_spawner.cpp new file mode 100644 index 0000000000..6fd1269fa9 --- /dev/null +++ b/controller_manager/test/test_hardware_spawner.cpp @@ -0,0 +1,282 @@ +// Copyright 2021 PAL Robotics S.L. +// +// 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 +#include +#include + +#include "controller_manager/controller_manager.hpp" +#include "controller_manager_test_common.hpp" +#include "lifecycle_msgs/msg/state.hpp" +#include "test_chainable_controller/test_chainable_controller.hpp" +#include "test_controller/test_controller.hpp" + +using ::testing::_; +using ::testing::Return; + +class RMServiceCaller +{ +public: + explicit RMServiceCaller(const std::string & cm_name) + { + list_srv_node_ = std::make_shared("list_srv_client"); + srv_executor_.add_node(list_srv_node_); + list_hw_components_client_ = + list_srv_node_->create_client( + cm_name + "/list_hardware_components"); + } + + lifecycle_msgs::msg::State get_component_state(const std::string & component_name) + { + auto request = + std::make_shared(); + EXPECT_TRUE(list_hw_components_client_->wait_for_service(std::chrono::milliseconds(500))); + auto future = list_hw_components_client_->async_send_request(request); + EXPECT_EQ(srv_executor_.spin_until_future_complete(future), rclcpp::FutureReturnCode::SUCCESS); + auto result = future.get(); + + auto it = std::find_if( + std::begin(result->component), std::end(result->component), + [&component_name](const auto & cmp) { return cmp.name == component_name; }); + + EXPECT_NE(it, std::end(result->component)); + + return it->state; + }; + +protected: + rclcpp::executors::SingleThreadedExecutor srv_executor_; + rclcpp::Node::SharedPtr list_srv_node_; + rclcpp::Client::SharedPtr + list_hw_components_client_; +}; + +using namespace std::chrono_literals; +class TestHardwareSpawner : public ControllerManagerFixture, + public RMServiceCaller +{ +public: + TestHardwareSpawner() + : ControllerManagerFixture(), RMServiceCaller(TEST_CM_NAME) + { + cm_->set_parameter( + rclcpp::Parameter("hardware_components_initial_state.unconfigured", "TestSystemHardware")); + } + + void SetUp() override + { + ControllerManagerFixture::SetUp(); + + update_executor_ = + std::make_shared(rclcpp::ExecutorOptions(), 2); + + update_executor_->add_node(cm_); + update_executor_spin_future_ = + std::async(std::launch::async, [this]() -> void { update_executor_->spin(); }); + // This sleep is needed to prevent a too fast test from ending before the + // executor has began to spin, which causes it to hang + std::this_thread::sleep_for(50ms); + } + + void TearDown() override { update_executor_->cancel(); } + +protected: + // Using a MultiThreadedExecutor so we can call update on a separate thread from service callbacks + std::shared_ptr update_executor_; + std::future update_executor_spin_future_; +}; + +int call_spawner(const std::string extra_args) +{ + std::string spawner_script = + "python3 -m coverage run --append --branch $(ros2 pkg prefix " + "controller_manager)/lib/controller_manager/hardware_spawner "; + return std::system((spawner_script + extra_args).c_str()); +} + +TEST_F(TestHardwareSpawner, spawner_with_no_arguments_errors) +{ + EXPECT_NE(call_spawner(""), 0) << "Missing mandatory arguments"; +} + +TEST_F(TestHardwareSpawner, spawner_without_manager_errors_with_given_timeout) +{ + EXPECT_NE(call_spawner("TestSystemHardware --controller-manager-timeout 1.0"), 0) + << "Wrong controller manager name"; +} + +TEST_F(TestHardwareSpawner, spawner_without_component_name_argument) +{ + EXPECT_NE(call_spawner("-c test_controller_manager"), 0) + << "Missing component name argument parameter"; +} + +TEST_F(TestHardwareSpawner, spawner_non_exising_hardware_component) +{ + EXPECT_NE(call_spawner("TestSystemHardware1 -c test_controller_manager"), 0) + << "Missing component name argument parameter"; +} + +TEST_F(TestHardwareSpawner, set_component_to_configured_state_and_back_to_activated) +{ + EXPECT_EQ(call_spawner("TestSystemHardware --configure -c test_controller_manager"), 0); + EXPECT_EQ( + get_component_state("TestSystemHardware").id, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + + EXPECT_EQ(call_spawner("TestSystemHardware --activate -c test_controller_manager"), 0); + EXPECT_EQ( + get_component_state("TestSystemHardware").id, lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); +} + +class TestHardwareSpawnerWithoutRobotDescription +: public ControllerManagerFixture, + public RMServiceCaller +{ +public: + TestHardwareSpawnerWithoutRobotDescription() + : ControllerManagerFixture(""), + RMServiceCaller(TEST_CM_NAME) + { + cm_->set_parameter(rclcpp::Parameter( + "hardware_components_initial_state.unconfigured", + std::vector{"TestSystemHardware"})); + } + +public: + void SetUp() override + { + ControllerManagerFixture::SetUp(); + + update_timer_ = cm_->create_wall_timer( + std::chrono::milliseconds(10), + [&]() + { + cm_->read(time_, PERIOD); + cm_->update(time_, PERIOD); + cm_->write(time_, PERIOD); + }); + + update_executor_ = + std::make_shared(rclcpp::ExecutorOptions(), 2); + + update_executor_->add_node(cm_); + update_executor_spin_future_ = + std::async(std::launch::async, [this]() -> void { update_executor_->spin(); }); + // This sleep is needed to prevent a too fast test from ending before the + // executor has began to spin, which causes it to hang + std::this_thread::sleep_for(50ms); + } + + void TearDown() override { update_executor_->cancel(); } + + rclcpp::TimerBase::SharedPtr robot_description_sending_timer_; + +protected: + rclcpp::TimerBase::SharedPtr update_timer_; + + // Using a MultiThreadedExecutor so we can call update on a separate thread from service callbacks + std::shared_ptr update_executor_; + std::future update_executor_spin_future_; +}; + +TEST_F(TestHardwareSpawnerWithoutRobotDescription, when_no_robot_description_spawner_times_out) +{ + EXPECT_EQ( + call_spawner( + "TestSystemHardware --configure -c test_controller_manager --controller-manager-timeout 1.0"), + 256) + << "could not change hardware state because not robot description and not services for " + "controller " + "manager are active"; +} + +TEST_F(TestHardwareSpawnerWithoutRobotDescription, spawner_with_later_load_of_robot_description) +{ + // Delay sending robot description + robot_description_sending_timer_ = cm_->create_wall_timer( + std::chrono::milliseconds(2500), [&]() { pass_robot_description_to_cm_and_rm(); }); + + EXPECT_EQ( + call_spawner( + "TestSystemHardware --configure -c test_controller_manager --controller-manager-timeout 1.0"), + 256) + << "could not activate control because not robot description"; + EXPECT_EQ( + call_spawner( + "TestSystemHardware --configure -c test_controller_manager --controller-manager-timeout 2.5"), + 0); + EXPECT_EQ( + get_component_state("TestSystemHardware").id, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); +} + +class TestHardwareSpawnerWithNamespacedCM +: public ControllerManagerFixture, + public RMServiceCaller +{ +public: + TestHardwareSpawnerWithNamespacedCM() + : ControllerManagerFixture( + ros2_control_test_assets::minimal_robot_urdf, "foo_namespace"), + RMServiceCaller("foo_namespace/" + std::string(TEST_CM_NAME)) + { + cm_->set_parameter( + rclcpp::Parameter("hardware_components_initial_state.unconfigured", "TestSystemHardware")); + } + +public: + void SetUp() override + { + ControllerManagerFixture::SetUp(); + + update_executor_ = + std::make_shared(rclcpp::ExecutorOptions(), 2); + + update_executor_->add_node(cm_); + update_executor_spin_future_ = + std::async(std::launch::async, [this]() -> void { update_executor_->spin(); }); + // This sleep is needed to prevent a too fast test from ending before the + // executor has began to spin, which causes it to hang + std::this_thread::sleep_for(50ms); + } + + void TearDown() override { update_executor_->cancel(); } + +protected: + // Using a MultiThreadedExecutor so we can call update on a separate thread from service callbacks + std::shared_ptr update_executor_; + std::future update_executor_spin_future_; +}; + +TEST_F(TestHardwareSpawnerWithNamespacedCM, set_component_to_configured_state_cm_namespace) +{ + ControllerManagerRunner cm_runner(this); + EXPECT_EQ( + call_spawner( + "TestSystemHardware --configure -c test_controller_manager --controller-manager-timeout 1.0"), + 256) + << "Should fail without defining the namespace"; + EXPECT_EQ( + call_spawner("TestSystemHardware --configure -c test_controller_manager --ros-args -r " + "__ns:=/foo_namespace"), + 0); + + EXPECT_EQ( + get_component_state("TestSystemHardware").id, + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); +} From 255da487b1a5f9d6b969c130b7995f4f6317331c Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Sat, 9 Nov 2024 09:01:16 +0000 Subject: [PATCH 2/4] Fix conflict --- controller_manager/CMakeLists.txt | 6 ------ 1 file changed, 6 deletions(-) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 1b37d1459b..47a462e13d 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -191,8 +191,6 @@ if(BUILD_TESTING) ros2_control_test_assets::ros2_control_test_assets ) -<<<<<<< HEAD -======= ament_add_gmock(test_hardware_spawner test/test_hardware_spawner TIMEOUT 120 @@ -202,10 +200,6 @@ if(BUILD_TESTING) ros2_control_test_assets::ros2_control_test_assets ) - install(FILES test/test_controller_spawner_with_fallback_controllers.yaml - DESTINATION test) - ->>>>>>> 5dbd6d5 (Fix Hardware spawner and add tests for it (#1759)) install(FILES test/test_controller_spawner_with_type.yaml DESTINATION test) From 282dfddfbed10e2d3c7e993343bcaadd197f4225 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sat, 9 Nov 2024 09:01:16 +0000 Subject: [PATCH 3/4] Fix the missing bcolors.ENDC in hardware_spawner log prints (#1870) --- .../controller_manager/hardware_spawner.py | 17 ++++++----------- 1 file changed, 6 insertions(+), 11 deletions(-) diff --git a/controller_manager/controller_manager/hardware_spawner.py b/controller_manager/controller_manager/hardware_spawner.py index 323e02584a..4f7afe714c 100644 --- a/controller_manager/controller_manager/hardware_spawner.py +++ b/controller_manager/controller_manager/hardware_spawner.py @@ -67,18 +67,15 @@ def handle_set_component_state_service_call( response = set_hardware_component_state(node, controller_manager_name, component, target_state) if response.ok and response.state == target_state: node.get_logger().info( - bcolors.OKGREEN - + f"{action} component '{component}'. Hardware now in state: {response.state}." + f"{bcolors.OKGREEN}{action} component '{component}'. Hardware now in state: {response.state}.{bcolors.ENDC}" ) elif response.ok and not response.state == target_state: node.get_logger().warn( - bcolors.WARNING - + f"Could not {action} component '{component}'. Service call returned ok=True, but state: {response.state} is not equal to target state '{target_state}'." + f"{bcolors.WARNING}Could not {action} component '{component}'. Service call returned ok=True, but state: {response.state} is not equal to target state '{target_state}'.{bcolors.ENDC}" ) else: node.get_logger().warn( - bcolors.WARNING - + f"Could not {action} component '{component}'. Service call failed. Wrong component name?" + f"{bcolors.WARNING}Could not {action} component '{component}'. Service call failed. Wrong component name?{bcolors.ENDC}" ) @@ -160,9 +157,7 @@ def main(args=None): node, controller_manager_name, hardware_component, controller_manager_timeout ): node.get_logger().warn( - bcolors.WARNING - + "Hardware Component is not loaded - state can not be changed." - + bcolors.ENDC + f"{bcolors.WARNING}Hardware Component is not loaded - state can not be changed.{bcolors.ENDC}" ) elif activate: activate_component(node, controller_manager_name, hardware_component) @@ -170,14 +165,14 @@ def main(args=None): configure_component(node, controller_manager_name, hardware_component) else: node.get_logger().error( - 'You need to either specify if the hardware component should be activated with the "--activate" flag or configured with the "--configure" flag' + f'{bcolors.FAIL}You need to either specify if the hardware component should be activated with the "--activate" flag or configured with the "--configure" flag{bcolors.ENDC}' ) parser.print_help() return 0 except KeyboardInterrupt: pass except ServiceNotFoundError as err: - node.get_logger().fatal(str(err)) + node.get_logger().fatal(f"{bcolors.FAIL}{str(err)}{bcolors.ENDC}") return 1 finally: rclpy.shutdown() From db456b9aa6ab09d62e5835694a3ec0bf9f88b1d4 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 10 Nov 2024 01:17:17 +0100 Subject: [PATCH 4/4] Choose pass_urdf_as_parameter as false --- controller_manager/test/test_hardware_spawner.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager/test/test_hardware_spawner.cpp b/controller_manager/test/test_hardware_spawner.cpp index 6fd1269fa9..b9f3027602 100644 --- a/controller_manager/test/test_hardware_spawner.cpp +++ b/controller_manager/test/test_hardware_spawner.cpp @@ -232,7 +232,7 @@ class TestHardwareSpawnerWithNamespacedCM public: TestHardwareSpawnerWithNamespacedCM() : ControllerManagerFixture( - ros2_control_test_assets::minimal_robot_urdf, "foo_namespace"), + ros2_control_test_assets::minimal_robot_urdf, false, "foo_namespace"), RMServiceCaller("foo_namespace/" + std::string(TEST_CM_NAME)) { cm_->set_parameter(