[RM] Rename load_urdf method to load_and_initialize_components and add error handling there to avoid stack crashing when error happens.

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();
   }
@@ -341,36 +347,32 @@ 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_->are_components_initialized())
   {
-    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();
+    RCLCPP_WARN(
+      get_logger(),
+      "ResourceManager has already loaded an urdf file. Ignoring attempt to reload a robot "
+      "description file.");
+    return;
   }
-  catch (std::runtime_error & e)
+  init_resource_manager(robot_description_);
+  if (resource_manager_->are_components_initialized())
   {
-    RCLCPP_ERROR_STREAM(
-      get_logger(),
-      "The published robot description file (urdf) seems not to be genuine. The following error "
-      "was caught:"
-        << e.what());
+    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(),
+      "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
   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,
+    expect_to_failure_when_invalid_urdf_is_given_and_be_able_to_submit_new_robot_description);
 
 public:
   TestableControllerManager(
@@ -59,29 +61,36 @@ 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());
+  ASSERT_FALSE(cm_->resource_manager_->are_components_initialized());
 }
 
-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());
+  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_urdf_called_after_invalid_urdf_passed)
+TEST_P(
+  TestControllerManagerWithTestableCM,
+  expect_to_failure_when_invalid_urdf_is_given_and_be_able_to_submit_new_robot_description)
 {
-  ASSERT_FALSE(cm_->resource_manager_->is_urdf_already_loaded());
-  // mimic callback
+  ASSERT_FALSE(cm_->resource_manager_->are_components_initialized());
+  // 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_->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;
@@ -88,20 +87,19 @@ 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.
    */
-  void load_urdf(
-    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_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;
+  bool are_components_initialized() const;
 
   /// Claim a state interface given its key.
   /**
@@ -408,7 +406,7 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
   bool state_interface_exists(const std::string & key) const;
 
 private:
-  void validate_storage(const std::vector<hardware_interface::HardwareInfo> & hardware_info) const;
+  bool validate_storage(const std::vector<hardware_interface::HardwareInfo> & hardware_info) const;
 
   void release_command_interface(const std::string & key);
 
@@ -423,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