Don't throw exception if something goes wrong with loading and initia…

…lization of the hardware components.

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

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(

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