__ -> _

load_urdf() -> load_and_initialize_components()

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();

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

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

hardware_interface/src/resource_manager.cpp

@@ -742,7 +742,7 @@ ResourceManager::ResourceManager(
   rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface)
 : resource_storage_(std::make_unique<ResourceStorage>(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)

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());
 }