Robustify spawner

controller_manager/CMakeLists.txt

@@ -201,6 +201,16 @@ if(BUILD_TESTING)
   ament_target_dependencies(test_hardware_management_srvs
     controller_manager_msgs
   )
+
+  install(
+    DIRECTORY test
+    DESTINATION share/${PROJECT_NAME}
+  )
+
+  find_package(launch_testing_ament_cmake)
+  add_launch_test(test/test_spawner_integration.py
+    TIMEOUT 180
+  )
 endif()
 
 install(

controller_manager/controller_manager/controller_manager_services.py

@@ -28,7 +28,9 @@
 import rclpy
 
 
-def service_caller(node, service_name, service_type, request, service_timeout=10.0):
+def service_caller(
+    node, service_name, service_type, request, service_timeout=10.0, max_attempts=3
+):
     cli = node.create_client(service_type, service_name)
 
     if not cli.service_is_ready():
@@ -39,12 +41,18 @@ def service_caller(node, service_name, service_type, request, service_timeout=10
             raise RuntimeError(f"Could not contact service {service_name}")
 
     node.get_logger().debug(f"requester: making request: {request}\n")
-    future = cli.call_async(request)
-    rclpy.spin_until_future_complete(node, future)
-    if future.result() is not None:
-        return future.result()
-    else:
-        raise RuntimeError(f"Exception while calling service: {future.exception()}")
+    future = None
+    for attempt in range(max_attempts):  # This is a rather arbitrary retry number
+        future = cli.call_async(request)
+        rclpy.spin_until_future_complete(node, future, timeout_sec=service_timeout)
+        if future.result() is None:
+            node.get_logger().warning(
+                f"Failed getting a result from calling {service_name} in "
+                f"{service_timeout}. (Attempt {attempt} of {max_attempts}.)"
+            )
+        else:
+            return future.result()
+    raise RuntimeError(f"Exception while calling service {service_name}: {future.exception()}")
 
 
 def configure_controller(node, controller_manager_name, controller_name, service_timeout=10.0):

controller_manager/controller_manager/spawner.py

@@ -130,8 +130,12 @@ def wait_for_controller_manager(node, controller_manager, timeout_duration):
     return False
 
 
-def is_controller_loaded(node, controller_manager, controller_name):
-    controllers = list_controllers(node, controller_manager).controller
+def is_controller_loaded(
+    node, controller_manager, controller_name, controller_manager_timeout=10.0
+):
+    controllers = list_controllers(
+        node, controller_manager, service_timeout=controller_manager_timeout
+    ).controller
     return any(c.name == controller_name for c in controllers)
 
 
@@ -217,20 +221,17 @@ def main(args=None):
             controller_manager_name = f"/{controller_manager_name}"
 
     try:
-        if not wait_for_controller_manager(
-            node, controller_manager_name, controller_manager_timeout
-        ):
-            node.get_logger().error(
-                bcolors.FAIL + "Controller manager not available" + bcolors.ENDC
-            )
-            return 1
-
         for controller_name in controller_names:
             prefixed_controller_name = controller_name
             if controller_namespace:
                 prefixed_controller_name = controller_namespace + "/" + controller_name
 
-            if is_controller_loaded(node, controller_manager_name, prefixed_controller_name):
+            if is_controller_loaded(
+                node,
+                controller_manager_name,
+                prefixed_controller_name,
+                controller_manager_timeout=controller_manager_timeout,
+            ):
                 node.get_logger().warn(
                     bcolors.WARNING
                     + "Controller already loaded, skipping load_controller"

controller_manager/test/test_controllers.yaml

@@ -0,0 +1,145 @@
+---
+controller_manager:
+  ros__parameters:
+    update_rate: 100
+
+    joint_state_broadcaster0:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster1:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster2:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster3:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster4:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster5:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster6:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster7:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster8:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster9:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster10:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster11:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster12:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster13:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster14:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster15:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster16:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster17:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster18:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster19:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster20:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster21:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster22:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster23:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster24:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster25:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster26:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster27:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster28:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster29:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster30:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster31:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster32:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster33:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster34:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster35:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster36:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster37:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster38:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster39:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster40:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster41:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster42:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster43:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster44:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster45:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster46:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster47:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster48:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    joint_state_broadcaster49:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    fts_broadcaster:
+      type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster
+
+    base_controller:
+      type: position_controllers/JointGroupPositionController
+
+    joint1_controller:
+      type: position_controllers/JointGroupPositionController
+
+    joint2_controller:
+      type: position_controllers/JointGroupPositionController
+
+fts_broadcaster:
+  ros__parameters:
+    sensor_name: base_ft_sensor
+    state_interface_names:
+      - force.x
+      - force.y
+      - force.z
+      - torque.x
+      - torque.y
+      - torque.z
+    frame_id: base_link
+    topic_name: base_wrench
+
+base_controller:
+  ros__parameters:
+    joints:
+      - base_joint
+
+joint1_controller:
+  ros__parameters:
+    joints:
+      - joint1
+
+joint2_controller:
+  ros__parameters:
+    joints:
+      - joint2