Initialize the controller manager services after initializing resourc…

…e manager (#1272) --------- Co-authored-by: Sai Kishor Kothakota <sai.kishor@pal-robotics.com>
ros-controls · Jan 12, 2024 · 385293e · 385293e
1 parent 036d426
commit 385293e
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 5 deletions.
diff --git a/controller_manager/src/controller_manager.cpp b/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)
   {

diff --git a/controller_manager/test/test_hardware_management_srvs.cpp b/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();
   }