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,7 @@
-import time
+import sys
 
 import pytest
+import rclpy
 from moveit_pro_test_utils.objective_test_fixture import (
     ExecuteObjectiveResource,
     execute_objective_resource as execute_objective_resource,
@@ -34,6 +35,16 @@
 }
 
 
+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_subscribers("/joint_states") > 0:
+            return True
+        rclpy.spin_once(node, timeout_sec=0.1)
+    raise TimeoutError("Timeout waiting for /joint_states to start publishing")
+
+
 @pytest.mark.parametrize(
     "objective_id, should_cancel",
     get_objective_pytest_params("lab_sim", cancel_objectives, skip_objectives),
@@ -43,4 +54,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)