From a31acf81a2d77d84b81cd8354801fe880dab6e63 Mon Sep 17 00:00:00 2001 From: Bence Magyar Date: Mon, 29 Jan 2024 19:16:09 +0000 Subject: [PATCH 01/20] __ -> _ load_urdf() -> load_and_initialize_components() --- controller_manager/src/controller_manager.cpp | 39 ++++++--------- .../test_controller_manager_urdf_passing.cpp | 20 +++++--- .../hardware_interface/resource_manager.hpp | 14 +++--- hardware_interface/src/resource_manager.cpp | 11 ++-- .../test/test_resource_manager.cpp | 50 ++++++++++--------- 5 files changed, 69 insertions(+), 65 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index ab57c5a196..2b390101f9 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -341,36 +341,29 @@ 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) + robot_description_ = robot_description.data; + 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; } + init_resource_manager(robot_description_); + init_services(); } 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_and_initialize_components(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(); diff --git a/controller_manager/test/test_controller_manager_urdf_passing.cpp b/controller_manager/test/test_controller_manager_urdf_passing.cpp index 5c2ebd14f6..2ea291d2a8 100644 --- a/controller_manager/test/test_controller_manager_urdf_passing.cpp +++ b/controller_manager/test/test_controller_manager_urdf_passing.cpp @@ -29,11 +29,13 @@ class TestableControllerManager : public controller_manager::ControllerManager { friend TestControllerManagerWithTestableCM; - 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, initial_no_load_and_initialize_components_called); + FRIEND_TEST( + TestControllerManagerWithTestableCM, load_and_initialize_components_called_after_callback); + FRIEND_TEST( + TestControllerManagerWithTestableCM, + load_and_initialize_components_called_after_invalid_urdf_passed); public: TestableControllerManager( @@ -59,12 +61,12 @@ class TestControllerManagerWithTestableCM } }; -TEST_P(TestControllerManagerWithTestableCM, initial_no_load_urdf_called) +TEST_P(TestControllerManagerWithTestableCM, initial_no_load_and_initialize_components_called) { ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded()); } -TEST_P(TestControllerManagerWithTestableCM, load_urdf_called_after_callback) +TEST_P(TestControllerManagerWithTestableCM, load_and_initialize_components_called_after_callback) { ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded()); // mimic callback @@ -74,7 +76,9 @@ TEST_P(TestControllerManagerWithTestableCM, load_urdf_called_after_callback) ASSERT_TRUE(cm_->resource_manager_->is_urdf_already_loaded()); } -TEST_P(TestControllerManagerWithTestableCM, load_urdf_called_after_invalid_urdf_passed) +TEST_P( + TestControllerManagerWithTestableCM, + load_and_initialize_components_called_after_invalid_urdf_passed) { ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded()); // mimic callback diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 743e548a4c..751ccf1606 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -89,17 +89,17 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * \param[in] load_and_initialize_components boolean argument indicating whether to load and * initialize the components present in the parsed URDF. Defaults to true. */ - void load_urdf( + bool load_and_initialize_components( const std::string & urdf, bool validate_interfaces = true, bool load_and_initialize_components = true); /** - * @brief if the resource manager load_urdf(...) function has been called this returns true. - * We want to permit to load the urdf later on but we currently don't want to permit multiple - * calls to load_urdf (reloading/loading different urdf). + * @brief if the resource manager load_and_initialize_components(...) function has been called + * this returns true. We want to permit to load the urdf later on but we currently don't want to + * permit multiple calls to load_and_initialize_components (reloading/loading different urdf). * - * @return true if resource manager's load_urdf() has been already called. - * @return false if resource manager's load_urdf() has not been yet called. + * @return true if resource manager's load_and_initialize_components() has been already called. + * @return false if resource manager's load_and_initialize_components() has not been yet called. */ bool is_urdf_already_loaded() const; @@ -423,7 +423,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager // Structure to store read and write status so it is not initialized in the real-time loop HardwareReadWriteStatus read_write_status; - bool is_urdf_loaded__ = false; + bool is_urdf_loaded_ = false; }; } // namespace hardware_interface diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 2e8ccc7b1f..1c9723a8ff 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -742,7 +742,7 @@ ResourceManager::ResourceManager( rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) : resource_storage_(std::make_unique(update_rate, clock_interface)) { - load_urdf(urdf, validate_interfaces); + load_and_initialize_components(urdf, validate_interfaces); if (activate_all) { @@ -756,15 +756,18 @@ ResourceManager::ResourceManager( } // CM API: Called in "callback/slow"-thread -void ResourceManager::load_urdf( +bool ResourceManager::load_and_initialize_components( const std::string & urdf, bool validate_interfaces, bool load_and_initialize_components) { - is_urdf_loaded__ = true; + bool result = true; + const std::string system_type = "system"; const std::string sensor_type = "sensor"; const std::string actuator_type = "actuator"; const auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf); + is_urdf_loaded_ = true; + if (load_and_initialize_components) { for (const auto & individual_hardware_info : hardware_info) @@ -799,7 +802,7 @@ void ResourceManager::load_urdf( resource_storage_->systems_.size()); } -bool ResourceManager::is_urdf_already_loaded() const { return is_urdf_loaded__; } +bool ResourceManager::is_urdf_already_loaded() const { return is_urdf_loaded_; } // CM API: Called in "update"-thread LoanedStateInterface ResourceManager::claim_state_interface(const std::string & key) diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index 4bb9e0c5fe..39c1813cf8 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -98,25 +98,25 @@ TEST_F(ResourceManagerTest, initialization_with_urdf) TEST_F(ResourceManagerTest, post_initialization_with_urdf) { TestableResourceManager rm; - ASSERT_NO_THROW(rm.load_urdf(ros2_control_test_assets::minimal_robot_urdf)); + ASSERT_NO_THROW(rm.load_and_initialize_components(ros2_control_test_assets::minimal_robot_urdf)); } 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 + // If the the hardware can not be initialized and load_and_initialize_components 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_and_initialize_components( + ros2_control_test_assets::minimal_unitilizable_robot_urdf, true)); } 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 + // 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 TestableResourceManager rm; - EXPECT_NO_THROW(rm.load_urdf(ros2_control_test_assets::minimal_unitilizable_robot_urdf, false)); + EXPECT_TRUE(rm.load_and_initialize_components( + ros2_control_test_assets::minimal_unitilizable_robot_urdf, false)); // test actuator EXPECT_FALSE(rm.state_interface_exists("joint1/position")); @@ -168,15 +168,17 @@ TEST_F(ResourceManagerTest, initialization_with_wrong_urdf) { // missing state keys { - EXPECT_THROW( - TestableResourceManager rm(ros2_control_test_assets::minimal_robot_missing_state_keys_urdf), - std::exception); + auto rm = TestableResourceManager(); + EXPECT_FALSE(rm.load_and_initialize_components( + ros2_control_test_assets::minimal_robot_missing_state_keys_urdf)); + ASSERT_TRUE(rm.is_urdf_already_loaded()); } // missing command keys { - EXPECT_THROW( - TestableResourceManager rm(ros2_control_test_assets::minimal_robot_missing_command_keys_urdf), - std::exception); + auto rm = TestableResourceManager(); + EXPECT_FALSE(rm.load_and_initialize_components( + ros2_control_test_assets::minimal_robot_missing_command_keys_urdf)); + ASSERT_TRUE(rm.is_urdf_already_loaded()); } } @@ -199,31 +201,33 @@ TEST_F(ResourceManagerTest, initialization_with_urdf_unclaimed) } } -TEST_F(ResourceManagerTest, no_load_urdf_function_called) +TEST_F(ResourceManagerTest, no_load_and_initialize_components_function_called) { TestableResourceManager rm; ASSERT_FALSE(rm.is_urdf_already_loaded()); } -TEST_F(ResourceManagerTest, load_urdf_called_if_urdf_is_invalid) +TEST_F(ResourceManagerTest, load_and_initialize_components_called_if_urdf_is_valid) { TestableResourceManager rm; - EXPECT_THROW( - rm.load_urdf(ros2_control_test_assets::minimal_robot_missing_state_keys_urdf), std::exception); - ASSERT_TRUE(rm.is_urdf_already_loaded()); + ASSERT_FALSE( + rm.load_and_initialize_components( + ros2_control_test_assets::minimal_robot_missing_state_keys_urdf), + std::exception); + ASSERT_FALSE(rm.is_urdf_already_loaded()); } -TEST_F(ResourceManagerTest, load_urdf_called_if_urdf_is_valid) +TEST_F(ResourceManagerTest, load_and_initialize_components_called_if_urdf_is_valid) { TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); ASSERT_TRUE(rm.is_urdf_already_loaded()); } -TEST_F(ResourceManagerTest, can_load_urdf_later) +TEST_F(ResourceManagerTest, can_load_and_initialize_components_later) { TestableResourceManager rm; ASSERT_FALSE(rm.is_urdf_already_loaded()); - rm.load_urdf(ros2_control_test_assets::minimal_robot_urdf); + rm.load_and_initialize_components(ros2_control_test_assets::minimal_robot_urdf); ASSERT_TRUE(rm.is_urdf_already_loaded()); } From c47ac6551930b0fa4d1ea5ddc48923dc02f50106 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 02/20] Added functionality and tests for getting robot description from the topic. --- controller_manager/CMakeLists.txt | 1 + .../controller_manager/controller_manager.hpp | 3 +- .../test/test_spawner_unspawner.cpp | 80 +++++++++++++++++++ .../hardware_interface/resource_manager.hpp | 3 +- hardware_interface/src/resource_manager.cpp | 32 +++++--- .../test/test_resource_manager.cpp | 8 +- 6 files changed, 110 insertions(+), 17 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/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); + } +} diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 751ccf1606..b7b7a414bb 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -88,6 +88,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * interfaces ought to be validated. Defaults to true. * \param[in] load_and_initialize_components boolean argument indicating whether to load and * initialize the components present in the parsed URDF. Defaults to true. + * \returns false if URDF validation has failed. */ bool load_and_initialize_components( const std::string & urdf, bool validate_interfaces = true, @@ -408,7 +409,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager bool state_interface_exists(const std::string & key) const; private: - void validate_storage(const std::vector & hardware_info) const; + bool validate_storage(const std::vector & hardware_info) const; void release_command_interface(const std::string & key); diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 1c9723a8ff..6e6f09632c 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -60,7 +60,7 @@ auto trigger_and_print_hardware_state_transition = } else { - RCUTILS_LOG_INFO_NAMED( + RCUTILS_LOG_WARN_NAMED( "resource_manager", "Failed to '%s' hardware '%s'", transition_name.c_str(), hardware_name.c_str()); } @@ -433,6 +433,7 @@ class ResourceStorage template void import_state_interfaces(HardwareT & hardware) { + // TODO(destogl): add here exception catch block - this can otherwise crash the whole system auto interfaces = hardware.export_state_interfaces(); std::vector interface_names; interface_names.reserve(interfaces.size()); @@ -450,6 +451,7 @@ class ResourceStorage template void import_command_interfaces(HardwareT & hardware) { + // TODO(destogl): add here exception catch block - this can otherwise crash the whole system auto interfaces = hardware.export_command_interfaces(); hardware_info_map_[hardware.get_name()].command_interfaces = add_command_interfaces(interfaces); } @@ -551,7 +553,7 @@ class ResourceStorage } else { - RCUTILS_LOG_WARN_NAMED( + RCUTILS_LOG_ERROR_NAMED( "resource_manager", "Sensor hardware component '%s' from plugin '%s' failed to initialize.", hardware_info.name.c_str(), hardware_info.hardware_plugin_name.c_str()); @@ -793,13 +795,15 @@ bool ResourceManager::load_and_initialize_components( // throw on missing state and command interfaces, not specified keys are being ignored if (validate_interfaces) { - validate_storage(hardware_info); + result = validate_storage(hardware_info); } std::lock_guard guard(resources_lock_); read_write_status.failed_hardware_names.reserve( resource_storage_->actuators_.size() + resource_storage_->sensors_.size() + resource_storage_->systems_.size()); + + return result; } bool ResourceManager::is_urdf_already_loaded() const { return is_urdf_loaded_; } @@ -1433,7 +1437,7 @@ bool ResourceManager::state_interface_exists(const std::string & key) const // BEGIN: private methods -void ResourceManager::validate_storage( +bool ResourceManager::validate_storage( const std::vector & hardware_info) const { std::vector missing_state_keys = {}; @@ -1489,20 +1493,28 @@ void ResourceManager::validate_storage( if (!missing_state_keys.empty() || !missing_command_keys.empty()) { - std::string err_msg = "Wrong state or command interface configuration.\n"; - err_msg += "missing state interfaces:\n"; + std::string message = "Wrong state or command interface configuration.\n"; + message += "missing state interfaces:\n"; for (const auto & missing_key : missing_state_keys) { - err_msg += "' " + missing_key + " '" + "\t"; + message += "' " + missing_key + " '" + "\t"; } - err_msg += "\nmissing command interfaces:\n"; + message += "\nmissing command interfaces:\n"; for (const auto & missing_key : missing_command_keys) { - err_msg += "' " + missing_key + " '" + "\t"; + message += "' " + missing_key + " '" + "\t"; } - throw std::runtime_error(err_msg); + RCUTILS_LOG_ERROR_NAMED( + "resource_manager", + "Discrepancy between robot description file (urdf) and actually exported HW interfaces." + "Details: %s", + message.c_str()); + + return false; } + + return true; } // END: private methods diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index 39c1813cf8..d534c2488a 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -207,13 +207,11 @@ TEST_F(ResourceManagerTest, no_load_and_initialize_components_function_called) ASSERT_FALSE(rm.is_urdf_already_loaded()); } -TEST_F(ResourceManagerTest, load_and_initialize_components_called_if_urdf_is_valid) +TEST_F(ResourceManagerTest, load_and_initialize_components_called_if_urdf_is_invalid) { TestableResourceManager rm; - ASSERT_FALSE( - rm.load_and_initialize_components( - ros2_control_test_assets::minimal_robot_missing_state_keys_urdf), - std::exception); + ASSERT_FALSE(rm.load_and_initialize_components( + ros2_control_test_assets::minimal_robot_missing_state_keys_urdf)); ASSERT_FALSE(rm.is_urdf_already_loaded()); } From c1ba8bc140f00eca491bd2b27ada0759f8a894ea Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Wed, 31 Jan 2024 20:57:23 +0100 Subject: [PATCH 03/20] Fixed compilation and tests. --- controller_manager/CMakeLists.txt | 1 - .../controller_manager/controller_manager.hpp | 3 +- .../test/test_spawner_unspawner.cpp | 80 ------------------- .../test/test_resource_manager.hpp | 2 +- 4 files changed, 2 insertions(+), 84 deletions(-) diff --git a/controller_manager/CMakeLists.txt b/controller_manager/CMakeLists.txt index 0d1b88a55a..376cef3362 100644 --- a/controller_manager/CMakeLists.txt +++ b/controller_manager/CMakeLists.txt @@ -150,7 +150,6 @@ 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 c0a22fe5bd..f5ee6854a1 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 init_controller_manager(); + void subscribe_to_robot_description_topic(); /** * Clear request lists used when switching controllers. The lists are shared between "callback" @@ -546,7 +546,6 @@ 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/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 353f6738ab..1ecc3164b8 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -31,7 +31,6 @@ using ::testing::Return; using namespace std::chrono_literals; class TestLoadController : public ControllerManagerFixture { -public: void SetUp() override { ControllerManagerFixture::SetUp(); @@ -275,82 +274,3 @@ 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); - } -} diff --git a/hardware_interface_testing/test/test_resource_manager.hpp b/hardware_interface_testing/test/test_resource_manager.hpp index 46a487f2ef..c9bd17490c 100644 --- a/hardware_interface_testing/test/test_resource_manager.hpp +++ b/hardware_interface_testing/test/test_resource_manager.hpp @@ -44,7 +44,7 @@ class TestableResourceManager : public hardware_interface::ResourceManager public: friend ResourceManagerTest; - FRIEND_TEST(ResourceManagerTest, initialization_with_urdf_manual_validation); + FRIEND_TEST(ResourceManagerTest, initialization_with_urdf_and_manual_validation); FRIEND_TEST(ResourceManagerTest, post_initialization_add_components); FRIEND_TEST(ResourceManagerTest, managing_controllers_reference_interfaces); FRIEND_TEST(ResourceManagerTest, resource_availability_and_claiming_in_lifecycle); From 26585af2095d67e592c757e65f056e0b1f4995a0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Thu, 1 Feb 2024 17:03:37 +0100 Subject: [PATCH 04/20] Don't throw exception if something goes wrong with loading and initialization of the hardware components. --- controller_manager/src/controller_manager.cpp | 23 +- .../test_controller_manager_urdf_passing.cpp | 10 +- .../hardware_interface/resource_manager.hpp | 11 +- hardware_interface/src/resource_manager.cpp | 191 +++++++---- .../mock_components/test_generic_system.cpp | 5 +- .../test/test_components/test_actuator.cpp | 5 + .../test/test_components/test_sensor.cpp | 2 +- .../test/test_components/test_system.cpp | 16 + .../test/test_resource_manager.cpp | 91 ++++-- .../test/test_resource_manager.hpp | 5 +- .../ros2_control_test_assets/descriptions.hpp | 300 +++++++++++++++++- 11 files changed, 536 insertions(+), 123 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 2b390101f9..82ee99bc4c 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -286,7 +286,13 @@ ControllerManager::ControllerManager( "[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(); + if (resource_manager_->are_components_initialized()) + { + RCLCPP_FATAL( + get_logger(), + "You have to restart the framework when using robot description from parameter!"); + init_services(); + } } diagnostics_updater_.setHardwareID("ros2_control"); @@ -313,7 +319,7 @@ ControllerManager::ControllerManager( RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value."); } - if (resource_manager_->is_urdf_already_loaded()) + if (resource_manager_->are_components_initialized()) { init_services(); } @@ -342,7 +348,7 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String & RCLCPP_DEBUG( get_logger(), "'Content of robot description file: %s", robot_description.data.c_str()); robot_description_ = robot_description.data; - if (resource_manager_->is_urdf_already_loaded()) + if (resource_manager_->are_components_initialized()) { RCLCPP_WARN( get_logger(), @@ -351,7 +357,10 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String & return; } init_resource_manager(robot_description_); - init_services(); + if (resource_manager_->are_components_initialized()) + { + init_services(); + } } void ControllerManager::init_resource_manager(const std::string & robot_description) @@ -360,9 +369,9 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript { 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..."); + "Could not load and initialize hardware. Please check previous output for more details. " + "After you have corrected your URDF, try to publish robot description again."); + return; } // Get all components and if they are not defined in parameters activate them automatically diff --git a/controller_manager/test/test_controller_manager_urdf_passing.cpp b/controller_manager/test/test_controller_manager_urdf_passing.cpp index 2ea291d2a8..602078c292 100644 --- a/controller_manager/test/test_controller_manager_urdf_passing.cpp +++ b/controller_manager/test/test_controller_manager_urdf_passing.cpp @@ -63,29 +63,29 @@ class TestControllerManagerWithTestableCM TEST_P(TestControllerManagerWithTestableCM, initial_no_load_and_initialize_components_called) { - ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded()); + ASSERT_FALSE(cm_->resource_manager_->are_components_initialized()); } TEST_P(TestControllerManagerWithTestableCM, load_and_initialize_components_called_after_callback) { - ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded()); + ASSERT_FALSE(cm_->resource_manager_->are_components_initialized()); // mimic callback auto msg = std_msgs::msg::String(); msg.data = ros2_control_test_assets::minimal_robot_urdf; cm_->robot_description_callback(msg); - ASSERT_TRUE(cm_->resource_manager_->is_urdf_already_loaded()); + ASSERT_TRUE(cm_->resource_manager_->are_components_initialized()); } TEST_P( TestControllerManagerWithTestableCM, load_and_initialize_components_called_after_invalid_urdf_passed) { - ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded()); + ASSERT_FALSE(cm_->resource_manager_->are_components_initialized()); // 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()); + ASSERT_TRUE(cm_->resource_manager_->are_components_initialized()); } INSTANTIATE_TEST_SUITE_P( diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index b7b7a414bb..65605ffb74 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -69,8 +69,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * activated. Currently used only in tests. */ explicit ResourceManager( - const std::string & urdf, bool validate_interfaces = true, bool activate_all = false, - unsigned int update_rate = 100, + const std::string & urdf, bool activate_all = false, unsigned int update_rate = 100, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr); ResourceManager(const ResourceManager &) = delete; @@ -90,9 +89,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * initialize the components present in the parsed URDF. Defaults to true. * \returns false if URDF validation has failed. */ - bool load_and_initialize_components( - const std::string & urdf, bool validate_interfaces = true, - bool load_and_initialize_components = true); + bool load_and_initialize_components(const std::string & urdf); /** * @brief if the resource manager load_and_initialize_components(...) function has been called @@ -102,7 +99,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * @return true if resource manager's load_and_initialize_components() has been already called. * @return false if resource manager's load_and_initialize_components() has not been yet called. */ - bool is_urdf_already_loaded() const; + bool are_components_initialized() const; /// Claim a state interface given its key. /** @@ -424,7 +421,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager // Structure to store read and write status so it is not initialized in the real-time loop HardwareReadWriteStatus read_write_status; - bool is_urdf_loaded_ = false; + bool components_are_loaded_and_initialized_ = false; }; } // namespace hardware_interface diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 6e6f09632c..b19cddb73b 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -90,17 +90,29 @@ class ResourceStorage } template - void load_hardware( + bool load_hardware( const HardwareInfo & hardware_info, pluginlib::ClassLoader & loader, std::vector & container) { RCUTILS_LOG_INFO_NAMED( "resource_manager", "Loading hardware '%s' ", hardware_info.name.c_str()); // hardware_plugin_name has to match class name in plugin xml description - auto interface = std::unique_ptr( - loader.createUnmanagedInstance(hardware_info.hardware_plugin_name)); - HardwareT hardware(std::move(interface)); - container.emplace_back(std::move(hardware)); + try + { + auto interface = std::unique_ptr( + loader.createUnmanagedInstance(hardware_info.hardware_plugin_name)); + HardwareT hardware(std::move(interface)); + container.emplace_back(std::move(hardware)); + } + catch (const std::exception & ex) + { + RCUTILS_LOG_ERROR_NAMED( + "resource_manager", + "The following exception was raised when creating instance of '%s' " + "from plugin '%s' component! %s", + hardware_info.name.c_str(), hardware_info.hardware_plugin_name.c_str(), ex.what()); + return false; + } // initialize static data about hardware component to reduce later calls HardwareComponentInfo component_info; component_info.name = hardware_info.name; @@ -111,6 +123,8 @@ class ResourceStorage hardware_info_map_.insert(std::make_pair(component_info.name, component_info)); hardware_used_by_controllers_.insert( std::make_pair(component_info.name, std::vector())); + + return true; } template @@ -499,24 +513,15 @@ class ResourceStorage } } - void check_for_duplicates(const HardwareInfo & hardware_info) - { - // Check for identical names - if (hardware_info_map_.find(hardware_info.name) != hardware_info_map_.end()) - { - throw std::runtime_error( - "Hardware name " + hardware_info.name + - " is duplicated. Please provide a unique 'name' in the URDF."); - } - } - // TODO(destogl): Propagate "false" up, if happens in initialize_hardware - void load_and_initialize_actuator(const HardwareInfo & hardware_info) + bool load_and_initialize_actuator(const HardwareInfo & hardware_info) { auto load_and_init_actuators = [&](auto & container) { - check_for_duplicates(hardware_info); - load_hardware(hardware_info, actuator_loader_, container); + if (!load_hardware(hardware_info, actuator_loader_, container)) + { + return false; + } if (initialize_hardware(hardware_info, container.back())) { import_state_interfaces(container.back()); @@ -524,29 +529,33 @@ class ResourceStorage } else { - RCUTILS_LOG_WARN_NAMED( + RCUTILS_LOG_ERROR_NAMED( "resource_manager", "Actuator hardware component '%s' from plugin '%s' failed to initialize.", hardware_info.name.c_str(), hardware_info.hardware_plugin_name.c_str()); + return false; } + return true; }; if (hardware_info.is_async) { - load_and_init_actuators(async_actuators_); + return load_and_init_actuators(async_actuators_); } else { - load_and_init_actuators(actuators_); + return load_and_init_actuators(actuators_); } } - void load_and_initialize_sensor(const HardwareInfo & hardware_info) + bool load_and_initialize_sensor(const HardwareInfo & hardware_info) { auto load_and_init_sensors = [&](auto & container) { - check_for_duplicates(hardware_info); - load_hardware(hardware_info, sensor_loader_, container); + if (!load_hardware(hardware_info, sensor_loader_, container)) + { + return false; + } if (initialize_hardware(hardware_info, container.back())) { import_state_interfaces(container.back()); @@ -557,25 +566,29 @@ class ResourceStorage "resource_manager", "Sensor hardware component '%s' from plugin '%s' failed to initialize.", hardware_info.name.c_str(), hardware_info.hardware_plugin_name.c_str()); + return false; } + return true; }; if (hardware_info.is_async) { - load_and_init_sensors(async_sensors_); + return load_and_init_sensors(async_sensors_); } else { - load_and_init_sensors(sensors_); + return load_and_init_sensors(sensors_); } } - void load_and_initialize_system(const HardwareInfo & hardware_info) + bool load_and_initialize_system(const HardwareInfo & hardware_info) { auto load_and_init_systems = [&](auto & container) { - check_for_duplicates(hardware_info); - load_hardware(hardware_info, system_loader_, container); + if (!load_hardware(hardware_info, system_loader_, container)) + { + return false; + } if (initialize_hardware(hardware_info, container.back())) { import_state_interfaces(container.back()); @@ -583,20 +596,22 @@ class ResourceStorage } else { - RCUTILS_LOG_WARN_NAMED( + RCUTILS_LOG_ERROR_NAMED( "resource_manager", "System hardware component '%s' from plugin '%s' failed to initialize.", hardware_info.name.c_str(), hardware_info.hardware_plugin_name.c_str()); + return false; } + return true; }; if (hardware_info.is_async) { - load_and_init_systems(async_systems_); + return load_and_init_systems(async_systems_); } else { - load_and_init_systems(systems_); + return load_and_init_systems(systems_); } } @@ -689,6 +704,26 @@ class ResourceStorage } } + void clear() + { + actuators_.clear(); + sensors_.clear(); + systems_.clear(); + + async_actuators_.clear(); + async_sensors_.clear(); + async_systems_.clear(); + + hardware_info_map_.clear(); + state_interface_map_.clear(); + command_interface_map_.clear(); + + available_state_interfaces_.clear(); + available_command_interfaces_.clear(); + + claimed_command_interface_map_.clear(); + } + // hardware plugins pluginlib::ClassLoader actuator_loader_; pluginlib::ClassLoader sensor_loader_; @@ -740,11 +775,11 @@ ResourceManager::ResourceManager( ResourceManager::~ResourceManager() = default; ResourceManager::ResourceManager( - const std::string & urdf, bool validate_interfaces, bool activate_all, unsigned int update_rate, + const std::string & urdf, bool activate_all, unsigned int update_rate, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) : resource_storage_(std::make_unique(update_rate, clock_interface)) { - load_and_initialize_components(urdf, validate_interfaces); + load_and_initialize_components(urdf); if (activate_all) { @@ -758,55 +793,83 @@ ResourceManager::ResourceManager( } // CM API: Called in "callback/slow"-thread -bool ResourceManager::load_and_initialize_components( - const std::string & urdf, bool validate_interfaces, bool load_and_initialize_components) +bool ResourceManager::load_and_initialize_components(const std::string & urdf) { - bool result = true; + components_are_loaded_and_initialized_ = true; + + const auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf); const std::string system_type = "system"; const std::string sensor_type = "sensor"; const std::string actuator_type = "actuator"; - const auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf); - is_urdf_loaded_ = true; - - if (load_and_initialize_components) + for (const auto & individual_hardware_info : hardware_info) { - for (const auto & individual_hardware_info : hardware_info) + // Check for identical names + if ( + resource_storage_->hardware_info_map_.find(individual_hardware_info.name) != + resource_storage_->hardware_info_map_.end()) { - if (individual_hardware_info.type == actuator_type) + RCUTILS_LOG_ERROR_NAMED( + "resource_manager", + "Hardware name %s is duplicated. Please provide a unique 'name' " + "in the URDF.", + individual_hardware_info.name.c_str()); + components_are_loaded_and_initialized_ = false; + break; + } + + if (individual_hardware_info.type == actuator_type) + { + std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_); + if (!resource_storage_->load_and_initialize_actuator(individual_hardware_info)) { - std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_); - resource_storage_->load_and_initialize_actuator(individual_hardware_info); + components_are_loaded_and_initialized_ = false; + break; } - if (individual_hardware_info.type == sensor_type) + } + if (individual_hardware_info.type == sensor_type) + { + std::lock_guard guard(resource_interfaces_lock_); + if (!resource_storage_->load_and_initialize_sensor(individual_hardware_info)) { - std::lock_guard guard(resource_interfaces_lock_); - resource_storage_->load_and_initialize_sensor(individual_hardware_info); + components_are_loaded_and_initialized_ = false; + break; } - if (individual_hardware_info.type == system_type) + } + if (individual_hardware_info.type == system_type) + { + std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_); + if (!resource_storage_->load_and_initialize_system(individual_hardware_info)) { - std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_); - resource_storage_->load_and_initialize_system(individual_hardware_info); + components_are_loaded_and_initialized_ = false; + break; } } } - // throw on missing state and command interfaces, not specified keys are being ignored - if (validate_interfaces) + if (components_are_loaded_and_initialized_ && validate_storage(hardware_info)) { - result = validate_storage(hardware_info); + std::lock_guard guard(resources_lock_); + read_write_status.failed_hardware_names.reserve( + resource_storage_->actuators_.size() + resource_storage_->sensors_.size() + + resource_storage_->systems_.size()); + } + else + { + std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_); + resource_storage_->clear(); + // cleanup everything + components_are_loaded_and_initialized_ = false; } - std::lock_guard guard(resources_lock_); - read_write_status.failed_hardware_names.reserve( - resource_storage_->actuators_.size() + resource_storage_->sensors_.size() + - resource_storage_->systems_.size()); - - return result; + return components_are_loaded_and_initialized_; } -bool ResourceManager::is_urdf_already_loaded() const { return is_urdf_loaded_; } +bool ResourceManager::are_components_initialized() const +{ + return components_are_loaded_and_initialized_; +} // CM API: Called in "update"-thread LoanedStateInterface ResourceManager::claim_state_interface(const std::string & key) @@ -1497,12 +1560,12 @@ bool ResourceManager::validate_storage( message += "missing state interfaces:\n"; for (const auto & missing_key : missing_state_keys) { - message += "' " + missing_key + " '" + "\t"; + message += " " + missing_key + "\t"; } message += "\nmissing command interfaces:\n"; for (const auto & missing_key : missing_command_keys) { - message += "' " + missing_key + " '" + "\t"; + message += " " + missing_key + "\t"; } RCUTILS_LOG_ERROR_NAMED( diff --git a/hardware_interface/test/mock_components/test_generic_system.cpp b/hardware_interface/test/mock_components/test_generic_system.cpp index 0641cfda57..a6b578d6c5 100644 --- a/hardware_interface/test/mock_components/test_generic_system.cpp +++ b/hardware_interface/test/mock_components/test_generic_system.cpp @@ -630,9 +630,8 @@ class TestableResourceManager : public hardware_interface::ResourceManager TestableResourceManager() : hardware_interface::ResourceManager() {} - TestableResourceManager( - const std::string & urdf, bool validate_interfaces = true, bool activate_all = false) - : hardware_interface::ResourceManager(urdf, validate_interfaces, activate_all) + explicit TestableResourceManager(const std::string & urdf, bool activate_all = false) + : hardware_interface::ResourceManager(urdf, activate_all) { } }; diff --git a/hardware_interface_testing/test/test_components/test_actuator.cpp b/hardware_interface_testing/test/test_components/test_actuator.cpp index 41f27e201b..bb54ea9443 100644 --- a/hardware_interface_testing/test/test_components/test_actuator.cpp +++ b/hardware_interface_testing/test/test_components/test_actuator.cpp @@ -32,6 +32,11 @@ class TestActuator : public ActuatorInterface return CallbackReturn::ERROR; } + if (info_.joints[0].state_interfaces[1].name == "does_not_exist") + { + return CallbackReturn::ERROR; + } + /* * a hardware can optional prove for incorrect info here. * diff --git a/hardware_interface_testing/test/test_components/test_sensor.cpp b/hardware_interface_testing/test/test_components/test_sensor.cpp index 4811244138..2c414aebb4 100644 --- a/hardware_interface_testing/test/test_components/test_sensor.cpp +++ b/hardware_interface_testing/test/test_components/test_sensor.cpp @@ -30,7 +30,7 @@ class TestSensor : public SensorInterface return CallbackReturn::ERROR; } // can only give feedback state for velocity - if (info_.sensors[0].state_interfaces.size() != 1) + if (info_.sensors[0].state_interfaces.size() == 2) { return CallbackReturn::ERROR; } diff --git a/hardware_interface_testing/test/test_components/test_system.cpp b/hardware_interface_testing/test/test_components/test_system.cpp index bc7a75df6f..8fb3098822 100644 --- a/hardware_interface_testing/test/test_components/test_system.cpp +++ b/hardware_interface_testing/test/test_components/test_system.cpp @@ -27,6 +27,22 @@ using hardware_interface::SystemInterface; class TestSystem : public SystemInterface { + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override + { + if (SystemInterface::on_init(info) != CallbackReturn::SUCCESS) + { + return CallbackReturn::ERROR; + } + + // Simulating initialization error + if (info_.joints[0].state_interfaces[1].name == "does_not_exist") + { + return CallbackReturn::ERROR; + } + + return CallbackReturn::SUCCESS; + } + std::vector export_state_interfaces() override { std::vector state_interfaces; diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index d534c2488a..730410ddd7 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -106,17 +106,21 @@ TEST_F(ResourceManagerTest, test_unitilizable_hardware_validation) // If the the hardware can not be initialized and load_and_initialize_components tried to validate // the interfaces a runtime exception is thrown TestableResourceManager rm; - EXPECT_FALSE(rm.load_and_initialize_components( - ros2_control_test_assets::minimal_unitilizable_robot_urdf, true)); + EXPECT_FALSE( + rm.load_and_initialize_components(ros2_control_test_assets::minimal_unitilizable_robot_urdf)); } -TEST_F(ResourceManagerTest, test_unitilizable_hardware_no_validation) +void test_load_and_initialized_components_failure(const std::string & urdf) { - // 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 TestableResourceManager rm; - EXPECT_TRUE(rm.load_and_initialize_components( - ros2_control_test_assets::minimal_unitilizable_robot_urdf, false)); + ASSERT_FALSE(rm.load_and_initialize_components(urdf)); + + ASSERT_FALSE(rm.are_components_initialized()); + + // resource manage should also not have any components + EXPECT_EQ(rm.actuator_components_size(), 0); + EXPECT_EQ(rm.sensor_components_size(), 0); + EXPECT_EQ(rm.system_components_size(), 0); // test actuator EXPECT_FALSE(rm.state_interface_exists("joint1/position")); @@ -140,7 +144,15 @@ TEST_F(ResourceManagerTest, test_unitilizable_hardware_no_validation) EXPECT_FALSE(rm.command_interface_exists("joint3/max_acceleration")); } -TEST_F(ResourceManagerTest, initialization_with_urdf_manual_validation) +TEST_F(ResourceManagerTest, test_unitilizable_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( + ros2_control_test_assets::minimal_unitilizable_robot_urdf); +} + +TEST_F(ResourceManagerTest, initialization_with_urdf_and_manual_validation) { // we validate the results manually TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf, false); @@ -164,22 +176,15 @@ TEST_F(ResourceManagerTest, initialization_with_urdf_manual_validation) EXPECT_TRUE(rm.command_interface_exists("joint3/velocity")); } -TEST_F(ResourceManagerTest, initialization_with_wrong_urdf) +TEST_F(ResourceManagerTest, expect_validation_failure_if_not_all_interfaces_are_exported) { // missing state keys - { - auto rm = TestableResourceManager(); - EXPECT_FALSE(rm.load_and_initialize_components( - ros2_control_test_assets::minimal_robot_missing_state_keys_urdf)); - ASSERT_TRUE(rm.is_urdf_already_loaded()); - } + test_load_and_initialized_components_failure( + ros2_control_test_assets::minimal_robot_missing_state_keys_urdf); + // missing command keys - { - auto rm = TestableResourceManager(); - EXPECT_FALSE(rm.load_and_initialize_components( - ros2_control_test_assets::minimal_robot_missing_command_keys_urdf)); - ASSERT_TRUE(rm.is_urdf_already_loaded()); - } + test_load_and_initialized_components_failure( + ros2_control_test_assets::minimal_robot_missing_command_keys_urdf); } TEST_F(ResourceManagerTest, initialization_with_urdf_unclaimed) @@ -204,29 +209,53 @@ TEST_F(ResourceManagerTest, initialization_with_urdf_unclaimed) TEST_F(ResourceManagerTest, no_load_and_initialize_components_function_called) { TestableResourceManager rm; - ASSERT_FALSE(rm.is_urdf_already_loaded()); + ASSERT_FALSE(rm.are_components_initialized()); } -TEST_F(ResourceManagerTest, load_and_initialize_components_called_if_urdf_is_invalid) +TEST_F( + ResourceManagerTest, expect_load_and_initialize_to_fail_when_a_hw_component_plugin_does_not_exist) { - TestableResourceManager rm; - ASSERT_FALSE(rm.load_and_initialize_components( - ros2_control_test_assets::minimal_robot_missing_state_keys_urdf)); - ASSERT_FALSE(rm.is_urdf_already_loaded()); + // Actuator + test_load_and_initialized_components_failure( + ros2_control_test_assets::minimal_robot_not_existing_actuator_plugin); + + // Sensor + test_load_and_initialized_components_failure( + ros2_control_test_assets::minimal_robot_not_existing_sensors_plugin); + + // System + test_load_and_initialized_components_failure( + ros2_control_test_assets::minimal_robot_not_existing_system_plugin); +} + +TEST_F( + ResourceManagerTest, expect_load_and_initialize_to_fail_when_a_hw_component_initialization_fails) +{ + // Actuator + test_load_and_initialized_components_failure( + ros2_control_test_assets::minimal_robot_actuator_initialization_error); + + // Sensor + test_load_and_initialized_components_failure( + ros2_control_test_assets::minimal_robot_sensor_initialization_error); + + // System + test_load_and_initialized_components_failure( + ros2_control_test_assets::minimal_robot_system_initialization_error); } TEST_F(ResourceManagerTest, load_and_initialize_components_called_if_urdf_is_valid) { TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); - ASSERT_TRUE(rm.is_urdf_already_loaded()); + ASSERT_TRUE(rm.are_components_initialized()); } TEST_F(ResourceManagerTest, can_load_and_initialize_components_later) { TestableResourceManager rm; - ASSERT_FALSE(rm.is_urdf_already_loaded()); + ASSERT_FALSE(rm.are_components_initialized()); rm.load_and_initialize_components(ros2_control_test_assets::minimal_robot_urdf); - ASSERT_TRUE(rm.is_urdf_already_loaded()); + ASSERT_TRUE(rm.are_components_initialized()); } TEST_F(ResourceManagerTest, resource_claiming) @@ -345,7 +374,7 @@ class ExternalComponent : public hardware_interface::ActuatorInterface TEST_F(ResourceManagerTest, post_initialization_add_components) { // we validate the results manually - TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf, false); + TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); // Activate components to get all interfaces available activate_components(rm); diff --git a/hardware_interface_testing/test/test_resource_manager.hpp b/hardware_interface_testing/test/test_resource_manager.hpp index c9bd17490c..72cf9e941b 100644 --- a/hardware_interface_testing/test/test_resource_manager.hpp +++ b/hardware_interface_testing/test/test_resource_manager.hpp @@ -52,9 +52,8 @@ class TestableResourceManager : public hardware_interface::ResourceManager TestableResourceManager() : hardware_interface::ResourceManager() {} - TestableResourceManager( - const std::string & urdf, bool validate_interfaces = true, bool activate_all = false) - : hardware_interface::ResourceManager(urdf, validate_interfaces, activate_all) + explicit TestableResourceManager(const std::string & urdf, bool activate_all = false) + : hardware_interface::ResourceManager(urdf, activate_all) { } }; diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp index a2f6195c67..713453bf62 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp @@ -225,7 +225,194 @@ const auto unitilizable_hardware_resources = )"; -const auto hardware_resources_missing_state_keys = +const auto hardware_resources_not_existing_actuator_plugin = + R"( + + + test_actuator23 + + + + + + + + + + + test_sensor + 2 + 2 + + + + + + + + test_system + 2 + 2 + + + + + + + + + + + + + + + + + + + +)"; + +const auto hardware_resources_not_existing_sensor_plugin = + R"( + + + test_actuator + + + + + + + + + + + test_sensor23 + 2 + 2 + + + + + + + + test_system + 2 + 2 + + + + + + + + + + + + + + + + + + + +)"; +const auto hardware_resources_not_existing_system_plugin = + R"( + + + test_actuator + + + + + + + + + + + test_sensor + 2 + 2 + + + + + + + + test_system23 + 2 + 2 + + + + + + + + + + + + + + + + + + + +)"; + +const auto hardware_resources_actuator_initializaion_error = + R"( + + + test_actuator + + + + + + + + + + test_sensor + 2 + 2 + + + + + + + + test_system + 2 + 2 + + + + + + + + + + + + +)"; + +const auto hardware_resources_sensor_initializaion_error = R"( @@ -248,6 +435,47 @@ const auto hardware_resources_missing_state_keys = + + + test_system + 2 + 2 + + + + + + + + + + + + +)"; + +const auto hardware_resources_system_initializaion_error = + R"( + + + test_actuator + + + + + + + + + + test_sensor + 2 + 2 + + + + + test_system @@ -267,6 +495,52 @@ const auto hardware_resources_missing_state_keys = )"; +const auto hardware_resources_missing_state_keys = + R"( + + + test_actuator + + + + + + + + + + + test_sensor + 2 + 2 + + + + + + + + + + test_system + 2 + 2 + + + + + + + + + + + + + + +)"; + const auto hardware_resources_missing_command_keys = R"( @@ -298,13 +572,15 @@ const auto hardware_resources_missing_command_keys = - + + + )"; @@ -458,6 +734,26 @@ const auto minimal_robot_urdf = const auto minimal_unitilizable_robot_urdf = std::string(urdf_head) + std::string(unitilizable_hardware_resources) + std::string(urdf_tail); +const auto minimal_robot_not_existing_actuator_plugin = + std::string(urdf_head) + std::string(hardware_resources_not_existing_actuator_plugin) + + std::string(urdf_tail); +const auto minimal_robot_not_existing_sensors_plugin = + std::string(urdf_head) + std::string(hardware_resources_not_existing_sensor_plugin) + + std::string(urdf_tail); +const auto minimal_robot_not_existing_system_plugin = + std::string(urdf_head) + std::string(hardware_resources_not_existing_system_plugin) + + std::string(urdf_tail); + +const auto minimal_robot_actuator_initialization_error = + std::string(urdf_head) + std::string(hardware_resources_not_existing_actuator_plugin) + + std::string(urdf_tail); +const auto minimal_robot_sensor_initialization_error = + std::string(urdf_head) + std::string(hardware_resources_sensor_initializaion_error) + + std::string(urdf_tail); +const auto minimal_robot_system_initialization_error = + std::string(urdf_head) + std::string(hardware_resources_system_initializaion_error) + + std::string(urdf_tail); + const auto minimal_robot_missing_state_keys_urdf = std::string(urdf_head) + std::string(hardware_resources_missing_state_keys) + std::string(urdf_tail); From 62e4384526567b9ed987be05fc5eae18028d939c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Thu, 1 Feb 2024 17:15:46 +0100 Subject: [PATCH 05/20] Make sure that we can send new robot description over topic is the first one fails. --- .../test/test_controller_manager_urdf_passing.cpp | 13 +++++++++---- hardware_interface/src/resource_manager.cpp | 2 +- 2 files changed, 10 insertions(+), 5 deletions(-) diff --git a/controller_manager/test/test_controller_manager_urdf_passing.cpp b/controller_manager/test/test_controller_manager_urdf_passing.cpp index 602078c292..e71b6ca4fe 100644 --- a/controller_manager/test/test_controller_manager_urdf_passing.cpp +++ b/controller_manager/test/test_controller_manager_urdf_passing.cpp @@ -35,7 +35,7 @@ class TestableControllerManager : public controller_manager::ControllerManager TestControllerManagerWithTestableCM, load_and_initialize_components_called_after_callback); FRIEND_TEST( TestControllerManagerWithTestableCM, - load_and_initialize_components_called_after_invalid_urdf_passed); + expect_to_failure_when_invalid_urdf_is_given_and_be_able_to_submit_new_robot_description); public: TestableControllerManager( @@ -78,12 +78,17 @@ TEST_P(TestControllerManagerWithTestableCM, load_and_initialize_components_calle TEST_P( TestControllerManagerWithTestableCM, - load_and_initialize_components_called_after_invalid_urdf_passed) + expect_to_failure_when_invalid_urdf_is_given_and_be_able_to_submit_new_robot_description) { ASSERT_FALSE(cm_->resource_manager_->are_components_initialized()); - // mimic callback + // wrong urdf auto msg = std_msgs::msg::String(); - msg.data = ros2_control_test_assets::minimal_robot_missing_command_keys_urdf; + msg.data = ros2_control_test_assets::minimal_unitilizable_robot_urdf; + cm_->robot_description_callback(msg); + ASSERT_FALSE(cm_->resource_manager_->are_components_initialized()); + // correct urdf + msg = std_msgs::msg::String(); + msg.data = ros2_control_test_assets::minimal_robot_urdf; cm_->robot_description_callback(msg); ASSERT_TRUE(cm_->resource_manager_->are_components_initialized()); } diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index b19cddb73b..736309b80e 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -1570,7 +1570,7 @@ bool ResourceManager::validate_storage( RCUTILS_LOG_ERROR_NAMED( "resource_manager", - "Discrepancy between robot description file (urdf) and actually exported HW interfaces." + "Discrepancy between robot description file (urdf) and actually exported HW interfaces.\n" "Details: %s", message.c_str()); From 2e13c3c843cc63f15bb54ae7268778bd7e65643d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Thu, 1 Feb 2024 19:44:01 +0100 Subject: [PATCH 06/20] Add some additional tests. --- .../test/test_resource_manager.cpp | 12 ++ .../ros2_control_test_assets/descriptions.hpp | 134 ++++++++++++++++-- 2 files changed, 131 insertions(+), 15 deletions(-) diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index 730410ddd7..7446e885cc 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -228,6 +228,12 @@ TEST_F( ros2_control_test_assets::minimal_robot_not_existing_system_plugin); } +TEST_F(ResourceManagerTest, expect_load_and_initialize_to_fail_when_there_are_dupplicate_of_hw_comp) +{ + test_load_and_initialized_components_failure( + ros2_control_test_assets::minimal_robot_duplicated_component); +} + TEST_F( ResourceManagerTest, expect_load_and_initialize_to_fail_when_a_hw_component_initialization_fails) { @@ -250,6 +256,12 @@ TEST_F(ResourceManagerTest, load_and_initialize_components_called_if_urdf_is_val ASSERT_TRUE(rm.are_components_initialized()); } +TEST_F(ResourceManagerTest, load_and_initialize_components_called_if_async_urdf_is_valid) +{ + TestableResourceManager rm(ros2_control_test_assets::minimal_async_robot_urdf); + ASSERT_TRUE(rm.are_components_initialized()); +} + TEST_F(ResourceManagerTest, can_load_and_initialize_components_later) { TestableResourceManager rm; diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp index 713453bf62..d70f9c0869 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp @@ -176,6 +176,55 @@ const auto hardware_resources = )"; +const auto async_hardware_resources = + R"( + + + test_actuator + + + + + + + + + + + test_sensor + 2 + 2 + + + + + + + + test_system + 2 + 2 + + + + + + + + + + + + + + + + + + + +)"; + const auto unitilizable_hardware_resources = R"( @@ -253,25 +302,25 @@ const auto hardware_resources_not_existing_actuator_plugin = test_system 2 2 - - + + - - + + - + - - + + )"; const auto hardware_resources_not_existing_sensor_plugin = @@ -302,25 +351,25 @@ const auto hardware_resources_not_existing_sensor_plugin = test_system 2 2 - - + + - - + + - - + + - - + + )"; const auto hardware_resources_not_existing_system_plugin = R"( @@ -371,6 +420,55 @@ const auto hardware_resources_not_existing_system_plugin = )"; +const auto hardware_resources_duplicated_component = + R"( + + + test_actuator + + + + + + + + + + + test_sensor + 2 + 2 + + + + + + + + test_system + 2 + 2 + + + + + + + + + + + + + + + + + + + +)"; + const auto hardware_resources_actuator_initializaion_error = R"( @@ -731,6 +829,8 @@ const auto diffbot_urdf = const auto minimal_robot_urdf = std::string(urdf_head) + std::string(hardware_resources) + std::string(urdf_tail); +const auto minimal_async_robot_urdf = + std::string(urdf_head) + std::string(async_hardware_resources) + std::string(urdf_tail); const auto minimal_unitilizable_robot_urdf = std::string(urdf_head) + std::string(unitilizable_hardware_resources) + std::string(urdf_tail); @@ -744,6 +844,10 @@ const auto minimal_robot_not_existing_system_plugin = std::string(urdf_head) + std::string(hardware_resources_not_existing_system_plugin) + std::string(urdf_tail); +const auto minimal_robot_duplicated_component = + std::string(urdf_head) + std::string(hardware_resources_duplicated_component) + + std::string(urdf_tail); + const auto minimal_robot_actuator_initialization_error = std::string(urdf_head) + std::string(hardware_resources_not_existing_actuator_plugin) + std::string(urdf_tail); From 15fb5c6982c47ce42e2e8778f7db70116b94ebbb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Fri, 2 Feb 2024 18:10:46 +0100 Subject: [PATCH 07/20] Optimize tests according to comments. --- .../test/test_resource_manager.cpp | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index 7446e885cc..d9c1f3b933 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -117,7 +117,7 @@ void test_load_and_initialized_components_failure(const std::string & urdf) ASSERT_FALSE(rm.are_components_initialized()); - // resource manage should also not have any components + // resource manager should also not have any components EXPECT_EQ(rm.actuator_components_size(), 0); EXPECT_EQ(rm.sensor_components_size(), 0); EXPECT_EQ(rm.system_components_size(), 0); @@ -146,6 +146,7 @@ void test_load_and_initialized_components_failure(const std::string & urdf) TEST_F(ResourceManagerTest, test_unitilizable_hardware_no_validation) { + SCOPED_TRACE("test_unitilizable_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( @@ -178,10 +179,12 @@ TEST_F(ResourceManagerTest, initialization_with_urdf_and_manual_validation) TEST_F(ResourceManagerTest, expect_validation_failure_if_not_all_interfaces_are_exported) { + SCOPED_TRACE("missing state keys"); // missing state keys test_load_and_initialized_components_failure( ros2_control_test_assets::minimal_robot_missing_state_keys_urdf); + SCOPED_TRACE("missing command keys"); // missing command keys test_load_and_initialized_components_failure( ros2_control_test_assets::minimal_robot_missing_command_keys_urdf); @@ -215,14 +218,17 @@ TEST_F(ResourceManagerTest, no_load_and_initialize_components_function_called) TEST_F( ResourceManagerTest, expect_load_and_initialize_to_fail_when_a_hw_component_plugin_does_not_exist) { + SCOPED_TRACE("Actuator plugin does not exist"); // Actuator test_load_and_initialized_components_failure( ros2_control_test_assets::minimal_robot_not_existing_actuator_plugin); + SCOPED_TRACE("Sensor plugin does not exist"); // Sensor test_load_and_initialized_components_failure( ros2_control_test_assets::minimal_robot_not_existing_sensors_plugin); + SCOPED_TRACE("System plugin does not exist"); // System test_load_and_initialized_components_failure( ros2_control_test_assets::minimal_robot_not_existing_system_plugin); @@ -230,6 +236,7 @@ TEST_F( TEST_F(ResourceManagerTest, expect_load_and_initialize_to_fail_when_there_are_dupplicate_of_hw_comp) { + SCOPED_TRACE("Duplicated components"); test_load_and_initialized_components_failure( ros2_control_test_assets::minimal_robot_duplicated_component); } @@ -237,14 +244,17 @@ TEST_F(ResourceManagerTest, expect_load_and_initialize_to_fail_when_there_are_du TEST_F( ResourceManagerTest, expect_load_and_initialize_to_fail_when_a_hw_component_initialization_fails) { + SCOPED_TRACE("Actuator initialization fails"); // Actuator test_load_and_initialized_components_failure( ros2_control_test_assets::minimal_robot_actuator_initialization_error); + SCOPED_TRACE("Sensor initialization fails"); // Sensor test_load_and_initialized_components_failure( ros2_control_test_assets::minimal_robot_sensor_initialization_error); + SCOPED_TRACE("System initialization fails"); // System test_load_and_initialized_components_failure( ros2_control_test_assets::minimal_robot_system_initialization_error); From 0c6761f236421dc970c65172887bacbcf57476ea Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Wed, 28 Feb 2024 18:29:52 +0100 Subject: [PATCH 08/20] Apply suggestions from code review Co-authored-by: Sai Kishor Kothakota --- controller_manager/src/controller_manager.cpp | 2 +- .../include/hardware_interface/resource_manager.hpp | 7 ++----- hardware_interface/src/resource_manager.cpp | 2 +- 3 files changed, 4 insertions(+), 7 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index 00f0bf677b..78765faef9 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -288,7 +288,7 @@ ControllerManager::ControllerManager( init_resource_manager(robot_description_); if (resource_manager_->are_components_initialized()) { - RCLCPP_FATAL( + RCLCPP_WARN( get_logger(), "You have to restart the framework when using robot description from parameter!"); init_services(); diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 65605ffb74..be6d46c8fe 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -84,9 +84,6 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * * \param[in] urdf string containing the URDF. * \param[in] validate_interfaces boolean argument indicating whether the exported - * interfaces ought to be validated. Defaults to true. - * \param[in] load_and_initialize_components boolean argument indicating whether to load and - * initialize the components present in the parsed URDF. Defaults to true. * \returns false if URDF validation has failed. */ bool load_and_initialize_components(const std::string & urdf); @@ -96,8 +93,8 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * this returns true. We want to permit to load the urdf later on but we currently don't want to * permit multiple calls to load_and_initialize_components (reloading/loading different urdf). * - * @return true if resource manager's load_and_initialize_components() has been already called. - * @return false if resource manager's load_and_initialize_components() has not been yet called. + * @return true if the resource manager has successfully loaded and initialized the components + * @return false if the resource manager doesn't have any components loaded and initialized. */ bool are_components_initialized() const; diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 736309b80e..d0d9bbfb1d 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -60,7 +60,7 @@ auto trigger_and_print_hardware_state_transition = } else { - RCUTILS_LOG_WARN_NAMED( + RCUTILS_LOG_ERROR_NAMED( "resource_manager", "Failed to '%s' hardware '%s'", transition_name.c_str(), hardware_name.c_str()); } From 4c03455d6a8bd916e3bdd18837c46befd9244bd7 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Wed, 28 Feb 2024 18:30:31 +0100 Subject: [PATCH 09/20] Update hardware_interface/include/hardware_interface/resource_manager.hpp --- .../include/hardware_interface/resource_manager.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index be6d46c8fe..0d78a4131d 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -83,7 +83,6 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * in which the URDF might not be present at first initialization. * * \param[in] urdf string containing the URDF. - * \param[in] validate_interfaces boolean argument indicating whether the exported * \returns false if URDF validation has failed. */ bool load_and_initialize_components(const std::string & urdf); From 2b4d9c0aa2ea7710495bf098da5ad487ae6c3439 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Mon, 4 Mar 2024 19:26:19 +0100 Subject: [PATCH 10/20] Adjust docs. --- .../include/hardware_interface/resource_manager.hpp | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 0d78a4131d..da91e20770 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -59,14 +59,13 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * hardware components listed within as well as populate their respective * state and command interfaces. * - * If the interfaces ought to be validated, the constructor throws an exception - * in case the URDF lists interfaces which are not available. - * * \param[in] urdf string containing the URDF. - * \param[in] validate_interfaces boolean argument indicating whether the exported - * interfaces ought to be validated. Defaults to true. * \param[in] activate_all boolean argument indicating if all resources should be immediately * activated. Currently used only in tests. + * \param[in] update_rate Update rate of the controller manager to calculate calling frequency + * of async components. + * \param[in] clock_interface reference to the clock interface of the CM node for getting time + * used for triggering async components. */ explicit ResourceManager( const std::string & urdf, bool activate_all = false, unsigned int update_rate = 100, @@ -78,7 +77,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager /// Load resources from on a given URDF. /** - * The resource manager can be post initialized with a given URDF. + * The resource manager can be post-initialized with a given URDF. * This is mainly used in conjunction with the default constructor * in which the URDF might not be present at first initialization. * @@ -89,7 +88,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager /** * @brief if the resource manager load_and_initialize_components(...) function has been called - * this returns true. We want to permit to load the urdf later on but we currently don't want to + * this returns true. We want to permit to loading the urdf later on, but we currently don't want to * permit multiple calls to load_and_initialize_components (reloading/loading different urdf). * * @return true if the resource manager has successfully loaded and initialized the components From 5d7bebf098989f82294eb45d66af82a618681da1 Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Mon, 11 Mar 2024 19:10:14 +0100 Subject: [PATCH 11/20] Update hardware_interface/include/hardware_interface/resource_manager.hpp --- .../include/hardware_interface/resource_manager.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index da91e20770..69f4104646 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -88,8 +88,8 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager /** * @brief if the resource manager load_and_initialize_components(...) function has been called - * this returns true. We want to permit to loading the urdf later on, but we currently don't want to - * permit multiple calls to load_and_initialize_components (reloading/loading different urdf). + * this returns true. We want to permit to loading the urdf later on, but we currently don't want + * to permit multiple calls to load_and_initialize_components (reloading/loading different urdf). * * @return true if the resource manager has successfully loaded and initialized the components * @return false if the resource manager doesn't have any components loaded and initialized. From 7b11c37755fa70558baa5253d3ff8927521603e7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Mon, 11 Mar 2024 19:26:21 +0100 Subject: [PATCH 12/20] Fixup formatting. --- .../test/test_controller_manager_srvs.cpp | 12 ++++-------- controller_manager/test/test_spawner_unspawner.cpp | 3 +-- .../test_force_torque_sensor.cpp | 3 +-- joint_limits/include/joint_limits/joint_limits.hpp | 9 +++------ 4 files changed, 9 insertions(+), 18 deletions(-) diff --git a/controller_manager/test/test_controller_manager_srvs.cpp b/controller_manager/test/test_controller_manager_srvs.cpp index 36d8aa7c4e..f1b2ea6241 100644 --- a/controller_manager/test/test_controller_manager_srvs.cpp +++ b/controller_manager/test/test_controller_manager_srvs.cpp @@ -832,8 +832,7 @@ TEST_F(TestControllerManagerSrvs, list_sorted_complex_chained_controllers) auto get_ctrl_pos = [result](const std::string & controller_name) -> int64_t { auto it = std::find_if( - result->controller.begin(), result->controller.end(), - [controller_name](auto itf) + result->controller.begin(), result->controller.end(), [controller_name](auto itf) { return (itf.name.find(controller_name) != std::string::npos); }); return std::distance(result->controller.begin(), it); }; @@ -1044,8 +1043,7 @@ TEST_F(TestControllerManagerSrvs, list_sorted_independent_chained_controllers) auto get_ctrl_pos = [result](const std::string & controller_name) -> int64_t { auto it = std::find_if( - result->controller.begin(), result->controller.end(), - [controller_name](auto itf) + result->controller.begin(), result->controller.end(), [controller_name](auto itf) { return (itf.name.find(controller_name) != std::string::npos); }); return std::distance(result->controller.begin(), it); }; @@ -1321,8 +1319,7 @@ TEST_F(TestControllerManagerSrvs, list_large_number_of_controllers_with_chains) auto get_ctrl_pos = [result](const std::string & controller_name) -> int64_t { auto it = std::find_if( - result->controller.begin(), result->controller.end(), - [controller_name](auto itf) + result->controller.begin(), result->controller.end(), [controller_name](auto itf) { return (itf.name.find(controller_name) != std::string::npos); }); return std::distance(result->controller.begin(), it); }; @@ -1546,8 +1543,7 @@ TEST_F(TestControllerManagerSrvs, list_sorted_large_chained_controller_tree) auto get_ctrl_pos = [result](const std::string & controller_name) -> int64_t { auto it = std::find_if( - result->controller.begin(), result->controller.end(), - [controller_name](auto itf) + result->controller.begin(), result->controller.end(), [controller_name](auto itf) { return (itf.name.find(controller_name) != std::string::npos); }); return std::distance(result->controller.begin(), it); }; diff --git a/controller_manager/test/test_spawner_unspawner.cpp b/controller_manager/test/test_spawner_unspawner.cpp index 1ecc3164b8..3b774035aa 100644 --- a/controller_manager/test/test_spawner_unspawner.cpp +++ b/controller_manager/test/test_spawner_unspawner.cpp @@ -264,8 +264,7 @@ TEST_F(TestLoadController, unload_on_kill) // Launch spawner with unload on kill // timeout command will kill it after the specified time with signal SIGINT std::stringstream ss; - ss << "timeout --signal=INT 5 " - << "ros2 run controller_manager spawner " + ss << "timeout --signal=INT 5 " << "ros2 run controller_manager spawner " << "ctrl_3 -c test_controller_manager -t " << std::string(test_controller::TEST_CONTROLLER_CLASS_NAME) << " --unload-on-kill"; diff --git a/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp b/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp index d226e08fd9..47b19f9769 100644 --- a/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp +++ b/hardware_interface/test/test_hardware_components/test_force_torque_sensor.cpp @@ -43,8 +43,7 @@ class TestForceTorqueSensor : public SensorInterface { if ( std::find_if( - state_interfaces.begin(), state_interfaces.end(), - [&ft_key](const auto & interface_info) + state_interfaces.begin(), state_interfaces.end(), [&ft_key](const auto & interface_info) { return interface_info.name == ft_key; }) == state_interfaces.end()) { return CallbackReturn::ERROR; diff --git a/joint_limits/include/joint_limits/joint_limits.hpp b/joint_limits/include/joint_limits/joint_limits.hpp index 809bfd777b..f9944a85b1 100644 --- a/joint_limits/include/joint_limits/joint_limits.hpp +++ b/joint_limits/include/joint_limits/joint_limits.hpp @@ -128,14 +128,11 @@ struct SoftJointLimits { std::stringstream ss_output; - ss_output << " soft position limits: " - << "[" << min_position << ", " << max_position << "]\n"; + ss_output << " soft position limits: " << "[" << min_position << ", " << max_position << "]\n"; - ss_output << " k-position: " - << "[" << k_position << "]\n"; + ss_output << " k-position: " << "[" << k_position << "]\n"; - ss_output << " k-velocity: " - << "[" << k_velocity << "]\n"; + ss_output << " k-velocity: " << "[" << k_velocity << "]\n"; return ss_output.str(); } From b54fa76ac932e6af1339885cf29f8e9a94735aa9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Mon, 1 Apr 2024 21:05:11 +0200 Subject: [PATCH 13/20] Fix typo when merging master. --- .../test/test_controller_manager_urdf_passing.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/controller_manager/test/test_controller_manager_urdf_passing.cpp b/controller_manager/test/test_controller_manager_urdf_passing.cpp index e71b6ca4fe..5db4cfc683 100644 --- a/controller_manager/test/test_controller_manager_urdf_passing.cpp +++ b/controller_manager/test/test_controller_manager_urdf_passing.cpp @@ -83,7 +83,7 @@ TEST_P( ASSERT_FALSE(cm_->resource_manager_->are_components_initialized()); // wrong urdf auto msg = std_msgs::msg::String(); - msg.data = ros2_control_test_assets::minimal_unitilizable_robot_urdf; + msg.data = ros2_control_test_assets::minimal_uninitializable_robot_urdf; cm_->robot_description_callback(msg); ASSERT_FALSE(cm_->resource_manager_->are_components_initialized()); // correct urdf From 9c5b684eb54b2adbabc1b0bb5f1db35dcdb7e2e2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Mon, 1 Apr 2024 21:07:08 +0200 Subject: [PATCH 14/20] Prepara RM for overriding. --- .../controller_manager/controller_manager.hpp | 12 +++++++++++- .../include/hardware_interface/resource_manager.hpp | 13 +++++++------ .../test/test_resource_manager.cpp | 2 +- 3 files changed, 19 insertions(+), 8 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 9a709c5f8f..1d7dda2c84 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -192,7 +192,17 @@ class ControllerManager : public rclcpp::Node // the executor (see issue #260). // rclcpp::CallbackGroup::SharedPtr deterministic_callback_group_; - // Per controller update rate support + /// Interface for extenal components to check if Resouce Manager is initialized. + /** + * Checks if components in Resouce Manager are loaded and initialized. + * \returns true if they are initialized, false otherwise. + */ + CONTROLLER_MANAGER_PUBLIC + bool is_resource_manager_initialized() const + { + return resource_manager_->are_components_initialized(); + } + CONTROLLER_MANAGER_PUBLIC unsigned int get_update_rate() const; diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 69f4104646..a7463501f3 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -400,6 +400,13 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager */ bool state_interface_exists(const std::string & key) const; +protected: + bool components_are_loaded_and_initialized_ = false; + + mutable std::recursive_mutex resource_interfaces_lock_; + mutable std::recursive_mutex claimed_command_interfaces_lock_; + mutable std::recursive_mutex resources_lock_; + private: bool validate_storage(const std::vector & hardware_info) const; @@ -407,16 +414,10 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager std::unordered_map claimed_command_interface_map_; - mutable std::recursive_mutex resource_interfaces_lock_; - mutable std::recursive_mutex claimed_command_interfaces_lock_; - mutable std::recursive_mutex resources_lock_; - std::unique_ptr resource_storage_; // Structure to store read and write status so it is not initialized in the real-time loop HardwareReadWriteStatus read_write_status; - - bool components_are_loaded_and_initialized_ = false; }; } // namespace hardware_interface diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index e57be2b4d3..2d7194e9c9 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -151,7 +151,7 @@ TEST_F(ResourceManagerTest, test_unitilizable_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( - ros2_control_test_assets::minimal_unitilizable_robot_urdf); + ros2_control_test_assets::minimal_uninitializable_robot_urdf); } TEST_F(ResourceManagerTest, initialization_with_urdf_and_manual_validation) From 2c0a022eb356c337ffb6d1f6108e73cc76d9e622 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Mon, 1 Apr 2024 21:07:33 +0200 Subject: [PATCH 15/20] Change how update rate is passed to RM so that we always work with correct parameter. --- .../controller_manager/controller_manager.hpp | 7 +++++++ controller_manager/src/controller_manager.cpp | 4 ++-- .../hardware_interface/resource_manager.hpp | 7 +++---- hardware_interface/src/resource_manager.cpp | 19 +++++++++---------- 4 files changed, 21 insertions(+), 16 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 1d7dda2c84..f2bd581475 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -203,6 +203,13 @@ class ControllerManager : public rclcpp::Node return resource_manager_->are_components_initialized(); } + /// Update rate of the main control loop in the controller manager. + /** + * Update rate of the main control loop in the controller manager. + * The method is used for per-controller update rate support. + * + * \returns update rate of the controller manager. + */ CONTROLLER_MANAGER_PUBLIC unsigned int get_update_rate() const; diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index aae141bc97..eb3a4d864a 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -188,7 +188,7 @@ ControllerManager::ControllerManager( const std::string & node_namespace, const rclcpp::NodeOptions & options) : rclcpp::Node(manager_node_name, node_namespace, options), resource_manager_(std::make_unique( - update_rate_, this->get_node_clock_interface())), + this->get_node_clock_interface())), diagnostics_updater_(this), executor_(executor), loader_(std::make_shared>( @@ -295,7 +295,7 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String & void ControllerManager::init_resource_manager(const std::string & robot_description) { - if (!resource_manager_->load_and_initialize_components(robot_description)) + if (!resource_manager_->load_and_initialize_components(robot_description, update_rate_)) { RCLCPP_WARN( get_logger(), diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index a7463501f3..d89d32f243 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -50,7 +50,6 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager public: /// Default constructor for the Resource Manager. ResourceManager( - unsigned int update_rate = 100, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr); /// Constructor for the Resource Manager. @@ -68,8 +67,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * used for triggering async components. */ explicit ResourceManager( - const std::string & urdf, bool activate_all = false, unsigned int update_rate = 100, - rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr); + const std::string & urdf, bool activate_all = false, const unsigned int update_rate = 100, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr); ResourceManager(const ResourceManager &) = delete; @@ -82,9 +80,10 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * in which the URDF might not be present at first initialization. * * \param[in] urdf string containing the URDF. + * \param[in] update_rate update rate of the main control loop, i.e., of the controller manager. * \returns false if URDF validation has failed. */ - bool load_and_initialize_components(const std::string & urdf); + virtual bool load_and_initialize_components(const std::string & urdf, const unsigned int update_rate = 100); /** * @brief if the resource manager load_and_initialize_components(...) function has been called diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index d0d9bbfb1d..265cc72fc8 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -79,12 +79,10 @@ class ResourceStorage // TODO(VX792): Change this when HW ifs get their own update rate, // because the ResourceStorage really shouldn't know about the cm's parameters ResourceStorage( - unsigned int update_rate = 100, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr) : actuator_loader_(pkg_name, actuator_interface_name), sensor_loader_(pkg_name, sensor_interface_name), system_loader_(pkg_name, system_interface_name), - cm_update_rate_(update_rate), clock_interface_(clock_interface) { } @@ -762,24 +760,23 @@ class ResourceStorage // Update rate of the controller manager, and the clock interface of its node // Used by async components. - unsigned int cm_update_rate_; + unsigned int cm_update_rate_ = 100; rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_; }; ResourceManager::ResourceManager( - unsigned int update_rate, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) -: resource_storage_(std::make_unique(update_rate, clock_interface)) + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) +: resource_storage_(std::make_unique(clock_interface)) { } ResourceManager::~ResourceManager() = default; ResourceManager::ResourceManager( - const std::string & urdf, bool activate_all, unsigned int update_rate, - rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) -: resource_storage_(std::make_unique(update_rate, clock_interface)) + const std::string & urdf, bool activate_all, const unsigned int update_rate, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) +: resource_storage_(std::make_unique(clock_interface)) { - load_and_initialize_components(urdf); + load_and_initialize_components(urdf, update_rate); if (activate_all) { @@ -793,10 +790,12 @@ ResourceManager::ResourceManager( } // CM API: Called in "callback/slow"-thread -bool ResourceManager::load_and_initialize_components(const std::string & urdf) +bool ResourceManager::load_and_initialize_components(const std::string & urdf, const unsigned int update_rate) { components_are_loaded_and_initialized_ = true; + resource_storage_->cm_update_rate_ = update_rate; + const auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf); const std::string system_type = "system"; From f3a9cad3f1f7ef6353f69b0933761efd989a832b Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Wed, 3 Apr 2024 11:00:22 +0200 Subject: [PATCH 16/20] Apply suggestions from code review Co-authored-by: Sai Kishor Kothakota --- .../include/controller_manager/controller_manager.hpp | 2 +- controller_manager/src/controller_manager.cpp | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index f2bd581475..de787a7e59 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -200,7 +200,7 @@ class ControllerManager : public rclcpp::Node CONTROLLER_MANAGER_PUBLIC bool is_resource_manager_initialized() const { - return resource_manager_->are_components_initialized(); + return resource_manager_ && resource_manager_->are_components_initialized(); } /// Update rate of the main control loop in the controller manager. diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index eb3a4d864a..6d1989a922 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -216,7 +216,7 @@ ControllerManager::ControllerManager( "[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 (resource_manager_->are_components_initialized()) + if (is_resource_manager_initialized()) { RCLCPP_WARN( get_logger(), @@ -249,7 +249,7 @@ ControllerManager::ControllerManager( RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value."); } - if (resource_manager_->are_components_initialized()) + if (is_resource_manager_initialized()) { init_services(); } @@ -278,7 +278,7 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String & RCLCPP_DEBUG( get_logger(), "'Content of robot description file: %s", robot_description.data.c_str()); robot_description_ = robot_description.data; - if (resource_manager_->are_components_initialized()) + if (is_resource_manager_initialized()) { RCLCPP_WARN( get_logger(), @@ -287,7 +287,7 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String & return; } init_resource_manager(robot_description_); - if (resource_manager_->are_components_initialized()) + if (is_resource_manager_initialized()) { init_services(); } From 92463007620795d896d06f1cc350fed78877d940 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Mon, 17 Jun 2024 20:53:27 +0200 Subject: [PATCH 17/20] Fix merge error. --- hardware_interface/src/resource_manager.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index c2f1a1b38d..be56ae2801 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -906,6 +906,7 @@ class ResourceStorage available_command_interfaces_.clear(); claimed_command_interface_map_.clear(); + } /** * Returns the return type of the hardware component group state, if the return type is other From 069572fba23428708a46526959bbac15543f6527 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Wed, 19 Jun 2024 19:51:00 +0200 Subject: [PATCH 18/20] Fix tests and remove dupplicated test after changes. --- .../test/test_resource_manager.cpp | 11 +--------- .../ros2_control_test_assets/descriptions.hpp | 20 +++++++++---------- 2 files changed, 11 insertions(+), 20 deletions(-) diff --git a/hardware_interface_testing/test/test_resource_manager.cpp b/hardware_interface_testing/test/test_resource_manager.cpp index 2d7194e9c9..5a2ea0210c 100644 --- a/hardware_interface_testing/test/test_resource_manager.cpp +++ b/hardware_interface_testing/test/test_resource_manager.cpp @@ -102,15 +102,6 @@ TEST_F(ResourceManagerTest, post_initialization_with_urdf) ASSERT_NO_THROW(rm.load_and_initialize_components(ros2_control_test_assets::minimal_robot_urdf)); } -TEST_F(ResourceManagerTest, test_uninitializable_hardware_validation) -{ - // If the the hardware can not be initialized and load_and_initialize_components tried to validate - // the interfaces a runtime exception is thrown - TestableResourceManager rm; - EXPECT_FALSE( - rm.load_and_initialize_components(ros2_control_test_assets::uninitializable_hardware_resources)); -} - void test_load_and_initialized_components_failure(const std::string & urdf) { TestableResourceManager rm; @@ -145,7 +136,7 @@ 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_no_validation) +TEST_F(ResourceManagerTest, test_unitilizable_hardware) { SCOPED_TRACE("test_unitilizable_hardware_no_validation"); // If the the hardware can not be initialized and load_and_initialize_components didn't try to diff --git a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp index ae3700628a..aeb7dcec5b 100644 --- a/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp +++ b/ros2_control_test_assets/include/ros2_control_test_assets/descriptions.hpp @@ -702,10 +702,10 @@ const auto async_hardware_resources = - + - + )"; @@ -796,10 +796,10 @@ const auto hardware_resources_not_existing_actuator_plugin = - + - + )"; @@ -845,10 +845,10 @@ const auto hardware_resources_not_existing_sensor_plugin = - + - + )"; const auto hardware_resources_not_existing_system_plugin = @@ -893,10 +893,10 @@ const auto hardware_resources_not_existing_system_plugin = - + - + )"; @@ -942,10 +942,10 @@ const auto hardware_resources_duplicated_component = - + - + )"; From b2bd5b6e3a8b810afb1532c49cc1335ecc7a4f59 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Wed, 19 Jun 2024 20:01:12 +0200 Subject: [PATCH 19/20] Correct formatting. --- .../include/controller_manager/controller_manager.hpp | 4 ++-- controller_manager/src/controller_manager.cpp | 4 ++-- .../include/hardware_interface/resource_manager.hpp | 9 +++++---- hardware_interface/src/resource_manager.cpp | 9 +++++---- 4 files changed, 14 insertions(+), 12 deletions(-) diff --git a/controller_manager/include/controller_manager/controller_manager.hpp b/controller_manager/include/controller_manager/controller_manager.hpp index 4d82c4105c..b2cf2197ea 100644 --- a/controller_manager/include/controller_manager/controller_manager.hpp +++ b/controller_manager/include/controller_manager/controller_manager.hpp @@ -193,9 +193,9 @@ class ControllerManager : public rclcpp::Node // the executor (see issue #260). // rclcpp::CallbackGroup::SharedPtr deterministic_callback_group_; - /// Interface for extenal components to check if Resouce Manager is initialized. + /// Interface for external components to check if Resource Manager is initialized. /** - * Checks if components in Resouce Manager are loaded and initialized. + * Checks if components in Resource Manager are loaded and initialized. * \returns true if they are initialized, false otherwise. */ CONTROLLER_MANAGER_PUBLIC diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index eefd1c40d2..f55f23190e 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -187,8 +187,8 @@ ControllerManager::ControllerManager( std::shared_ptr executor, const std::string & manager_node_name, const std::string & node_namespace, const rclcpp::NodeOptions & options) : rclcpp::Node(manager_node_name, node_namespace, options), - resource_manager_(std::make_unique( - this->get_node_clock_interface())), + resource_manager_( + std::make_unique(this->get_node_clock_interface())), diagnostics_updater_(this), executor_(executor), loader_(std::make_shared>( diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 497c05d918..afc79b0957 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -49,8 +49,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager { public: /// Default constructor for the Resource Manager. - ResourceManager( - rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr); + ResourceManager(rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr); /// Constructor for the Resource Manager. /** @@ -67,7 +66,8 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * used for triggering async components. */ explicit ResourceManager( - const std::string & urdf, bool activate_all = false, const unsigned int update_rate = 100, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr); + const std::string & urdf, bool activate_all = false, const unsigned int update_rate = 100, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr); ResourceManager(const ResourceManager &) = delete; @@ -83,7 +83,8 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager * \param[in] update_rate update rate of the main control loop, i.e., of the controller manager. * \returns false if URDF validation has failed. */ - virtual bool load_and_initialize_components(const std::string & urdf, const unsigned int update_rate = 100); + virtual bool load_and_initialize_components( + const std::string & urdf, const unsigned int update_rate = 100); /** * @brief if the resource manager load_and_initialize_components(...) function has been called diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index be56ae2801..113339f59c 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -98,8 +98,7 @@ class ResourceStorage public: // TODO(VX792): Change this when HW ifs get their own update rate, // because the ResourceStorage really shouldn't know about the cm's parameters - ResourceStorage( - rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr) + ResourceStorage(rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr) : actuator_loader_(pkg_name, actuator_interface_name), sensor_loader_(pkg_name, sensor_interface_name), system_loader_(pkg_name, system_interface_name), @@ -981,7 +980,8 @@ ResourceManager::ResourceManager( ResourceManager::~ResourceManager() = default; ResourceManager::ResourceManager( - const std::string & urdf, bool activate_all, const unsigned int update_rate, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) + const std::string & urdf, bool activate_all, const unsigned int update_rate, + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface) : resource_storage_(std::make_unique(clock_interface)) { load_and_initialize_components(urdf, update_rate); @@ -998,7 +998,8 @@ ResourceManager::ResourceManager( } // CM API: Called in "callback/slow"-thread -bool ResourceManager::load_and_initialize_components(const std::string & urdf, const unsigned int update_rate) +bool ResourceManager::load_and_initialize_components( + const std::string & urdf, const unsigned int update_rate) { components_are_loaded_and_initialized_ = true; From 366f65cc7b4aa015faab82cb26900c35964fc516 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Dr=2E=20Denis=20=C5=A0togl?= Date: Wed, 19 Jun 2024 20:06:12 +0200 Subject: [PATCH 20/20] Correct formatting. --- .../include/hardware_interface/resource_manager.hpp | 3 ++- hardware_interface/src/resource_manager.cpp | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index afc79b0957..37baf96388 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -49,7 +49,8 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager { public: /// Default constructor for the Resource Manager. - ResourceManager(rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr); + explicit ResourceManager( + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr); /// Constructor for the Resource Manager. /** diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 113339f59c..7be0cd0cb6 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -98,7 +98,8 @@ class ResourceStorage public: // TODO(VX792): Change this when HW ifs get their own update rate, // because the ResourceStorage really shouldn't know about the cm's parameters - ResourceStorage(rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr) + explicit ResourceStorage( + rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr) : actuator_loader_(pkg_name, actuator_interface_name), sensor_loader_(pkg_name, sensor_interface_name), system_loader_(pkg_name, system_interface_name),