Make sure AMRs are up before sending task

Signed-off-by: Luca Della Vedova <[email protected]>

nexus_integration_tests/package.xml

@@ -22,6 +22,7 @@
   <depend>rclcpp_components</depend>
   <depend>rclcpp_lifecycle</depend>
   <depend>rclpy</depend>
+  <depend>rmf_fleet_msgs</depend>
   <depend>std_srvs</depend>
   <depend>tf2_ros</depend>
   <depend>yaml-cpp</depend>

nexus_integration_tests/ros_testcase.py

@@ -24,6 +24,7 @@
 import rclpy.node
 from lifecycle_msgs.msg import State
 from lifecycle_msgs.srv import GetState
+from rmf_fleet_msgs.msg import RobotState
 
 T = TypeVar("T", bound="RosTestCase")
 
@@ -102,6 +103,14 @@ def wait_for_nodes(self, *nodes: Sequence[str]):
             time.sleep(0.1)
             undiscovered.difference_update(self.node.get_node_names())
 
+    async def wait_for_robot_state(self):
+        fut = rclpy.Future()
+        sub = self.node.create_subscription(
+            RobotState, "/robot_state", fut.set_result, 10
+        )
+        result = await fut
+        self.assertIsNotNone(result)
+
     async def ros_sleep(self, secs: float):
         """
         async sleep using ros timers

nexus_integration_tests/test_pick_and_place.py

@@ -51,6 +51,8 @@ async def asyncSetUp(self):
         print("all workcells are ready")
         await self.wait_for_transporters("transporter_node")
         print("all transporters are ready")
+        await self.wait_for_robot_state()
+        print("AMRs are ready")
 
         # give some time for discovery to happen
         await self.ros_sleep(5)