From 1e923b9b2d9b6075d4b3c620a418772190969922 Mon Sep 17 00:00:00 2001
From: Sai Kishor Kothakota <sai.kishor@pal-robotics.com>
Date: Mon, 8 Jan 2024 12:18:16 +0100
Subject: [PATCH 1/2] Initialize the controller manager services after
 initializing resource manager (#1271)

(cherry picked from commit bdad0091946097542cb0397fbdf8130bfac42b3a)

# Conflicts:
#	controller_manager/src/controller_manager.cpp
---
 controller_manager/src/controller_manager.cpp    | 16 ++++++++++++++--
 .../test/test_hardware_management_srvs.cpp       | 12 +++++++++---
 2 files changed, 23 insertions(+), 5 deletions(-)

diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp
index 6cbcd4633e..7645b6d557 100644
--- a/controller_manager/src/controller_manager.cpp
+++ b/controller_manager/src/controller_manager.cpp
@@ -280,13 +280,17 @@ ControllerManager::ControllerManager(
       get_logger(),
       "[Deprecated] Passing the robot description parameter directly to the control_manager node "
       "is deprecated. Use '~/robot_description' topic from 'robot_state_publisher' instead.");
+<<<<<<< HEAD
     init_resource_manager(robot_description);
+=======
+    init_resource_manager(robot_description_);
+    init_services();
+>>>>>>> bdad009 (Initialize the controller manager services after initializing resource manager (#1271))
   }
 
   diagnostics_updater_.setHardwareID("ros2_control");
   diagnostics_updater_.add(
     "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback);
-  init_services();
 }
 
 ControllerManager::ControllerManager(
@@ -308,12 +312,15 @@ ControllerManager::ControllerManager(
     RCLCPP_WARN(get_logger(), "'update_rate' parameter not set, using default value.");
   }
 
+  if (resource_manager_->is_urdf_already_loaded())
+  {
+    init_services();
+  }
   subscribe_to_robot_description_topic();
 
   diagnostics_updater_.setHardwareID("ros2_control");
   diagnostics_updater_.add(
     "Controllers Activity", this, &ControllerManager::controller_activity_diagnostic_callback);
-  init_services();
 }
 
 void ControllerManager::subscribe_to_robot_description_topic()
@@ -345,7 +352,12 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String &
         "description file.");
       return;
     }
+<<<<<<< HEAD
     init_resource_manager(robot_description.data.c_str());
+=======
+    init_resource_manager(robot_description_);
+    init_services();
+>>>>>>> bdad009 (Initialize the controller manager services after initializing resource manager (#1271))
   }
   catch (std::runtime_error & e)
   {
diff --git a/controller_manager/test/test_hardware_management_srvs.cpp b/controller_manager/test/test_hardware_management_srvs.cpp
index 0fc7a2f27e..0b4b107ee1 100644
--- a/controller_manager/test/test_hardware_management_srvs.cpp
+++ b/controller_manager/test/test_hardware_management_srvs.cpp
@@ -84,7 +84,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();
   }
@@ -383,7 +385,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();
   }
@@ -440,7 +444,9 @@ class TestControllerManagerHWManagementSrvsOldParameters
         "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();
   }

From cc540eb1fb85a22c29340b61e88d6476a8c186c8 Mon Sep 17 00:00:00 2001
From: Bence Magyar <bence.magyar.robotics@gmail.com>
Date: Mon, 8 Jan 2024 18:27:37 +0000
Subject: [PATCH 2/2] fixup conflicts

Co-authored-by: Sai Kishor Kothakota <sai.kishor@pal-robotics.com>
---
 controller_manager/src/controller_manager.cpp | 8 --------
 1 file changed, 8 deletions(-)

diff --git a/controller_manager/src/controller_manager.cpp b/controller_manager/src/controller_manager.cpp
index 7645b6d557..62db7f8242 100644
--- a/controller_manager/src/controller_manager.cpp
+++ b/controller_manager/src/controller_manager.cpp
@@ -280,12 +280,8 @@ ControllerManager::ControllerManager(
       get_logger(),
       "[Deprecated] Passing the robot description parameter directly to the control_manager node "
       "is deprecated. Use '~/robot_description' topic from 'robot_state_publisher' instead.");
-<<<<<<< HEAD
     init_resource_manager(robot_description);
-=======
-    init_resource_manager(robot_description_);
     init_services();
->>>>>>> bdad009 (Initialize the controller manager services after initializing resource manager (#1271))
   }
 
   diagnostics_updater_.setHardwareID("ros2_control");
@@ -352,12 +348,8 @@ void ControllerManager::robot_description_callback(const std_msgs::msg::String &
         "description file.");
       return;
     }
-<<<<<<< HEAD
     init_resource_manager(robot_description.data.c_str());
-=======
-    init_resource_manager(robot_description_);
     init_services();
->>>>>>> bdad009 (Initialize the controller manager services after initializing resource manager (#1271))
   }
   catch (std::runtime_error & e)
   {