From 0792a35966f981f40bf977d211d89de53940e258 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 19 Jul 2024 18:31:07 +0200 Subject: [PATCH] Fix controller starting with later load of robot description test (#1624) * Fix the duplicated restart of the controller_manager services initialization * Scope the ControllerManagerRunner to avoid malloc and other test issues * reorder the tests for consistent log at the end --- controller_manager/src/controller_manager.cpp | 39 +++--- .../test/test_spawner_unspawner.cpp | 116 +++++++++--------- 2 files changed, 74 insertions(+), 81 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index fa1fd067da..2c564fe659 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -220,20 +220,7 @@ ControllerManager::ControllerManager( std::make_shared>( kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)) { - if (!get_parameter("update_rate", update_rate_)) - { - RCLCPP_WARN( - get_logger(), "'update_rate' parameter not set, using default value of %d Hz.", update_rate_); - } - - if (is_resource_manager_initialized()) - { - init_services(); - } - - diagnostics_updater_.setHardwareID("ros2_control"); - diagnostics_updater_.add( - "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); + init_controller_manager(); } ControllerManager::ControllerManager( @@ -262,17 +249,20 @@ void ControllerManager::init_controller_manager() get_logger(), "'update_rate' parameter not set, using default value of %d Hz.", update_rate_); } - robot_description_notification_timer_ = create_wall_timer( - std::chrono::seconds(1), - [&]() - { - RCLCPP_WARN( - get_logger(), "Waiting for data on 'robot_description' topic to finish initialization"); - }); if (is_resource_manager_initialized()) { init_services(); } + else + { + robot_description_notification_timer_ = create_wall_timer( + std::chrono::seconds(1), + [&]() + { + RCLCPP_WARN( + get_logger(), "Waiting for data on 'robot_description' topic to finish initialization"); + }); + } // set QoS to transient local to get messages that have already been published // (if robot state publisher starts before controller manager) @@ -305,6 +295,10 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String & init_resource_manager(robot_description_); if (is_resource_manager_initialized()) { + RCLCPP_INFO( + get_logger(), + "Resource Manager has been successfully initialized. Starting Controller Manager " + "services..."); init_services(); } } @@ -374,9 +368,6 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript State::PRIMARY_STATE_ACTIVE, hardware_interface::lifecycle_state_names::ACTIVE); resource_manager_->set_component_state(component, active_state); } - - // Init CM services first after the URDF is loaded an components are set - init_services(); robot_description_notification_timer_->cancel(); } diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 37db345cb7..a9e915dba6 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -327,6 +327,61 @@ TEST_F(TestLoadController, unload_on_kill) ASSERT_EQ(cm_->get_loaded_controllers().size(), 0ul); } +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 { @@ -393,70 +448,17 @@ TEST_F( robot_description_sending_timer_ = cm_->create_wall_timer( std::chrono::milliseconds(2500), [&]() { pass_robot_description_to_cm_and_rm(); }); - ControllerManagerRunner cm_runner(this); - EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 0) - << "could not activate control because not robot description"; - - 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_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 0) + << "could not activate control because not robot description"; } -} - -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); + ASSERT_EQ(ctrl_1.c->get_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); } }