SPAWNER: Wait for the clock to be available before proceeding (#432)

This patch is to resolve an issue when the simulated clock is not published right away (coming from a remote gazebo instance for example). If the clock is not published right away, the rosservice wait_for_service times out prematurely and the controllers fail to be loaded since the system is not yet ready (no Gazebo clock). With no, gazebo system, the clock will be ready immediately and the spawner will proceed. Issue: #431
ros-controls · Jul 15, 2022 · 66a4ef7 · 66a4ef7
1 parent 83a392b
commit 66a4ef7
Showing 1 changed file with 7 additions and 0 deletions.
diff --git a/controller_manager/scripts/spawner b/controller_manager/scripts/spawner
@@ -116,6 +116,13 @@ def main():
     unload_controller_service = robot_namespace+"controller_manager/unload_controller"
     switch_controller_service = robot_namespace+"controller_manager/switch_controller"
 
+    # Wait for the clock to be published
+    if rospy.get_param('/use_sim_time', False):
+        while rospy.get_rostime() == rospy.Time(0):
+            rospy.loginfo_throttle(30, "Waiting for /clock to be available...")
+            rospy.sleep(0.2)
+        rospy.loginfo("/clock is published. Proceeding to load the controller(s).")
+
     try:
         # loader
         rospy.loginfo("Controller Spawner: Waiting for service "+load_controller_service)