From 4b5d5ffa239e3587855e8f0e34858082c8c9930f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Thu, 1 Feb 2024 20:18:49 +0100 Subject: [PATCH 1/6] Remove support for getting robot description from the parameter in CM. --- controller_manager/src/controller_manager.cpp | 28 +++------- .../test/controller_manager_test_common.hpp | 50 +++++++----------- .../test_controller_manager_urdf_passing.cpp | 52 +++++++++++++------ .../test/test_hardware_management_srvs.cpp | 27 +--------- 4 files changed, 64 insertions(+), 93 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 9b9b0dcf8f..35c486157f 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -341,30 +341,16 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String & RCLCPP_INFO(get_logger(), "Received robot description from topic."); RCLCPP_DEBUG( get_logger(), "'Content of robot description file: %s", robot_description.data.c_str()); - // TODO(Manuel): errors should probably be caught since we don't want controller_manager node - // to die if a non valid urdf is passed. However, should maybe be fine tuned. - try - { - robot_description_ = robot_description.data; - if (resource_manager_->is_urdf_already_loaded()) - { - RCLCPP_WARN( - get_logger(), - "ResourceManager has already loaded an urdf file. Ignoring attempt to reload a robot " - "description file."); - return; - } - init_resource_manager(robot_description_); - init_services(); - } - catch (std::runtime_error & e) + if (resource_manager_->is_urdf_already_loaded()) { - RCLCPP_ERROR_STREAM( + RCLCPP_WARN( get_logger(), - "The published robot description file (urdf) seems not to be genuine. The following error " - "was caught:" - << e.what()); + "ResourceManager has already loaded an urdf file. Ignoring attempt to reload a robot " + "description file."); + return; } + robot_description_ = robot_description.data; + init_resource_manager(robot_description_); } void ControllerManager::init_resource_manager(const std::string & robot_description) diff --git a/controller_manager/test/controller_manager_test_common.hpp b/controller_manager/test/controller_manager_test_common.hpp index 7bf5cae628..8b6608bbb3 100644 --- a/controller_manager/test/controller_manager_test_common.hpp +++ b/controller_manager/test/controller_manager_test_common.hpp @@ -65,40 +65,16 @@ 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) - : robot_description_(robot_description), pass_urdf_as_parameter_(pass_urdf_as_parameter) + const std::string & robot_description = ros2_control_test_assets::minimal_robot_urdf) + : robot_description_(robot_description) { executor_ = std::make_shared(); - // We want to be able to create a ResourceManager where no urdf file has been passed to - if (robot_description_.empty()) + cm_ = std::make_shared( + std::make_unique(), executor_, TEST_CM_NAME); + // We want to be able to not pass robot description immediately + if (!robot_description_.empty()) { - cm_ = std::make_shared( - std::make_unique(), executor_, TEST_CM_NAME); - } - else - { - // can be removed later, needed if we want to have the deprecated way of passing the robot - // description file to the controller manager covered by tests - if (pass_urdf_as_parameter_) - { - cm_ = std::make_shared( - std::make_unique(robot_description_, true, true), - executor_, TEST_CM_NAME); - } - else - { - // 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 - cm_ = std::make_shared( - std::make_unique(), executor_, TEST_CM_NAME); - // 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_); } time_ = rclcpp::Time(0, 0, cm_->get_node_clock_interface()->get_clock()->get_clock_type()); } @@ -134,6 +110,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, @@ -156,7 +143,6 @@ class ControllerManagerFixture : public ::testing::Test std::thread updater_; bool run_updater_; const std::string robot_description_; - const bool pass_urdf_as_parameter_; rclcpp::Time time_; }; diff --git a/controller_manager/test/test_controller_manager_urdf_passing.cpp b/controller_manager/test/test_controller_manager_urdf_passing.cpp index 5c2ebd14f6..30b0f5821e 100644 --- a/controller_manager/test/test_controller_manager_urdf_passing.cpp +++ b/controller_manager/test/test_controller_manager_urdf_passing.cpp @@ -20,8 +20,12 @@ #include "controller_manager/controller_manager.hpp" #include "controller_manager_test_common.hpp" - #include "ros2_control_test_assets/descriptions.hpp" +#include "test_controller/test_controller.hpp" + +const auto CONTROLLER_NAME = "test_controller"; +using test_controller::TEST_CONTROLLER_CLASS_NAME; +using strvec = std::vector; class TestControllerManagerWithTestableCM; @@ -32,8 +36,8 @@ class TestableControllerManager : public controller_manager::ControllerManager FRIEND_TEST(TestControllerManagerWithTestableCM, initial_no_load_urdf_called); FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_callback); FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_invalid_urdf_passed); - FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_callback); - FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_callback); + FRIEND_TEST( + TestControllerManagerWithTestableCM, when_starting_controller_without_hw_expect_error); public: TestableControllerManager( @@ -53,10 +57,7 @@ class TestControllerManagerWithTestableCM { public: // create cm with no urdf - TestControllerManagerWithTestableCM() - : ControllerManagerFixture("", false) - { - } + TestControllerManagerWithTestableCM() : ControllerManagerFixture("") {} }; TEST_P(TestControllerManagerWithTestableCM, initial_no_load_urdf_called) @@ -67,22 +68,43 @@ TEST_P(TestControllerManagerWithTestableCM, initial_no_load_urdf_called) TEST_P(TestControllerManagerWithTestableCM, load_urdf_called_after_callback) { ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded()); - // mimic callback - auto msg = std_msgs::msg::String(); - msg.data = ros2_control_test_assets::minimal_robot_urdf; - cm_->robot_description_callback(msg); + pass_robot_description_to_cm_and_rm(); ASSERT_TRUE(cm_->resource_manager_->is_urdf_already_loaded()); } TEST_P(TestControllerManagerWithTestableCM, load_urdf_called_after_invalid_urdf_passed) { ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded()); - // mimic callback - auto msg = std_msgs::msg::String(); - msg.data = ros2_control_test_assets::minimal_robot_missing_command_keys_urdf; - cm_->robot_description_callback(msg); + pass_robot_description_to_cm_and_rm( + ros2_control_test_assets::minimal_robot_missing_command_keys_urdf); ASSERT_TRUE(cm_->resource_manager_->is_urdf_already_loaded()); } +TEST_P(TestControllerManagerWithTestableCM, when_starting_controller_without_hw_expect_error) +{ + ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded()); + auto controller_if = cm_->load_controller(CONTROLLER_NAME, TEST_CONTROLLER_CLASS_NAME); + ASSERT_NE(controller_if, nullptr); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id()); + cm_->configure_controller(CONTROLLER_NAME); + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); + + // Set ControllerManager into Debug-Mode output to have detailed output on updating controllers + cm_->get_logger().set_level(rclcpp::Logger::Level::Debug); + + switch_test_controllers( + strvec{CONTROLLER_NAME}, strvec{}, STRICT, std::future_status::timeout, + controller_interface::return_type::ERROR); + + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); + + // mimic callback + // auto msg = std_msgs::msg::String(); + // msg.data = ros2_control_test_assets::minimal_robot_missing_command_keys_urdf; + // cm_->robot_description_callback(msg); + // ASSERT_TRUE(cm_->resource_manager_->is_urdf_already_loaded()); +} + INSTANTIATE_TEST_SUITE_P( test_best_effort, TestControllerManagerWithTestableCM, testing::Values(best_effort)); diff --git a/controller_manager/test/test_hardware_management_srvs.cpp b/controller_manager/test/test_hardware_management_srvs.cpp index 0b4b107ee1..93697d3ca6 100644 --- a/controller_manager/test/test_hardware_management_srvs.cpp +++ b/controller_manager/test/test_hardware_management_srvs.cpp @@ -67,8 +67,6 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs std::make_unique(), executor_, TEST_CM_NAME); run_updater_ = false; - cm_->set_parameter( - rclcpp::Parameter("robot_description", ros2_control_test_assets::minimal_robot_urdf)); cm_->set_parameter(rclcpp::Parameter( "hardware_components_initial_state.unconfigured", std::vector({TEST_SYSTEM_HARDWARE_NAME}))); @@ -76,16 +74,8 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs "hardware_components_initial_state.inactive", std::vector({TEST_SENSOR_HARDWARE_NAME}))); - std::string robot_description = ""; - cm_->get_parameter("robot_description", robot_description); - if (robot_description.empty()) - { - throw std::runtime_error( - "Unable to initialize resource manager, no robot description found."); - } - auto msg = std_msgs::msg::String(); - msg.data = robot_description_; + msg.data = ros2_control_test_assets::minimal_robot_urdf; cm_->robot_description_callback(msg); SetUpSrvsCMExecutor(); @@ -372,21 +362,8 @@ class TestControllerManagerHWManagementSrvsWithoutParams std::make_unique(), executor_, TEST_CM_NAME); run_updater_ = false; - // TODO(destogl): separate this to init_tests method where parameter can be set for each test - // separately - cm_->set_parameter( - rclcpp::Parameter("robot_description", ros2_control_test_assets::minimal_robot_urdf)); - - std::string robot_description = ""; - cm_->get_parameter("robot_description", robot_description); - if (robot_description.empty()) - { - throw std::runtime_error( - "Unable to initialize resource manager, no robot description found."); - } - auto msg = std_msgs::msg::String(); - msg.data = robot_description_; + msg.data = ros2_control_test_assets::minimal_robot_urdf; cm_->robot_description_callback(msg); SetUpSrvsCMExecutor(); From d46c4129e9908b12c817a3fba2a8fac9931fe63b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Mon, 18 Dec 2023 14:53:25 +0100 Subject: [PATCH 2/6] Added functionality and tests for getting robot description from the topic. --- controller_manager/CMakeLists.txt | 1 + .../controller_manager/controller_manager.hpp | 3 +- controller_manager/src/controller_manager.cpp | 81 ++++----- .../test_controller_manager_urdf_passing.cpp | 156 +++++++++++++++--- .../test/test_spawner_unspawner.cpp | 80 +++++++++ 5 files changed, 256 insertions(+), 65 deletions(-) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 376cef3362..0d1b88a55a 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -150,6 +150,7 @@ if(BUILD_TESTING) ) target_link_libraries(test_controller_manager_urdf_passing controller_manager + test_controller ros2_control_test_assets::ros2_control_test_assets ) ament_target_dependencies(test_controller_manager_urdf_passing diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index f5ee6854a1..c0a22fe5bd 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -322,7 +322,7 @@ class ControllerManager : public rclcpp::Node std::vector get_controller_names(); std::pair split_command_interface( const std::string & command_interface); - void subscribe_to_robot_description_topic(); + void init_controller_manager(); /** * Clear request lists used when switching controllers. The lists are shared between "callback" @@ -546,6 +546,7 @@ class ControllerManager : public rclcpp::Node std::string robot_description_; rclcpp::Subscription::SharedPtr robot_description_subscription_; + rclcpp::TimerBase::SharedPtr robot_description_notification_timer_; struct SwitchParams { diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 35c486157f..2c958bf591 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -267,31 +267,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."); - } - - robot_description_ = ""; - // TODO(destogl): remove support at the end of 2023 - get_parameter("robot_description", robot_description_); - if (robot_description_.empty()) - { - subscribe_to_robot_description_topic(); - } - else - { - RCLCPP_WARN( - get_logger(), - "[Deprecated] Passing the robot description parameter directly to the control_manager node " - "is deprecated. Use '~/robot_description' topic from 'robot_state_publisher' instead."); - init_resource_manager(robot_description_); - init_services(); - } - - diagnostics_updater_.setHardwareID("ros2_control"); - diagnostics_updater_.add( - "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); + init_controller_manager(); } ControllerManager::ControllerManager( @@ -308,24 +284,17 @@ ControllerManager::ControllerManager( std::make_shared>( kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)) { + init_controller_manager(); +} + +void ControllerManager::init_controller_manager() +{ + // Get parameters needed for RT "update" loop to work if (!get_parameter("update_rate", update_rate_)) { RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value."); } - if (resource_manager_->is_urdf_already_loaded()) - { - init_services(); - } - subscribe_to_robot_description_topic(); - - diagnostics_updater_.setHardwareID("ros2_control"); - diagnostics_updater_.add( - "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); -} - -void ControllerManager::subscribe_to_robot_description_topic() -{ // set QoS to transient local to get messages that have already been published // (if robot state publisher starts before controller manager) robot_description_subscription_ = create_subscription( @@ -334,6 +303,19 @@ void ControllerManager::subscribe_to_robot_description_topic() RCLCPP_INFO( get_logger(), "Subscribing to '%s' topic for robot description.", robot_description_subscription_->get_topic_name()); + + // Setup diagnostics + diagnostics_updater_.setHardwareID("ros2_control"); + diagnostics_updater_.add( + "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); + + 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"); + }); } void ControllerManager::robot_description_callback(const std_msgs::msg::String & robot_description) @@ -355,8 +337,14 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String & void ControllerManager::init_resource_manager(const std::string & robot_description) { - // TODO(destogl): manage this when there is an error - CM should not die because URDF is wrong... - resource_manager_->load_urdf(robot_description); + if (!resource_manager_->load_urdf(robot_description)) + { + RCLCPP_WARN( + get_logger(), + "URDF validation went wrong check the previous output. This might only mean that interfaces " + "defined in URDF and exported by the hardware do not match. Therefore continue initializing " + "controller manager..."); + } // Get all components and if they are not defined in parameters activate them automatically auto components_to_activate = resource_manager_->get_components_status(); @@ -451,6 +439,10 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript 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(); } void ControllerManager::init_services() @@ -824,6 +816,15 @@ controller_interface::return_type ControllerManager::switch_controller( const std::vector & deactivate_controllers, int strictness, bool activate_asap, const rclcpp::Duration & timeout) { + if (!resource_manager_->is_urdf_already_loaded()) + { + RCLCPP_ERROR( + get_logger(), + "Resource Manager is not initialized yet! Please provide robot description on " + "'~/robot_description' topic before trying to switch controllers."); + return controller_interface::return_type::ERROR; + } + switch_params_ = SwitchParams(); if (!deactivate_request_.empty() || !activate_request_.empty()) diff --git a/controller_manager/test/test_controller_manager_urdf_passing.cpp b/controller_manager/test/test_controller_manager_urdf_passing.cpp index 30b0f5821e..b5adb257ab 100644 --- a/controller_manager/test/test_controller_manager_urdf_passing.cpp +++ b/controller_manager/test/test_controller_manager_urdf_passing.cpp @@ -37,7 +37,14 @@ class TestableControllerManager : public controller_manager::ControllerManager FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_callback); FRIEND_TEST(TestControllerManagerWithTestableCM, load_urdf_called_after_invalid_urdf_passed); FRIEND_TEST( - TestControllerManagerWithTestableCM, when_starting_controller_without_hw_expect_error); + TestControllerManagerWithTestableCM, + when_starting_broadcaster_expect_error_before_rm_is_initialized_with_robot_description); + FRIEND_TEST( + TestControllerManagerWithTestableCM, + when_starting_controller_expect_error_before_rm_is_initialized_with_robot_description); + FRIEND_TEST( + TestControllerManagerWithTestableCM, + when_starting_controller_expect_error_before_rm_is_initialized_after_some_time); public: TestableControllerManager( @@ -58,6 +65,45 @@ class TestControllerManagerWithTestableCM public: // create cm with no urdf TestControllerManagerWithTestableCM() : ControllerManagerFixture("") {} + + void prepare_controller() + { + ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded()); + test_controller_ = std::make_shared(); + cm_->add_controller(test_controller_, CONTROLLER_NAME, TEST_CONTROLLER_CLASS_NAME); + ASSERT_NE(test_controller_, nullptr); + ASSERT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, test_controller_->get_state().id()); + } + + void configure_and_try_switch_that_returns_error() + { + // configure controller + cm_->configure_controller(CONTROLLER_NAME); + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_->get_state().id()); + + // Set ControllerManager into Debug-Mode output to have detailed output on updating controllers + cm_->get_logger().set_level(rclcpp::Logger::Level::Debug); + + switch_test_controllers( + strvec{CONTROLLER_NAME}, strvec{}, STRICT, std::future_status::ready, + controller_interface::return_type::ERROR); + + EXPECT_EQ( + lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, test_controller_->get_state().id()); + } + + void try_successful_switch() + { + switch_test_controllers( + strvec{CONTROLLER_NAME}, strvec{}, STRICT, std::future_status::timeout, + controller_interface::return_type::OK); + + EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, test_controller_->get_state().id()); + } + + std::shared_ptr test_controller_; }; TEST_P(TestControllerManagerWithTestableCM, initial_no_load_urdf_called) @@ -80,30 +126,92 @@ TEST_P(TestControllerManagerWithTestableCM, load_urdf_called_after_invalid_urdf_ ASSERT_TRUE(cm_->resource_manager_->is_urdf_already_loaded()); } -TEST_P(TestControllerManagerWithTestableCM, when_starting_controller_without_hw_expect_error) +TEST_P( + TestControllerManagerWithTestableCM, + when_starting_broadcaster_expect_error_before_rm_is_initialized_with_robot_description) { - ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded()); - auto controller_if = cm_->load_controller(CONTROLLER_NAME, TEST_CONTROLLER_CLASS_NAME); - ASSERT_NE(controller_if, nullptr); - ASSERT_EQ( - lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED, controller_if->get_state().id()); - cm_->configure_controller(CONTROLLER_NAME); - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); - - // Set ControllerManager into Debug-Mode output to have detailed output on updating controllers - cm_->get_logger().set_level(rclcpp::Logger::Level::Debug); - - switch_test_controllers( - strvec{CONTROLLER_NAME}, strvec{}, STRICT, std::future_status::timeout, - controller_interface::return_type::ERROR); - - EXPECT_EQ(lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE, controller_if->get_state().id()); - - // mimic callback - // auto msg = std_msgs::msg::String(); - // msg.data = ros2_control_test_assets::minimal_robot_missing_command_keys_urdf; - // cm_->robot_description_callback(msg); - // ASSERT_TRUE(cm_->resource_manager_->is_urdf_already_loaded()); + prepare_controller(); + + // setup state interfaces to claim from controllers + controller_interface::InterfaceConfiguration state_itfs_cfg; + state_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_STATE_INTERFACES) + { + state_itfs_cfg.names.push_back(interface); + } + for (const auto & interface : ros2_control_test_assets::TEST_SENSOR_HARDWARE_STATE_INTERFACES) + { + state_itfs_cfg.names.push_back(interface); + } + test_controller_->set_state_interface_configuration(state_itfs_cfg); + + configure_and_try_switch_that_returns_error(); + + pass_robot_description_to_cm_and_rm(); + ASSERT_TRUE(cm_->resource_manager_->is_urdf_already_loaded()); + + try_successful_switch(); +} + +TEST_P( + TestControllerManagerWithTestableCM, + when_starting_controller_expect_error_before_rm_is_initialized_with_robot_description) +{ + prepare_controller(); + + // setup command interfaces to claim from controllers + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES) + { + cmd_itfs_cfg.names.push_back(interface); + } + test_controller_->set_command_interface_configuration(cmd_itfs_cfg); + + configure_and_try_switch_that_returns_error(); + + pass_robot_description_to_cm_and_rm(); + ASSERT_TRUE(cm_->resource_manager_->is_urdf_already_loaded()); + + try_successful_switch(); +} + +TEST_P( + TestControllerManagerWithTestableCM, + when_starting_controller_expect_error_before_rm_is_initialized_after_some_time) +{ + prepare_controller(); + + // setup state interfaces to claim from controllers + controller_interface::InterfaceConfiguration state_itfs_cfg; + state_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_STATE_INTERFACES) + { + state_itfs_cfg.names.push_back(interface); + } + for (const auto & interface : ros2_control_test_assets::TEST_SENSOR_HARDWARE_STATE_INTERFACES) + { + state_itfs_cfg.names.push_back(interface); + } + test_controller_->set_state_interface_configuration(state_itfs_cfg); + + // setup command interfaces to claim from controllers + controller_interface::InterfaceConfiguration cmd_itfs_cfg; + cmd_itfs_cfg.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto & interface : ros2_control_test_assets::TEST_ACTUATOR_HARDWARE_COMMAND_INTERFACES) + { + cmd_itfs_cfg.names.push_back(interface); + } + test_controller_->set_command_interface_configuration(cmd_itfs_cfg); + + configure_and_try_switch_that_returns_error(); + + std::this_thread::sleep_for(std::chrono::milliseconds(3000)); + + pass_robot_description_to_cm_and_rm(); + ASSERT_TRUE(cm_->resource_manager_->is_urdf_already_loaded()); + + try_successful_switch(); } INSTANTIATE_TEST_SUITE_P( diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 1ecc3164b8..353f6738ab 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -31,6 +31,7 @@ using ::testing::Return; using namespace std::chrono_literals; class TestLoadController : public ControllerManagerFixture { +public: void SetUp() override { ControllerManagerFixture::SetUp(); @@ -274,3 +275,82 @@ TEST_F(TestLoadController, unload_on_kill) ASSERT_EQ(cm_->get_loaded_controllers().size(), 0ul); } + +class TestLoadControllerWithoutRobotDescription +: public ControllerManagerFixture +{ +public: + TestLoadControllerWithoutRobotDescription() + : ControllerManagerFixture("") + { + } + + 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(TestLoadControllerWithoutRobotDescription, when_no_robot_description_spawner_times_out) +{ + cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 1) + << "could not spawn controller because not robot description and not services for controller " + "manager are active"; +} + +TEST_F( + TestLoadControllerWithoutRobotDescription, + controller_starting_with_later_load_of_robot_description) +{ + cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); + + // Delay sending robot description + robot_description_sending_timer_ = cm_->create_wall_timer( + std::chrono::milliseconds(1500), [&]() { pass_robot_description_to_cm_and_rm(); }); + + ControllerManagerRunner cm_runner(this); + EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 1) + << "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); + } +} From bf3c96a65797d7325f8608a09a41381c54d3b3f9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Mon, 18 Dec 2023 15:06:24 +0100 Subject: [PATCH 3/6] Fix tests. --- controller_manager/test/test_spawner_unspawner.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 353f6738ab..03917dcd6e 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -327,7 +327,7 @@ TEST_F(TestLoadControllerWithoutRobotDescription, when_no_robot_description_spaw cm_->set_parameter(rclcpp::Parameter("ctrl_1.type", test_controller::TEST_CONTROLLER_CLASS_NAME)); ControllerManagerRunner cm_runner(this); - EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 1) + EXPECT_EQ(call_spawner("ctrl_1 -c test_controller_manager"), 256) << "could not spawn controller because not robot description and not services for controller " "manager are active"; } @@ -340,10 +340,10 @@ TEST_F( // Delay sending robot description robot_description_sending_timer_ = cm_->create_wall_timer( - std::chrono::milliseconds(1500), [&]() { pass_robot_description_to_cm_and_rm(); }); + 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"), 1) + 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); From 760c0dfe31530be10df57d01d32e892dc7a9d693 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Fri, 26 Jan 2024 18:32:28 +0100 Subject: [PATCH 4/6] Add small behavior fixes. --- controller_manager/src/controller_manager.cpp | 16 ++++++++-------- .../test/test_resource_manager.cpp | 6 ++---- 2 files changed, 10 insertions(+), 12 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 2c958bf591..932dfd392b 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -295,6 +295,14 @@ void ControllerManager::init_controller_manager() RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value."); } + 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) robot_description_subscription_ = create_subscription( @@ -308,14 +316,6 @@ void ControllerManager::init_controller_manager() diagnostics_updater_.setHardwareID("ros2_control"); diagnostics_updater_.add( "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); - - 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"); - }); } void ControllerManager::robot_description_callback(const std_msgs::msg::String & robot_description) diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index 4bb9e0c5fe..d21b4f26ae 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -106,9 +106,7 @@ TEST_F(ResourceManagerTest, test_unitilizable_hardware_validation) // If the the hardware can not be initialized and load_urdf tried to validate the interfaces a // runtime exception is thrown TestableResourceManager rm; - ASSERT_THROW( - rm.load_urdf(ros2_control_test_assets::minimal_unitilizable_robot_urdf, true), - std::runtime_error); + EXPECT_FALSE(rm.load_urdf(ros2_control_test_assets::minimal_unitilizable_robot_urdf, true)); } TEST_F(ResourceManagerTest, test_unitilizable_hardware_no_validation) @@ -116,7 +114,7 @@ TEST_F(ResourceManagerTest, test_unitilizable_hardware_no_validation) // If the the hardware can not be initialized and load_urdf didn't try to validate the interfaces, // the interface should not show up TestableResourceManager rm; - EXPECT_NO_THROW(rm.load_urdf(ros2_control_test_assets::minimal_unitilizable_robot_urdf, false)); + EXPECT_TRUE(rm.load_urdf(ros2_control_test_assets::minimal_unitilizable_robot_urdf, false)); // test actuator EXPECT_FALSE(rm.state_interface_exists("joint1/position")); From e5056e59de15a19ae4b6c7cabfe1f4fe6a31d86d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Thu, 1 Feb 2024 20:42:38 +0100 Subject: [PATCH 5/6] Revert changes on RM tests. --- hardware_interface_testing/test/test_resource_manager.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index d21b4f26ae..4bb9e0c5fe 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -106,7 +106,9 @@ TEST_F(ResourceManagerTest, test_unitilizable_hardware_validation) // If the the hardware can not be initialized and load_urdf tried to validate the interfaces a // runtime exception is thrown TestableResourceManager rm; - EXPECT_FALSE(rm.load_urdf(ros2_control_test_assets::minimal_unitilizable_robot_urdf, true)); + ASSERT_THROW( + rm.load_urdf(ros2_control_test_assets::minimal_unitilizable_robot_urdf, true), + std::runtime_error); } TEST_F(ResourceManagerTest, test_unitilizable_hardware_no_validation) @@ -114,7 +116,7 @@ TEST_F(ResourceManagerTest, test_unitilizable_hardware_no_validation) // If the the hardware can not be initialized and load_urdf didn't try to validate the interfaces, // the interface should not show up TestableResourceManager rm; - EXPECT_TRUE(rm.load_urdf(ros2_control_test_assets::minimal_unitilizable_robot_urdf, false)); + EXPECT_NO_THROW(rm.load_urdf(ros2_control_test_assets::minimal_unitilizable_robot_urdf, false)); // test actuator EXPECT_FALSE(rm.state_interface_exists("joint1/position")); From 8a3056a10cef0afa25352bfd577e5f6ec9075003 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Mon, 15 Jul 2024 12:46:55 +0000 Subject: [PATCH 6/6] Update migration notes --- doc/migration.rst | 1 + doc/release_notes.rst | 1 + 2 files changed, 2 insertions(+) diff --git a/doc/migration.rst b/doc/migration.rst index 1880f5d380..68359f072f 100644 --- a/doc/migration.rst +++ b/doc/migration.rst @@ -64,6 +64,7 @@ controller_manager +* The support for the ``description`` parameter for loading the URDF was removed (`#1358 `_). Use ``robot_description`` topic instead, e.g., you can use the `robot_state_publisher `_. For an example, see `this PR `_ where the change was applied to the demo repository. hardware_interface ****************** diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 5d1773afe8..a4a6f31734 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -68,6 +68,7 @@ controller_manager The parameters within the ``ros2_control`` tag are not supported any more. +* The support for the ``description`` parameter for loading the URDF was removed (`#1358 `_). hardware_interface ******************