Fix the initialization of the parsed resource managers (#3346)

Author: saikishor

controller_manager/include/controller_manager/controller_manager.hpp

@@ -342,7 +342,7 @@ class ControllerManager : public rclcpp::Node
   unsigned int update_rate_;
   std::vector<std::vector<std::string>> chained_controllers_configuration_;
 
-  std::unique_ptr<hardware_interface::ResourceManager> resource_manager_;
+  std::unique_ptr<hardware_interface::ResourceManager> resource_manager_ = nullptr;
 
 private:
   std::vector<std::string> get_controller_names();

controller_manager/src/controller_manager.cpp

@@ -574,9 +574,14 @@ ControllerManager::ControllerManager(
       kControllerInterfaceNamespace, kControllerInterfaceClassName)),
   chainable_loader_(
     std::make_shared<pluginlib::ClassLoader<controller_interface::ChainableControllerInterface>>(
-      kControllerInterfaceNamespace, kChainableControllerInterfaceClassName)),
-  robot_description_(resource_manager_->get_robot_description())
+      kControllerInterfaceNamespace, kChainableControllerInterfaceClassName))
 {
+  if (resource_manager_ == nullptr)
+  {
+    throw std::runtime_error("The parsed resource manager is a nullptr!");
+  }
+
+  robot_description_ = resource_manager_->get_robot_description();
   initialize_parameters();
   if (is_resource_manager_initialized())
   {
@@ -586,8 +591,20 @@ ControllerManager::ControllerManager(
   }
   else
   {
-    RCLCPP_FATAL(get_logger(), "The resource manager is not properly initialized");
-    throw std::runtime_error("Resource manager object is not valid. See the FATAL message above.");
+    if (!robot_description_.empty())
+    {
+      RCLCPP_FATAL(get_logger(), "The resource manager is not properly initialized");
+      throw std::runtime_error(
+        "Resource manager object is not valid. See the FATAL message above.");
+    }
+    else
+    {
+      RCLCPP_WARN(
+        get_logger(),
+        "The resource manager is not yet initialized, will wait for the robot description to "
+        "initialize it..");
+      init_controller_manager();
+    }
   }
 }
 
@@ -803,7 +820,10 @@ void ControllerManager::init_resource_manager(const std::string & robot_descript
   params.return_failed_hardware_names_on_return_deactivate_write_cycle_ =
     params_->defaults.deactivate_controllers_on_hardware_self_deactivate;
   params.handle_exceptions = params_->handle_exceptions;
-  resource_manager_ = std::make_unique<hardware_interface::ResourceManager>(params, false);
+  if (resource_manager_ == nullptr)
+  {
+    resource_manager_ = std::make_unique<hardware_interface::ResourceManager>(params, false);
+  }
 
   resource_manager_->set_on_component_state_switch_callback(
     std::bind(&ControllerManager::publish_activity, this));

controller_manager/test/test_controller_manager_with_resource_manager.cpp

@@ -116,13 +116,31 @@ TEST_F(ControllerManagerTest, robot_description_callback_handles_no_geometry)
   EXPECT_TRUE(cm.is_resource_manager_initialized());
 }
 
-TEST_F(ControllerManagerTest, cm_constructor_with_invalid_rm_object)
+TEST_F(ControllerManagerTest, cm_constructor_with_nullptr_rm_object)
 {
+  test_resource_manager_ = nullptr;
   EXPECT_THROW(
     { TestControllerManager cm(std::move(test_resource_manager_), executor_); },
     std::runtime_error);
 }
 
+TEST_F(ControllerManagerTest, cm_constructor_with_uninitialized_rm_object_with_valid_urdf)
+{
+  hardware_interface::ResourceManagerParams rm_param;
+  rm_param.logger = node_->get_logger();
+  rm_param.clock = node_->get_clock();
+  rm_param.robot_description = ros2_control_test_assets::minimal_robot_urdf;
+  test_resource_manager_ = std::make_unique<hardware_interface::ResourceManager>(rm_param, false);
+  EXPECT_THROW(
+    { TestControllerManager cm(std::move(test_resource_manager_), executor_); },
+    std::runtime_error);
+}
+
+TEST_F(ControllerManagerTest, cm_constructor_with_uninitialized_rm_object_with_no_urdf)
+{
+  EXPECT_NO_THROW({ TestControllerManager cm(std::move(test_resource_manager_), executor_); });
+}
+
 int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);

hardware_interface/src/resource_manager.cpp

@@ -170,6 +170,9 @@ class ResourceStorage
   : ResourceStorage(rm_param.clock, rm_param.logger)
   {
     handle_exception_ = rm_param.handle_exceptions;
+    robot_description_ = rm_param.robot_description;
+    cm_update_rate_ = rm_param.update_rate;
+    handle_exception_ = rm_param.handle_exceptions;
   }
 
   template <class HardwareT, class HardwareInterfaceT>