From b6807b29c6613b083c61075a37cd3fb31cb20a2b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Thu, 15 Aug 2024 14:09:45 +0200 Subject: [PATCH 1/8] Tests for hardware spawner. --- controller_manager/CMakeLists.txt | 9 + .../controller_manager/hardware_spawner.py | 14 +- .../test/test_hardware_spawner.cpp | 210 ++++++++++++++++++ 3 files changed, 228 insertions(+), 5 deletions(-) create mode 100644 controller_manager/test/test_hardware_spawner.cpp diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 5dea15c0d1..a9140d8fbe 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -193,6 +193,15 @@ if(BUILD_TESTING) ros2_control_test_assets::ros2_control_test_assets ) + 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) diff --git a/controller_manager/controller_manager/hardware_spawner.py b/controller_manager/controller_manager/hardware_spawner.py index 29c0b5e97c..a981c8f054 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) @@ -105,11 +105,12 @@ def configure_components(node, controller_manager_name, components_to_configure) 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.", + nargs="+" ) parser.add_argument( "-c", @@ -126,13 +127,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,12 +142,15 @@ def main(args=None): command_line_args = rclpy.utilities.remove_ros_args(args=sys.argv)[1:] args = parser.parse_args(command_line_args) + hardware_component = args.hardware_component_name 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 + print(f"CMD Arguments: {command_line_args}") + print(f"Arguments: {args}") + node = Node("hardware_spawner") if not controller_manager_name.startswith("/"): spawner_namespace = node.get_namespace() diff --git a/controller_manager/test/test_hardware_spawner.cpp b/controller_manager/test/test_hardware_spawner.cpp new file mode 100644 index 0000000000..8957cde5bf --- /dev/null +++ b/controller_manager/test/test_hardware_spawner.cpp @@ -0,0 +1,210 @@ +// 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; + +using namespace std::chrono_literals; +class TestHardwareSpawner : public ControllerManagerFixture +{ +public: + TestHardwareSpawner() + : ControllerManagerFixture() + { + 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 = "ros2 run 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(call_spawner("TestSystemHardware --activate -c test_controller_manager"), 0); +} + + +class TestHardwareSpawnerWithoutRobotDescription +: public ControllerManagerFixture +{ +public: + TestHardwareSpawnerWithoutRobotDescription() + : ControllerManagerFixture("") + { + cm_->set_parameter(rclcpp::Parameter("hardware_components_initial_state.unconfigured", "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"), 1) + << "could not activate control because not robot description"; +} + +class TestHardwareSpawnerWithNamespacedCM +: public ControllerManagerFixture +{ +public: + TestHardwareSpawnerWithNamespacedCM() + : ControllerManagerFixture( + ros2_control_test_assets::minimal_robot_urdf, "foo_namespace") + { + 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); +} From fb0547b573e26a317ae7d07d50dd98371afeecb4 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 12 Sep 2024 11:32:29 +0200 Subject: [PATCH 2/8] Fully support spawning a list of hardware components --- .../controller_manager/hardware_spawner.py | 63 +++++++++---------- 1 file changed, 31 insertions(+), 32 deletions(-) diff --git a/controller_manager/controller_manager/hardware_spawner.py b/controller_manager/controller_manager/hardware_spawner.py index a981c8f054..1c52025989 100644 --- a/controller_manager/controller_manager/hardware_spawner.py +++ b/controller_manager/controller_manager/hardware_spawner.py @@ -82,24 +82,22 @@ 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): @@ -108,9 +106,9 @@ def main(args=None): 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.", - nargs="+" + "hardware_component_names", + help="The name of the hardware components which should be activated.", + nargs="+", ) parser.add_argument( "-c", @@ -142,7 +140,7 @@ def main(args=None): command_line_args = rclpy.utilities.remove_ros_args(args=sys.argv)[1:] args = parser.parse_args(command_line_args) - hardware_component = args.hardware_component_name + hardware_components = args.hardware_component_names controller_manager_name = args.controller_manager controller_manager_timeout = args.controller_manager_timeout activate = args.activate @@ -160,24 +158,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: From 0d548af47cbc259011680c21ceab4ee2c51f7782 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 12 Sep 2024 13:02:25 +0200 Subject: [PATCH 3/8] Make tests work --- .../test/test_hardware_spawner.cpp | 63 ++++++++++++------- 1 file changed, 39 insertions(+), 24 deletions(-) diff --git a/controller_manager/test/test_hardware_spawner.cpp b/controller_manager/test/test_hardware_spawner.cpp index 8957cde5bf..c8ce8e8ce2 100644 --- a/controller_manager/test/test_hardware_spawner.cpp +++ b/controller_manager/test/test_hardware_spawner.cpp @@ -32,10 +32,10 @@ using namespace std::chrono_literals; class TestHardwareSpawner : public ControllerManagerFixture { public: - TestHardwareSpawner() - : ControllerManagerFixture() + TestHardwareSpawner() : ControllerManagerFixture() { - cm_->set_parameter(rclcpp::Parameter("hardware_components_initial_state.unconfigured", "TestSystemHardware")); + cm_->set_parameter( + rclcpp::Parameter("hardware_components_initial_state.unconfigured", "TestSystemHardware")); } void SetUp() override @@ -43,11 +43,11 @@ class TestHardwareSpawner : public ControllerManagerFixture(rclcpp::ExecutorOptions(), 2); + 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(); }); + 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); @@ -80,14 +80,14 @@ TEST_F(TestHardwareSpawner, spawner_without_manager_errors_with_given_timeout) TEST_F(TestHardwareSpawner, spawner_without_component_name_argument) { - EXPECT_NE(call_spawner("-c test_controller_manager"), 0) << - "Missing component name argument parameter"; + 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"; + 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) @@ -97,7 +97,6 @@ TEST_F(TestHardwareSpawner, set_component_to_configured_state_and_back_to_activa EXPECT_EQ(call_spawner("TestSystemHardware --activate -c test_controller_manager"), 0); } - class TestHardwareSpawnerWithoutRobotDescription : public ControllerManagerFixture { @@ -105,7 +104,9 @@ class TestHardwareSpawnerWithoutRobotDescription TestHardwareSpawnerWithoutRobotDescription() : ControllerManagerFixture("") { - cm_->set_parameter(rclcpp::Parameter("hardware_components_initial_state.unconfigured", "TestSystemHardware")); + cm_->set_parameter(rclcpp::Parameter( + "hardware_components_initial_state.unconfigured", + std::vector{"TestSystemHardware"})); } public: @@ -123,11 +124,11 @@ class TestHardwareSpawnerWithoutRobotDescription }); update_executor_ = - std::make_shared(rclcpp::ExecutorOptions(), 2); + 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(); }); + 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); @@ -147,21 +148,30 @@ class TestHardwareSpawnerWithoutRobotDescription 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 " + 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) +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"), 1) + 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); } class TestHardwareSpawnerWithNamespacedCM @@ -172,7 +182,8 @@ class TestHardwareSpawnerWithNamespacedCM : ControllerManagerFixture( ros2_control_test_assets::minimal_robot_urdf, "foo_namespace") { - cm_->set_parameter(rclcpp::Parameter("hardware_components_initial_state.unconfigured", "TestSystemHardware")); + cm_->set_parameter( + rclcpp::Parameter("hardware_components_initial_state.unconfigured", "TestSystemHardware")); } public: @@ -181,11 +192,11 @@ class TestHardwareSpawnerWithNamespacedCM ControllerManagerFixture::SetUp(); update_executor_ = - std::make_shared(rclcpp::ExecutorOptions(), 2); + 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(); }); + 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); @@ -203,8 +214,12 @@ TEST_F(TestHardwareSpawnerWithNamespacedCM, set_component_to_configured_state_cm { ControllerManagerRunner cm_runner(this); EXPECT_EQ( - call_spawner("TestSystemHardware --configure -c test_controller_manager --controller-manager-timeout 1.0"), 256) + 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); + call_spawner("TestSystemHardware --configure -c test_controller_manager --ros-args -r " + "__ns:=/foo_namespace"), + 0); } From a8f9c6254c2465932810c271183fd49c808908cf Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 12 Sep 2024 13:07:42 +0200 Subject: [PATCH 4/8] Remove temporary debug output --- controller_manager/controller_manager/hardware_spawner.py | 3 --- 1 file changed, 3 deletions(-) diff --git a/controller_manager/controller_manager/hardware_spawner.py b/controller_manager/controller_manager/hardware_spawner.py index 1c52025989..323e02584a 100644 --- a/controller_manager/controller_manager/hardware_spawner.py +++ b/controller_manager/controller_manager/hardware_spawner.py @@ -146,9 +146,6 @@ def main(args=None): activate = args.activate configure = args.configure - print(f"CMD Arguments: {command_line_args}") - print(f"Arguments: {args}") - node = Node("hardware_spawner") if not controller_manager_name.startswith("/"): spawner_namespace = node.get_namespace() From ad8cfa58a72060a92e891a7184ba6f195872b2e9 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 12 Sep 2024 13:08:18 +0200 Subject: [PATCH 5/8] Use python3 coverage instead of ros2 run in tests This way we should get coverage --- controller_manager/test/test_hardware_spawner.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/controller_manager/test/test_hardware_spawner.cpp b/controller_manager/test/test_hardware_spawner.cpp index c8ce8e8ce2..f7e5aa52b6 100644 --- a/controller_manager/test/test_hardware_spawner.cpp +++ b/controller_manager/test/test_hardware_spawner.cpp @@ -63,7 +63,9 @@ class TestHardwareSpawner : public ControllerManagerFixture Date: Tue, 24 Sep 2024 19:51:16 +0000 Subject: [PATCH 6/8] Actually check component's state after changing it --- .../test/test_hardware_spawner.cpp | 66 +++++++++++++++++-- 1 file changed, 60 insertions(+), 6 deletions(-) diff --git a/controller_manager/test/test_hardware_spawner.cpp b/controller_manager/test/test_hardware_spawner.cpp index f7e5aa52b6..ac68902038 100644 --- a/controller_manager/test/test_hardware_spawner.cpp +++ b/controller_manager/test/test_hardware_spawner.cpp @@ -28,11 +28,51 @@ using ::testing::_; using ::testing::Return; +class RMServiceCaller +{ +public: + 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 +class TestHardwareSpawner : public ControllerManagerFixture, + public RMServiceCaller { public: - TestHardwareSpawner() : ControllerManagerFixture() + TestHardwareSpawner() + : ControllerManagerFixture(), + RMServiceCaller(TEST_CM_NAME) { cm_->set_parameter( rclcpp::Parameter("hardware_components_initial_state.unconfigured", "TestSystemHardware")); @@ -95,16 +135,21 @@ TEST_F(TestHardwareSpawner, spawner_non_exising_hardware_component) 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 ControllerManagerFixture, public RMServiceCaller { public: TestHardwareSpawnerWithoutRobotDescription() - : ControllerManagerFixture("") + : ControllerManagerFixture(""), RMServiceCaller(TEST_CM_NAME) { cm_->set_parameter(rclcpp::Parameter( "hardware_components_initial_state.unconfigured", @@ -174,15 +219,20 @@ TEST_F(TestHardwareSpawnerWithoutRobotDescription, spawner_with_later_load_of_ro 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 ControllerManagerFixture, + public RMServiceCaller { public: TestHardwareSpawnerWithNamespacedCM() : ControllerManagerFixture( - ros2_control_test_assets::minimal_robot_urdf, "foo_namespace") + 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")); @@ -224,4 +274,8 @@ TEST_F(TestHardwareSpawnerWithNamespacedCM, set_component_to_configured_state_cm 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 d381ca504924ecd6003a8a64af0d18aeed37ce3c Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 24 Sep 2024 22:20:08 +0200 Subject: [PATCH 7/8] Update hardware_spawner's user documentation --- controller_manager/doc/userdoc.rst | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/controller_manager/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index bc9f75384e..72ce9ca3d2 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -193,16 +193,20 @@ There are two scripts to interact with controller manager from launch files: .. 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. From ccc1cc7bb103f1e82997417121ca49064c28bf6d Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 24 Sep 2024 22:24:20 +0200 Subject: [PATCH 8/8] Code formatting --- controller_manager/test/test_hardware_spawner.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/controller_manager/test/test_hardware_spawner.cpp b/controller_manager/test/test_hardware_spawner.cpp index ac68902038..6fd1269fa9 100644 --- a/controller_manager/test/test_hardware_spawner.cpp +++ b/controller_manager/test/test_hardware_spawner.cpp @@ -31,7 +31,7 @@ using ::testing::Return; class RMServiceCaller { public: - RMServiceCaller(const std::string & cm_name) + explicit RMServiceCaller(const std::string & cm_name) { list_srv_node_ = std::make_shared("list_srv_client"); srv_executor_.add_node(list_srv_node_); @@ -71,8 +71,7 @@ class TestHardwareSpawner : public ControllerManagerFixture(), - RMServiceCaller(TEST_CM_NAME) + : ControllerManagerFixture(), RMServiceCaller(TEST_CM_NAME) { cm_->set_parameter( rclcpp::Parameter("hardware_components_initial_state.unconfigured", "TestSystemHardware")); @@ -145,11 +144,13 @@ TEST_F(TestHardwareSpawner, set_component_to_configured_state_and_back_to_activa } class TestHardwareSpawnerWithoutRobotDescription -: public ControllerManagerFixture, public RMServiceCaller +: public ControllerManagerFixture, + public RMServiceCaller { public: TestHardwareSpawnerWithoutRobotDescription() - : ControllerManagerFixture(""), RMServiceCaller(TEST_CM_NAME) + : ControllerManagerFixture(""), + RMServiceCaller(TEST_CM_NAME) { cm_->set_parameter(rclcpp::Parameter( "hardware_components_initial_state.unconfigured",