From c0de993f02c5317f7f9040a84fb535c868b3cf8b Mon Sep 17 00:00:00 2001 From: Manuel M Date: Mon, 3 Apr 2023 11:56:50 +0200 Subject: [PATCH] rename flag, add some ttests in resource manager --- controller_manager/src/controller_manager.cpp | 5 ++-- .../hardware_interface/resource_manager.hpp | 14 ++++++---- hardware_interface/src/resource_manager.cpp | 9 +++--- .../test/test_resource_manager.cpp | 28 +++++++++++++++++++ 4 files changed, 43 insertions(+), 13 deletions(-) diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp index a661aadc8d9..a55b30993a1 100644 --- a/controller_manager/src/controller_manager.cpp +++ b/controller_manager/src/controller_manager.cpp @@ -211,11 +211,12 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String & // to die if a non valid urdf is passed. However, should maybe be fine tuned. try { - if (resource_manager_->is_initialized()) + if (resource_manager_->load_urdf_called()) { RCLCPP_WARN( get_logger(), - "ResourceManager has already been initialized. Ignoring received robot description file."); + "ResourceManager has already loaded an urdf file. Ignoring attempt to reload a robot " + "description file."); return; } init_resource_manager(robot_description.data.c_str()); diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp index 0d529e27956..4a6567a7ea3 100644 --- a/hardware_interface/include/hardware_interface/resource_manager.hpp +++ b/hardware_interface/include/hardware_interface/resource_manager.hpp @@ -66,7 +66,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager */ explicit ResourceManager( const std::string & urdf, bool validate_interfaces = true, bool activate_all = false, - bool initialized = false); + bool urdf_loaded = false); ResourceManager(const ResourceManager &) = delete; @@ -85,12 +85,14 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager void load_urdf(const std::string & urdf, bool validate_interfaces = true); /** - * @brief if the resource manager has been initialized with a valid urdf file this returns 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) * - * @return true if resource manager has been initialized with a valid urdf file - * @return false if resource manager has not been initialized + * @return true if resource manager's load_urdf() has been called + * @return false if resource manager's load_urdf() has not been called */ - bool is_initialized() const; + bool load_urdf_called() const; /// Claim a state interface given its key. /** @@ -415,7 +417,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 initialized_ = false; + bool load_urdf_called_ = false; }; } // namespace hardware_interface diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp index 9bb0bd26646..9d438dfad09 100644 --- a/hardware_interface/src/resource_manager.cpp +++ b/hardware_interface/src/resource_manager.cpp @@ -574,8 +574,8 @@ ResourceManager::ResourceManager() : resource_storage_(std::make_unique()), initialized_(initialized) + const std::string & urdf, bool validate_interfaces, bool activate_all, bool load_urdf_called) +: resource_storage_(std::make_unique()), load_urdf_called_(load_urdf_called) { load_urdf(urdf, validate_interfaces); @@ -593,6 +593,7 @@ ResourceManager::ResourceManager( // CM API: Called in "callback/slow"-thread void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfaces) { + load_urdf_called_ = true; const std::string system_type = "system"; const std::string sensor_type = "sensor"; const std::string actuator_type = "actuator"; @@ -629,11 +630,9 @@ void ResourceManager::load_urdf(const std::string & urdf, bool validate_interfac read_write_status.failed_hardware_names.reserve( resource_storage_->actuators_.size() + resource_storage_->sensors_.size() + resource_storage_->systems_.size()); - - initialized_ = true; } -bool ResourceManager::is_initialized() const { return initialized_; } +bool ResourceManager::load_urdf_called() const { return load_urdf_called_; } // CM API: Called in "update"-thread LoanedStateInterface ResourceManager::claim_state_interface(const std::string & key) diff --git a/hardware_interface/test/test_resource_manager.cpp b/hardware_interface/test/test_resource_manager.cpp index 59257f4edc3..f7d19623166 100644 --- a/hardware_interface/test/test_resource_manager.cpp +++ b/hardware_interface/test/test_resource_manager.cpp @@ -212,6 +212,34 @@ TEST_F(ResourceManagerTest, initialization_with_urdf_unclaimed) } } +TEST_F(ResourceManagerTest, no_load_urdf_function_called) +{ + TestableResourceManager rm; + ASSERT_FALSE(rm.load_urdf_called()); +} + +TEST_F(ResourceManagerTest, load_urdf_called_if_urdf_is_invalid) +{ + TestableResourceManager rm; + EXPECT_THROW( + rm.load_urdf(ros2_control_test_assets::minimal_robot_missing_state_keys_urdf), std::exception); + ASSERT_TRUE(rm.load_urdf_called()); +} + +TEST_F(ResourceManagerTest, load_urdf_called_if_urdf_is_valid) +{ + TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf); + ASSERT_TRUE(rm.load_urdf_called()); +} + +TEST_F(ResourceManagerTest, can_load_urdf_later) +{ + TestableResourceManager rm; + ASSERT_FALSE(rm.load_urdf_called()); + rm.load_urdf(ros2_control_test_assets::minimal_robot_urdf); + ASSERT_TRUE(rm.load_urdf_called()); +} + TEST_F(ResourceManagerTest, resource_claiming) { TestableResourceManager rm(ros2_control_test_assets::minimal_robot_urdf);