From 9b96aa2cbba71dad9d557a1a3105966141bdc2e6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Thu, 15 Aug 2024 19:33:26 +0200 Subject: [PATCH] Test fixes. --- controller_manager/CMakeLists.txt | 4 + .../controller_manager/spawner.py | 27 ++++- .../test/controller_manager_test_common.hpp | 27 +++-- .../test_controller_spawner_with_type.yaml | 27 +++++ .../test/test_spawner_unspawner.cpp | 101 +----------------- 5 files changed, 79 insertions(+), 107 deletions(-) create mode 100644 controller_manager/test/test_controller_spawner_with_type.yaml diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 29d489ad81..543daeae29 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -169,11 +169,15 @@ if(BUILD_TESTING) ament_add_gmock( test_spawner_unspawner test/test_spawner_unspawner.cpp + TIMEOUT 120 ) target_include_directories(test_spawner_unspawner PRIVATE include) target_link_libraries(test_spawner_unspawner ${PROJECT_NAME} test_controller) ament_target_dependencies(test_spawner_unspawner ros2_control_test_assets) + install(FILES test/test_controller_spawner_with_type.yaml + DESTINATION test) + ament_add_gmock( test_hardware_management_srvs test/test_hardware_management_srvs.cpp diff --git a/controller_manager/controller_manager/spawner.py b/controller_manager/controller_manager/spawner.py index 6496a821d2..12574fe29a 100644 --- a/controller_manager/controller_manager/spawner.py +++ b/controller_manager/controller_manager/spawner.py @@ -19,6 +19,7 @@ import sys import time import warnings +import yaml from controller_manager import ( configure_controller, @@ -79,6 +80,24 @@ def is_controller_loaded(node, controller_manager, controller_name, service_time return any(c.name == controller_name for c in controllers) +def get_parameter_from_param_file(controller_name, namespace, parameter_file, parameter_name): + with open(parameter_file) as f: + namespaced_controller = ( + controller_name if namespace == "/" else f"{namespace}/{controller_name}" + ) + parameters = yaml.safe_load(f) + if namespaced_controller in parameters: + value = parameters[namespaced_controller] + if not isinstance(value, dict) or "ros__parameters" not in value: + raise RuntimeError( + f"YAML file : {parameter_file} is not a valid ROS parameter file for controller : {namespaced_controller}" + ) + if parameter_name in parameters[namespaced_controller]["ros__parameters"]: + return parameters[namespaced_controller]["ros__parameters"][parameter_name] + else: + return None + + def main(args=None): rclpy.init(args=args, signal_handler_options=SignalHandlerOptions.NO) @@ -152,7 +171,6 @@ def main(args=None): controller_manager_name = args.controller_manager controller_namespace = args.namespace param_file = args.param_file - controller_type = args.controller_type controller_manager_timeout = args.controller_manager_timeout if param_file and not os.path.isfile(param_file): @@ -182,6 +200,13 @@ def main(args=None): + bcolors.ENDC ) else: + controller_type = ( + None + if param_file is None + else get_parameter_from_param_file( + controller_name, spawner_namespace, param_file, "type" + ) + ) if controller_type: parameter = Parameter() parameter.name = prefixed_controller_name + ".type" diff --git a/controller_manager/test/controller_manager_test_common.hpp b/controller_manager/test/controller_manager_test_common.hpp index 5e6e02029f..570190e81c 100644 --- a/controller_manager/test/controller_manager_test_common.hpp +++ b/controller_manager/test/controller_manager_test_common.hpp @@ -67,7 +67,8 @@ class ControllerManagerFixture : public ::testing::Test public: explicit ControllerManagerFixture( const std::string & robot_description = ros2_control_test_assets::minimal_robot_urdf, - const bool & pass_urdf_as_parameter = false) + const bool & pass_urdf_as_parameter = false, + const std::string & cm_namespace = "") : robot_description_(robot_description), pass_urdf_as_parameter_(pass_urdf_as_parameter) { executor_ = std::make_shared(); @@ -75,7 +76,8 @@ class ControllerManagerFixture : public ::testing::Test if (robot_description_.empty()) { cm_ = std::make_shared( - std::make_unique(), executor_, TEST_CM_NAME); + std::make_unique(), executor_, TEST_CM_NAME, + cm_namespace); } else { @@ -85,7 +87,7 @@ class ControllerManagerFixture : public ::testing::Test { cm_ = std::make_shared( std::make_unique(robot_description_, true, true), - executor_, TEST_CM_NAME); + executor_, TEST_CM_NAME, cm_namespace); } else { @@ -94,11 +96,10 @@ class ControllerManagerFixture : public ::testing::Test // this is just a workaround to skip passing cm_ = std::make_shared( - std::make_unique(), executor_, TEST_CM_NAME); + std::make_unique(), executor_, TEST_CM_NAME, + cm_namespace); // mimic topic call - auto msg = std_msgs::msg::String(); - msg.data = robot_description_; - cm_->robot_description_callback(msg); + pass_robot_description_to_cm_and_rm(robot_description_); } } } @@ -134,6 +135,17 @@ class ControllerManagerFixture : public ::testing::Test } } + void pass_robot_description_to_cm_and_rm( + const std::string & robot_description = ros2_control_test_assets::minimal_robot_urdf) + { + // TODO(Manuel) : passing via topic not working in test setup, tested cm does + // not receive msg. Have to check this... + // this is just a workaround to skip passing - mimic topic call + auto msg = std_msgs::msg::String(); + msg.data = robot_description; + cm_->robot_description_callback(msg); + } + void switch_test_controllers( const std::vector & start_controllers, const std::vector & stop_controllers, const int strictness, @@ -157,6 +169,7 @@ class ControllerManagerFixture : public ::testing::Test std::thread updater_; bool run_updater_; const std::string robot_description_; + rclcpp::Time time_; const bool pass_urdf_as_parameter_; }; diff --git a/controller_manager/test/test_controller_spawner_with_type.yaml b/controller_manager/test/test_controller_spawner_with_type.yaml new file mode 100644 index 0000000000..892427bab7 --- /dev/null +++ b/controller_manager/test/test_controller_spawner_with_type.yaml @@ -0,0 +1,27 @@ +ctrl_with_parameters_and_type: + ros__parameters: + type: "controller_manager/test_controller" + joint_names: ["joint0"] + +chainable_ctrl_with_parameters_and_type: + ros__parameters: + type: "controller_manager/test_chainable_controller" + joint_names: ["joint1"] + +ctrl_with_parameters_and_no_type: + ros__parameters: + joint_names: ["joint2"] + +/foo_namespace/ctrl_with_parameters_and_type: + ros__parameters: + type: "controller_manager/test_controller" + joint_names: ["joint1"] + +/foo_namespace/chainable_ctrl_with_parameters_and_type: + ros__parameters: + type: "controller_manager/test_chainable_controller" + joint_names: ["joint1"] + +/foo_namespace/ctrl_with_parameters_and_no_type: + ros__parameters: + joint_names: ["joint2"] diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 1eeb489663..017e98937c 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -23,6 +23,7 @@ #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::_; @@ -222,51 +223,11 @@ TEST_F(TestLoadController, spawner_test_type_in_arg) std::string(test_controller::TEST_CONTROLLER_CLASS_NAME)), 0); -<<<<<<< HEAD ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul); auto ctrl_2 = cm_->get_loaded_controllers()[0]; ASSERT_EQ(ctrl_2.info.name, "ctrl_2"); ASSERT_EQ(ctrl_2.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); -======= - ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); - - auto ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[0]; - ASSERT_EQ(ctrl_with_parameters_and_type.info.name, "ctrl_with_parameters_and_type"); - ASSERT_EQ(ctrl_with_parameters_and_type.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_EQ( - ctrl_with_parameters_and_type.c->get_state().id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); - - auto chain_ctrl_with_parameters_and_type = cm_->get_loaded_controllers()[1]; - ASSERT_EQ( - chain_ctrl_with_parameters_and_type.info.name, "chainable_ctrl_with_parameters_and_type"); - ASSERT_EQ( - chain_ctrl_with_parameters_and_type.info.type, - test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_EQ( - chain_ctrl_with_parameters_and_type.c->get_state().id(), - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); - - EXPECT_EQ( - call_spawner( - "ctrl_with_parameters_and_no_type -c test_controller_manager --controller-manager-timeout " - "1.0 -p " + - test_file_path), - 256); - // Will still be same as the current call will fail - ASSERT_EQ(cm_->get_loaded_controllers().size(), 2ul); - - auto ctrl_1 = cm_->get_loaded_controllers()[0]; - ASSERT_EQ(ctrl_1.info.name, "ctrl_with_parameters_and_type"); - ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); - - auto ctrl_2 = cm_->get_loaded_controllers()[1]; - ASSERT_EQ(ctrl_2.info.name, "chainable_ctrl_with_parameters_and_type"); - ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); ->>>>>>> af4b48f (Handle waiting in Spawner and align Hardware Spawner functionality (#1562)) } TEST_F(TestLoadController, unload_on_kill) @@ -284,63 +245,6 @@ TEST_F(TestLoadController, unload_on_kill) ASSERT_EQ(cm_->get_loaded_controllers().size(), 0ul); } -<<<<<<< HEAD -======= - -TEST_F(TestLoadController, spawner_test_fallback_controllers) -{ - const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + - "/test/test_controller_spawner_with_fallback_controllers.yaml"; - - cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); - cm_->set_parameter(rclcpp::Parameter("ctrl_2.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); - cm_->set_parameter(rclcpp::Parameter("ctrl_3.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); - - ControllerManagerRunner cm_runner(this); - EXPECT_EQ( - call_spawner( - "ctrl_1 -c test_controller_manager --load-only --fallback_controllers ctrl_3 ctrl_4 ctrl_5 " - "-p " + - test_file_path), - 0); - - ASSERT_EQ(cm_->get_loaded_controllers().size(), 1ul); - { - auto ctrl_1 = cm_->get_loaded_controllers()[0]; - ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); - ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_THAT( - ctrl_1.info.fallback_controllers_names, testing::ElementsAre("ctrl_3", "ctrl_4", "ctrl_5")); - ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); - } - - // Try to spawn now the controller with fallback controllers inside the yaml - EXPECT_EQ( - call_spawner("ctrl_2 ctrl_3 -c test_controller_manager --load-only -p " + test_file_path), 0); - - ASSERT_EQ(cm_->get_loaded_controllers().size(), 3ul); - { - auto ctrl_1 = cm_->get_loaded_controllers()[0]; - ASSERT_EQ(ctrl_1.info.name, "ctrl_1"); - ASSERT_EQ(ctrl_1.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_THAT( - ctrl_1.info.fallback_controllers_names, testing::ElementsAre("ctrl_3", "ctrl_4", "ctrl_5")); - ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); - - auto ctrl_2 = cm_->get_loaded_controllers()[1]; - ASSERT_EQ(ctrl_2.info.name, "ctrl_2"); - ASSERT_EQ(ctrl_2.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_THAT( - ctrl_2.info.fallback_controllers_names, testing::ElementsAre("ctrl_6", "ctrl_7", "ctrl_8")); - ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); - - auto ctrl_3 = cm_->get_loaded_controllers()[2]; - ASSERT_EQ(ctrl_3.info.name, "ctrl_3"); - ASSERT_EQ(ctrl_3.info.type, test_controller::TEST_CONTROLLER_CLASS_NAME); - ASSERT_THAT(ctrl_3.info.fallback_controllers_names, testing::ElementsAre("ctrl_9")); - ASSERT_EQ(ctrl_3.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); - } -} class TestLoadControllerWithoutRobotDescription : public ControllerManagerFixture @@ -429,7 +333,7 @@ class TestLoadControllerWithNamespacedCM public: TestLoadControllerWithNamespacedCM() : ControllerManagerFixture( - ros2_control_test_assets::minimal_robot_urdf, "foo_namespace") + ros2_control_test_assets::minimal_robot_urdf, false, "foo_namespace") { } @@ -699,4 +603,3 @@ TEST_F( ASSERT_EQ(ctrl_2.info.type, test_chainable_controller::TEST_CONTROLLER_CLASS_NAME); ASSERT_EQ(ctrl_2.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); } ->>>>>>> af4b48f (Handle waiting in Spawner and align Hardware Spawner functionality (#1562))