diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index c3c0de7739..7c437d35e0 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -152,6 +152,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/doc/userdoc.rst b/controller_manager/doc/userdoc.rst index 5053eed3e4..02e532866d 100644 --- a/controller_manager/doc/userdoc.rst +++ b/controller_manager/doc/userdoc.rst @@ -48,7 +48,7 @@ jitter, using a lowlatency kernel can improve things a lot with being really eas Subscribers ----------- -~/robot_description [std_msgs::msg::String] +robot_description [std_msgs::msg::String] String with the URDF xml, e.g., from ``robot_state_publisher``. Reloading of the URDF is not supported yet. All joints defined in the ````-tag have to be present in the URDF. diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index f83339edc7..393c0e64f9 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -356,7 +356,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" @@ -585,6 +585,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 fb9bce879a..fa1fd067da 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -200,38 +200,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_); - } - - 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 the 'robot_description' topic from 'robot_state_publisher' instead."); - init_resource_manager(robot_description_); - if (is_resource_manager_initialized()) - { - RCLCPP_WARN( - get_logger(), - "You have to restart the framework when using robot description from parameter!"); - init_services(); - } - } - - diagnostics_updater_.setHardwareID("ros2_control"); - diagnostics_updater_.add( - "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback); + init_controller_manager(); } ControllerManager::ControllerManager( @@ -261,7 +230,6 @@ ControllerManager::ControllerManager( { init_services(); } - subscribe_to_robot_description_topic(); diagnostics_updater_.setHardwareID("ros2_control"); diagnostics_updater_.add( @@ -282,25 +250,30 @@ 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 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(); } - 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( @@ -309,6 +282,11 @@ 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); } void ControllerManager::robot_description_callback(const std_msgs::msg::String & robot_description) @@ -321,8 +299,7 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String & { RCLCPP_WARN( get_logger(), - "ResourceManager has already loaded an urdf file. Ignoring attempt to reload a robot " - "description file."); + "ResourceManager has already loaded a urdf. Ignoring attempt to reload a robot description."); return; } init_resource_manager(robot_description_); @@ -397,6 +374,10 @@ 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(); } void ControllerManager::init_services() @@ -898,6 +879,15 @@ controller_interface::return_type ControllerManager::switch_controller( const std::vector & deactivate_controllers, int strictness, bool activate_asap, const rclcpp::Duration & timeout) { + if (!is_resource_manager_initialized()) + { + 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/controller_manager_test_common.hpp b/controller_manager/test/controller_manager_test_common.hpp index f8cf9a7c11..8b6bd91376 100644 --- a/controller_manager/test/controller_manager_test_common.hpp +++ b/controller_manager/test/controller_manager_test_common.hpp @@ -65,46 +65,18 @@ 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( + rm_node_->get_node_clock_interface(), rm_node_->get_node_logging_interface()), + 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( - rm_node_->get_node_clock_interface(), rm_node_->get_node_logging_interface()), - 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_, rm_node_->get_node_clock_interface(), - rm_node_->get_node_logging_interface(), true, 100), - executor_, TEST_CM_NAME); - } - else - { - // TODO(mamueluth) : 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( - rm_node_->get_node_clock_interface(), rm_node_->get_node_logging_interface()), - 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()); } @@ -140,6 +112,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, @@ -162,7 +145,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_; protected: diff --git a/controller_manager/test/test_controller_manager_urdf_passing.cpp b/controller_manager/test/test_controller_manager_urdf_passing.cpp index 5db4cfc683..1556211a7f 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; @@ -55,10 +59,46 @@ class TestControllerManagerWithTestableCM { public: // create cm with no urdf - TestControllerManagerWithTestableCM() - : ControllerManagerFixture("", false) + TestControllerManagerWithTestableCM() : ControllerManagerFixture("") {} + + void prepare_controller() { + ASSERT_FALSE(cm_->is_resource_manager_initialized()); + 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_and_initialize_components_called) @@ -68,7 +108,9 @@ TEST_P(TestControllerManagerWithTestableCM, initial_no_load_and_initialize_compo TEST_P(TestControllerManagerWithTestableCM, load_and_initialize_components_called_after_callback) { - ASSERT_FALSE(cm_->resource_manager_->are_components_initialized()); + ASSERT_FALSE(cm_->is_resource_manager_initialized()); + pass_robot_description_to_cm_and_rm(); + ASSERT_TRUE(cm_->is_resource_manager_initialized()); // mimic callback auto msg = std_msgs::msg::String(); msg.data = ros2_control_test_assets::minimal_robot_urdf; @@ -80,7 +122,10 @@ TEST_P( TestControllerManagerWithTestableCM, expect_to_failure_when_invalid_urdf_is_given_and_be_able_to_submit_new_robot_description) { - ASSERT_FALSE(cm_->resource_manager_->are_components_initialized()); + ASSERT_FALSE(cm_->is_resource_manager_initialized()); + pass_robot_description_to_cm_and_rm( + ros2_control_test_assets::minimal_robot_missing_command_keys_urdf); + ASSERT_FALSE(cm_->is_resource_manager_initialized()); // wrong urdf auto msg = std_msgs::msg::String(); msg.data = ros2_control_test_assets::minimal_uninitializable_robot_urdf; @@ -93,5 +138,93 @@ TEST_P( ASSERT_TRUE(cm_->resource_manager_->are_components_initialized()); } +TEST_P( + TestControllerManagerWithTestableCM, + when_starting_broadcaster_expect_error_before_rm_is_initialized_with_robot_description) +{ + 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_->is_resource_manager_initialized()); + + 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_->is_resource_manager_initialized()); + + 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_->is_resource_manager_initialized()); + + try_successful_switch(); +} + 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 c0c9bbd9a4..2704368789 100644 --- a/controller_manager/test/test_hardware_management_srvs.cpp +++ b/controller_manager/test/test_hardware_management_srvs.cpp @@ -66,8 +66,6 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs cm_ = std::make_shared(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}))); @@ -75,16 +73,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(); @@ -371,21 +361,8 @@ class TestControllerManagerHWManagementSrvsWithoutParams cm_ = std::make_shared(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(); diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index a5dd8fcda3..37db345cb7 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -32,6 +32,7 @@ using ::testing::Return; using namespace std::chrono_literals; class TestLoadController : public ControllerManagerFixture { +public: void SetUp() override { ControllerManagerFixture::SetUp(); @@ -326,6 +327,85 @@ 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"), 256) + << "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(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); + } +} + TEST_F(TestLoadController, spawner_test_fallback_controllers) { const std::string test_file_path = ament_index_cpp::get_package_prefix("controller_manager") + 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 ******************