Initialize the controller manager services after initializing resource manager (#1272)

---------

Co-authored-by: Sai Kishor Kothakota <[email protected]>

Author: dyackzan

controller_manager/src/controller_manager.cpp

@@ -284,9 +284,8 @@ 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();
   }
-
-  init_services();
 }
 
 ControllerManager::ControllerManager(
@@ -311,7 +310,10 @@ ControllerManager::ControllerManager(
   {
     subscribe_to_robot_description_topic();
   }
-  init_services();
+  else
+  {
+    init_services();
+  }
 }
 
 void ControllerManager::subscribe_to_robot_description_topic()
@@ -343,6 +345,7 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String &
       return;
     }
     init_resource_manager(robot_description.data.c_str());
+    init_services();
   }
   catch (std::runtime_error & e)
   {

controller_manager/test/test_hardware_management_srvs.cpp

@@ -81,7 +81,9 @@ class TestControllerManagerHWManagementSrvs : public TestControllerManagerSrvs
         "Unable to initialize resource manager, no robot description found.");
     }
 
-    cm_->init_resource_manager(robot_description);
+    auto msg = std_msgs::msg::String();
+    msg.data = robot_description;
+    cm_->robot_description_callback(msg);
 
     SetUpSrvsCMExecutor();
   }
@@ -223,7 +225,9 @@ class TestControllerManagerHWManagementSrvsWithoutParams
         "Unable to initialize resource manager, no robot description found.");
     }
 
-    cm_->init_resource_manager(robot_description);
+    auto msg = std_msgs::msg::String();
+    msg.data = robot_description;
+    cm_->robot_description_callback(msg);
 
     SetUpSrvsCMExecutor();
   }