diff --git a/artefacts.yaml b/artefacts.yaml index f2c40b7..97202d7 100644 --- a/artefacts.yaml +++ b/artefacts.yaml @@ -37,7 +37,8 @@ jobs: - name: pose_based_waypoint_mission_test params: policy: - - stumbling - baseline + - stumbling + metrics: metrics.json run: "uv run dataflow --test-waypoint-poses" \ No newline at end of file diff --git a/dataflow/pyproject.toml b/dataflow/pyproject.toml index 70a3589..fcd025d 100644 --- a/dataflow/pyproject.toml +++ b/dataflow/pyproject.toml @@ -14,6 +14,7 @@ dependencies = [ "dora-rs-cli>=0.3.13", "typer>=0.20.0", "artefacts-toolkit>=0.7.1", + "pytest-custom-exit-code>=0.3.0", ] [project.scripts] diff --git a/dataflow/src/dataflow/dataflow.py b/dataflow/src/dataflow/dataflow.py index eefb0c6..7fd9ec9 100644 --- a/dataflow/src/dataflow/dataflow.py +++ b/dataflow/src/dataflow/dataflow.py @@ -163,11 +163,21 @@ def run_dataflow( junit_xml_path = ( output_path / ".." / "tests_junit.xml" ) # Save junit in the root outputs folder + tester = dataflow.add_node( id="tester", path="pytest", - args=f"{nodes_path / 'tester/tester' / test} -s --junit-xml={str(junit_xml_path)}", + args=" ".join( + [ + f"{nodes_path / 'tester/tester' / test} -s", + f"--junit-xml={str(junit_xml_path)}", + # Avoid failing the dataflow on test failures + # https://docs.pytest.org/en/stable/reference/exit-codes.html + "--suppress-tests-failed-exit-code", + ] + ), ) + tester.add_input("waypoints", "simulation/waypoints") tester.add_input("scene_info", "simulation/scene_info") tester.add_input("robot_pose", "simulation/robot_pose") diff --git a/msgs/src/msgs/__init__.py b/msgs/src/msgs/__init__.py index 7164c68..158f7ff 100644 --- a/msgs/src/msgs/__init__.py +++ b/msgs/src/msgs/__init__.py @@ -32,6 +32,10 @@ def now(cls) -> "Timestamp": def float_seconds(self) -> float: return self.seconds + self.nanoseconds / 1e9 + @property + def float_milliseconds(self) -> float: + return self.float_seconds * 1e3 + @dataclass class Transform(ArrowMessage): diff --git a/nodes/tester/tester/conftest.py b/nodes/tester/tester/conftest.py index 9e01e9e..43f9fe2 100644 --- a/nodes/tester/tester/conftest.py +++ b/nodes/tester/tester/conftest.py @@ -1,4 +1,7 @@ # noqa: D100 +import os +from pathlib import Path + import msgs import pyarrow as pa import pytest @@ -74,3 +77,27 @@ def node(request, session_node: TestNode): session_node.set_timeout(clock_timeout.args[0]) yield session_node session_node.reset_timeout() + + +@pytest.fixture(scope="session") +def metrics(): + """Collect test metrics during the test session.""" + metrics = {} + + yield metrics + + workspace_path = Path(__file__).parent.parent.parent.parent + metrics_path = ( + Path( + os.getenv( + "ARTEFACTS_SCENARIO_UPLOAD_DIR", workspace_path / "outputs/artefacts" + ) + ) + / ".." + / "metrics.json" + ) + metrics_path.parent.mkdir(parents=True, exist_ok=True) + with open(metrics_path, "w") as f: + import json + + json.dump(metrics, f, indent=2) diff --git a/nodes/tester/tester/stuck_detector.py b/nodes/tester/tester/stuck_detector.py new file mode 100644 index 0000000..ee35067 --- /dev/null +++ b/nodes/tester/tester/stuck_detector.py @@ -0,0 +1,52 @@ +"""Helper class to detect if the robot is stuck.""" + +import msgs + + +class StuckDetector: + """Helper class to detect if the robot is stuck.""" + + def __init__(self, threshold: float = 0.1, max_no_progress_time: float = 3.0): + """Initialize the stuck detector. + + Args: + threshold: Distance threshold in meters to consider as progress. + max_no_progress_time: Maximum time in seconds without progress before + considering the robot as stuck. + + """ + self.threshold = threshold + self.max_no_progress_time = max_no_progress_time + self.last_position = None + self.last_progress_time = None + + def step_is_stuck(self, position: msgs.Transform, current_time: float) -> bool: + """Update the detector with the current position. + + Returns True if the robot is considered stuck. + """ + if self.last_position is None or self.last_progress_time is None: + self.last_position = position + self.last_progress_time = current_time + return False + + distance_moved = ( + (position.x - self.last_position.x) ** 2 + + (position.y - self.last_position.y) ** 2 + + (position.z - self.last_position.z) ** 2 + ) ** 0.5 + + if distance_moved >= self.threshold: + self.last_position = position + self.last_progress_time = current_time + return False + else: + if current_time - self.last_progress_time > self.max_no_progress_time: + print( + "Robot is stuck: no significant movement detected." + f" Distance moved: {distance_moved:.3f} m in" + f" {current_time - self.last_progress_time:.2f} s." + ) + return True + + return False diff --git a/nodes/tester/tester/test_waypoints_poses.py b/nodes/tester/tester/test_waypoints_poses.py index 54b9fef..65b5cc7 100644 --- a/nodes/tester/tester/test_waypoints_poses.py +++ b/nodes/tester/tester/test_waypoints_poses.py @@ -1,10 +1,9 @@ """Dora node that test waypoint mission completions using the robot pose.""" -import time - import msgs import pytest +from tester.stuck_detector import StuckDetector from tester.transforms import Transforms @@ -20,22 +19,28 @@ def test_receives_scene_info_on_startup(node): @pytest.mark.parametrize("difficulty", [0.1, 0.7, 1.1]) @pytest.mark.clock_timeout(30) -def test_completes_waypoint_mission_with_variable_height_steps(node, difficulty: float): +def test_completes_waypoint_mission_with_variable_height_steps( + node, difficulty: float, metrics: dict +): """Test that the waypoint mission completes successfully. The pyramid steps height is configured via difficulty. """ - run_waypoint_mission_test(node, scene="generated_pyramid", difficulty=difficulty) + run_waypoint_mission_test( + node, scene="generated_pyramid", difficulty=difficulty, metrics=metrics + ) @pytest.mark.parametrize("scene", ["rail_blocks", "stone_stairs", "excavator"]) @pytest.mark.clock_timeout(30) -def test_completes_waypoint_mission_in_photo_realistic_env(node, scene: str): +def test_completes_waypoint_mission_in_photo_realistic_env( + node, scene: str, metrics: dict +): """Test that the waypoint mission completes successfully.""" - run_waypoint_mission_test(node, scene, difficulty=1.0) + run_waypoint_mission_test(node, scene, difficulty=1.0, metrics=metrics) -def run_waypoint_mission_test(node, scene: str, difficulty: float): +def run_waypoint_mission_test(node, scene: str, difficulty: float, metrics: dict): """Run the waypoint mission test.""" transforms = Transforms() node.send_output( @@ -45,7 +50,22 @@ def run_waypoint_mission_test(node, scene: str, difficulty: float): waypoint_list: list[str] = [] next_waypoint_index = 0 + metrics_key = f"completion_time.{scene}_{difficulty}" + start_time_ms = None + current_time_ms = None + stuck_detector = StuckDetector(max_no_progress_time=5000) # 5 seconds + for event in node: + if event["type"] == "INPUT" and event["id"] == "clock": + now = msgs.Timestamp.from_arrow(event["value"]).float_milliseconds + if start_time_ms is None or now < start_time_ms: + start_time_ms = now + current_time_ms = now + + # Bail if we haven't started yet + if current_time_ms is None: + continue + if event["type"] == "INPUT" and event["id"] == "waypoints": waypoint_list_msg = msgs.WaypointList.from_arrow(event["value"]) waypoints = waypoint_list_msg.waypoints @@ -60,7 +80,7 @@ def run_waypoint_mission_test(node, scene: str, difficulty: float): transforms.add_transform( wp.transform, - int(time.time()), + int(current_time_ms), parent_frame="world", child_frame=waypoint_frame, ) @@ -71,7 +91,7 @@ def run_waypoint_mission_test(node, scene: str, difficulty: float): continue transform = msgs.Transform.from_arrow(event["value"]) - timestamp = int(time.time()) + timestamp = int(current_time_ms) transforms.add_transform( transform, timestamp, @@ -95,3 +115,13 @@ def run_waypoint_mission_test(node, scene: str, difficulty: float): else: print("All waypoints completed!") break + + if stuck_detector.step_is_stuck(transform, timestamp): + raise AssertionError( + "Robot is stuck and cannot complete the waypoint mission." + ) + + if start_time_ms is not None and current_time_ms is not None: + elapsed_time_ms = current_time_ms - start_time_ms + # Convert to seconds + metrics[metrics_key] = elapsed_time_ms / 1000.0 diff --git a/uv.lock b/uv.lock index 77fade0..c44076e 100644 --- a/uv.lock +++ b/uv.lock @@ -533,6 +533,7 @@ dependencies = [ { name = "msgs" }, { name = "navigator" }, { name = "policy-controller" }, + { name = "pytest-custom-exit-code" }, { name = "simulation" }, { name = "teleop" }, { name = "tester" }, @@ -551,6 +552,7 @@ requires-dist = [ { name = "msgs", editable = "msgs" }, { name = "navigator", editable = "nodes/navigator" }, { name = "policy-controller", editable = "nodes/policy_controller" }, + { name = "pytest-custom-exit-code", specifier = ">=0.3.0" }, { name = "simulation", editable = "nodes/simulation" }, { name = "teleop", editable = "nodes/teleop" }, { name = "tester", editable = "nodes/tester" }, @@ -2129,6 +2131,18 @@ wheels = [ { url = "https://files.pythonhosted.org/packages/a8/a4/20da314d277121d6534b3a980b29035dcd51e6744bd79075a6ce8fa4eb8d/pytest-8.4.2-py3-none-any.whl", hash = "sha256:872f880de3fc3a5bdc88a11b39c9710c3497a547cfa9320bc3c5e62fbf272e79", size = 365750, upload-time = "2025-09-04T14:34:20.226Z" }, ] +[[package]] +name = "pytest-custom-exit-code" +version = "0.3.0" +source = { registry = "https://pypi.org/simple" } +dependencies = [ + { name = "pytest" }, +] +sdist = { url = "https://files.pythonhosted.org/packages/92/9d/e1eb0af5e96a5c34f59b9aa69dfb680764420fe60f2ec28cfbc5339f99f8/pytest-custom_exit_code-0.3.0.tar.gz", hash = "sha256:51ffff0ee2c1ddcc1242e2ddb2a5fd02482717e33a2326ef330e3aa430244635", size = 3633, upload-time = "2019-08-07T09:45:15.781Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/35/a0/effb6cbbccfd1c106c572d3d619b3418d71093afb4cd4f91f51e6a1799d2/pytest_custom_exit_code-0.3.0-py3-none-any.whl", hash = "sha256:6e0ce6e57ce3a583cb7e5023f7d1021e19dfec22be41d9ad345bae2fc61caf3b", size = 4055, upload-time = "2019-08-07T09:45:13.767Z" }, +] + [[package]] name = "pytest-timeout" version = "2.4.0"