From 0c17054773d2962f695cdc3b0650aa20a3ba9c92 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Sun, 14 Jul 2024 23:56:12 +0200 Subject: [PATCH 1/6] Use a single file for migration + release notes (#1611) --- doc/{migration/Jazzy.rst => migration.rst} | 0 doc/migration/Galactic.rst | 53 ------------------- .../Jazzy.rst => release_notes.rst} | 0 3 files changed, 53 deletions(-) rename doc/{migration/Jazzy.rst => migration.rst} (100%) delete mode 100644 doc/migration/Galactic.rst rename doc/{release_notes/Jazzy.rst => release_notes.rst} (100%) diff --git a/doc/migration/Jazzy.rst b/doc/migration.rst similarity index 100% rename from doc/migration/Jazzy.rst rename to doc/migration.rst diff --git a/doc/migration/Galactic.rst b/doc/migration/Galactic.rst deleted file mode 100644 index 4c6e958951e..00000000000 --- a/doc/migration/Galactic.rst +++ /dev/null @@ -1,53 +0,0 @@ -:github_url: https://github.com/ros-controls/ros2_control/blob/{REPOS_FILE_BRANCH}/doc/release_notes/Galactic.rst - -Foxy to Galactic -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -hardware_interface -************************************** -Between Foxy and Galactic we made substantial changes to the interface of hardware components to enable management of their lifecycle. -The following list shows a set of quick changes to port existing hardware components to Galactic: - -1. Rename ``configure`` to ``on_init`` and change return type to ``CallbackReturn`` - -2. If using BaseInterface as base class then you should remove it. Specifically, change: - - .. code-block:: c++ - - hardware_interface::BaseInterface - - to - - .. code-block:: c++ - - hardware_interface::[Actuator|Sensor|System]Interface - -3. Remove include of headers ``base_interface.hpp`` and ``hardware_interface_status_values.hpp`` - -4. Add include of header ``rclcpp_lifecycle/state.hpp`` although this may not be strictly necessary - -5. replace first three lines in ``on_init`` to - - .. code-block:: c++ - - if (hardware_interface::[Actuator|Sensor|System]Interface::on_init(info) != CallbackReturn::SUCCESS) - { - return CallbackReturn::ERROR; - } - - -6. Change last return of ``on_init`` to ``return CallbackReturn::SUCCESS;`` - -7. Remove all lines with ``status_ = ...`` or ``status::...`` - -8. Rename ``start()`` to ``on_activate(const rclcpp_lifecycle::State & previous_state)`` and - ``stop()`` to ``on_deactivate(const rclcpp_lifecycle::State & previous_state)`` - -9. Change return type of ``on_activate`` and ``on_deactivate`` to ``CallbackReturn`` - -10. Change last return of ``on_activate`` and ``on_deactivate`` to ``return CallbackReturn::SUCCESS;`` - -11. If you have any ``return_type::ERROR`` in ``on_init``, ``on_activate``, or ``in_deactivate`` change to ``CallbackReturn::ERROR`` - -12. If you get link errors with undefined refernences to symbols in ``rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface``, then add - ``rclcpp_lifecyle`` package dependency to ``CMakeLists.txt`` and ``package.xml`` diff --git a/doc/release_notes/Jazzy.rst b/doc/release_notes.rst similarity index 100% rename from doc/release_notes/Jazzy.rst rename to doc/release_notes.rst From 7be0c6a88472ae4919c8b8c1b57f188e065f4c65 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Tue, 16 Jul 2024 09:47:31 +0200 Subject: [PATCH 2/6] [ResourceManager] Make destructor virtual for use in derived classes (#1607) --- .../include/hardware_interface/resource_manager.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 5313162c29a..313debcf587 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -75,7 +75,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager ResourceManager(const ResourceManager &) = delete; - ~ResourceManager(); + virtual ~ResourceManager(); /// Load resources from on a given URDF. /** From 1f8ca08923ee53f75f86b4f8298c337d46a861e7 Mon Sep 17 00:00:00 2001 From: Parker Drouillard Date: Tue, 16 Jul 2024 07:30:49 -0400 Subject: [PATCH 3/6] Fix typos in test_resource_manager.cpp (#1609) --- hardware_interface_testing/test/test_resource_manager.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index 5fb155fa3a8..51d81a90ab7 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -137,9 +137,9 @@ void test_load_and_initialized_components_failure(const std::string & urdf) EXPECT_FALSE(rm.command_interface_exists("joint3/max_acceleration")); } -TEST_F(ResourceManagerTest, test_unitilizable_hardware) +TEST_F(ResourceManagerTest, test_uninitializable_hardware) { - SCOPED_TRACE("test_unitilizable_hardware_no_validation"); + SCOPED_TRACE("test_uninitializable_hardware_no_validation"); // If the the hardware can not be initialized and load_and_initialize_components didn't try to // validate the interfaces, the interface should not show up test_load_and_initialized_components_failure( From 9d1aff96291302c6db153f863792350e352f66a9 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Wed, 17 Jul 2024 19:35:47 +0200 Subject: [PATCH 4/6] [CM] Remove support for the description parameter and use only `robot_description` topic (#1358) --------- Co-authored-by: Felix Exner Co-authored-by: Christoph Froehlich --- controller_manager/CMakeLists.txt | 1 + controller_manager/doc/userdoc.rst | 2 +- .../controller_manager/controller_manager.hpp | 3 +- controller_manager/src/controller_manager.cpp | 76 ++++------ .../test/controller_manager_test_common.hpp | 58 +++---- .../test_controller_manager_urdf_passing.cpp | 143 +++++++++++++++++- .../test/test_hardware_management_srvs.cpp | 27 +--- .../test/test_spawner_unspawner.cpp | 80 ++++++++++ doc/migration.rst | 1 + doc/release_notes.rst | 1 + 10 files changed, 279 insertions(+), 113 deletions(-) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index c3c0de7739e..7c437d35e01 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 5053eed3e4d..02e532866d4 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 f83339edc7e..393c0e64f9e 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 fb9bce879ae..fa1fd067da1 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 f8cf9a7c119..8b6bd913768 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 5db4cfc6837..1556211a7ff 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 c0c9bbd9a4f..27043687892 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 a5dd8fcda3a..37db345cb7d 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 1880f5d380a..68359f072f0 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 5d1773afe86..a4a6f317341 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 ****************** From 152d19bee505ad0bdf924cae642c5350d93deaf4 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 19 Jul 2024 18:29:02 +0200 Subject: [PATCH 5/6] move critical variables to the private context (#1623) --- .../controller_interface/controller_interface_base.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/controller_interface/include/controller_interface/controller_interface_base.hpp b/controller_interface/include/controller_interface/controller_interface_base.hpp index 1b5fd2e0593..cfaf4a9799a 100644 --- a/controller_interface/include/controller_interface/controller_interface_base.hpp +++ b/controller_interface/include/controller_interface/controller_interface_base.hpp @@ -258,12 +258,12 @@ class ControllerInterfaceBase : public rclcpp_lifecycle::node_interfaces::Lifecy protected: std::vector command_interfaces_; std::vector state_interfaces_; - unsigned int update_rate_ = 0; - bool is_async_ = false; - std::string urdf_ = ""; private: std::shared_ptr node_; + unsigned int update_rate_ = 0; + bool is_async_ = false; + std::string urdf_ = ""; }; using ControllerInterfaceBaseSharedPtr = std::shared_ptr; From 0792a35966f981f40bf977d211d89de53940e258 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 19 Jul 2024 18:31:07 +0200 Subject: [PATCH 6/6] 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 fa1fd067da1..2c564fe659e 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 37db345cb7d..a9e915dba6b 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); } }