Wait for joint_states before running test

Signed-off-by: Paul Gesel <[email protected]>

Author: pac48

src/lab_sim/test/test_core_objectives.py

@@ -1,6 +1,5 @@
-import time
-
 import pytest
+import rclpy
 from moveit_pro_test_utils.objective_test_fixture import (
     ExecuteObjectiveResource,
     execute_objective_resource as execute_objective_resource,
@@ -23,6 +22,7 @@
     "Plan and Save Trajectory",
     "Record and Replay Scanning Motion",
 }
+
 skip_objectives = {
     "Grasp Planning",
     "Joint Diagnostic",
@@ -34,6 +34,22 @@
 }
 
 
+def wait_for_joint_states(timeout: int = 180):
+    node = rclpy.create_node("test_node")
+    start_time = node.get_clock().now()
+    while (node.get_clock().now() - start_time).to_msg().sec < timeout:
+        if (
+            node.count_publishers("/joint_states") > 0
+            and node.count_publishers("/wrist_camera/color") > 0
+            and node.count_publishers("/display_contacts")
+        ):
+            return True
+        rclpy.spin_once(node, timeout_sec=0.1)
+    raise TimeoutError(
+        "Timeout waiting for /joint_states and /wrist_camera/color and /display_contacts to start publishing"
+    )
+
+
 @pytest.mark.parametrize(
     "objective_id, should_cancel",
     get_objective_pytest_params("lab_sim", cancel_objectives, skip_objectives),
@@ -43,4 +59,5 @@ def test_all_objectives(
     should_cancel: bool,
     execute_objective_resource: ExecuteObjectiveResource,
 ):
+    wait_for_joint_states()
     run_objective(objective_id, should_cancel, execute_objective_resource)