From 814922f8caabcf77e1a0f4be9b0ba52f2bdb6442 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=D0=94=D0=B0=D0=BD=D0=B8=D0=B8=D0=BB=20=D0=93=D1=80=D0=B0?= =?UTF-8?q?=D0=B1=D0=B0=D1=80=D1=8C?= Date: Sun, 8 Nov 2026 10:49:13 +1000 Subject: [PATCH] feat: spawn deterministic grant scene --- .../iiwa_experement/scene_spawner.py | 199 ++++++++++++++++++ .../test/test_scene_spawner.py | 172 +++++++++++++++ 2 files changed, 371 insertions(+) create mode 100644 src/iiwa_experement/iiwa_experement/scene_spawner.py create mode 100644 src/iiwa_experement/test/test_scene_spawner.py diff --git a/src/iiwa_experement/iiwa_experement/scene_spawner.py b/src/iiwa_experement/iiwa_experement/scene_spawner.py new file mode 100644 index 0000000..2d58958 --- /dev/null +++ b/src/iiwa_experement/iiwa_experement/scene_spawner.py @@ -0,0 +1,199 @@ +"""Deterministic scene spawner and readiness service.""" +from __future__ import annotations + +import re +from dataclasses import dataclass, field +from pathlib import Path + +import rclpy +from ament_index_python.packages import get_package_share_directory +from rclpy.executors import ExternalShutdownException +from rclpy.node import Node +from std_srvs.srv import Trigger +from webots_ros2_msgs.srv import SpawnNodeFromString + +from .scenario import ScenarioSpec, load_scenario +from .webots_nodes import build_nodes + + +SPAWN_SERVICE = '/Ros2Supervisor/spawn_node_from_string' +READY_SERVICE = '/iiwa_experement/scene_ready' +DEFAULT_SERVICE_TIMEOUT = 120.0 +DEFAULT_RESPONSE_TIMEOUT = 30.0 + +_NAME_RE = re.compile(r'name "([^"]+)"') + + +class SpawnError(RuntimeError): + """A spawn request failed or its outcome is unknown.""" + + +@dataclass +class Readiness: + ready: bool = False + message: str = 'scene not ready' + + +@dataclass +class ScenePlan: + experiment_id: int + spec: ScenarioSpec + nodes: list[str] + readiness: Readiness = field(default_factory=Readiness) + + +def node_name(data: str) -> str: + match = _NAME_RE.search(data) + return match.group(1) if match else 'unnamed object' + + +def build_plan( + experiment_id: int, + scene_config: Path | str, + asset_root: Path | str, +) -> ScenePlan: + """Validate the experiment and prepare every node before spawning.""" + spec = load_scenario(experiment_id, scene_config) + return ScenePlan(experiment_id, spec, build_nodes(spec, asset_root)) + + +def spawn_nodes( + node, + client, + nodes: list[str], + *, + service_timeout_sec: float = DEFAULT_SERVICE_TIMEOUT, + response_timeout_sec: float = DEFAULT_RESPONSE_TIMEOUT, + spin_until_future_complete=None, +) -> None: + """Spawn nodes sequentially through the Webots supervisor. + + A timeout on the response is ambiguous: the first request may still have + been executed. It is reported as an error and is never retried, because a + retry could create a duplicate object. + """ + spin = spin_until_future_complete or rclpy.spin_until_future_complete + try: + available = client.wait_for_service(timeout_sec=service_timeout_sec) + except Exception as error: # pragma: no cover - depends on rclpy internals + raise SpawnError(f'Webots supervisor service failed: {error}') from error + if not available: + raise SpawnError( + f'Webots supervisor service {SPAWN_SERVICE} unavailable after ' + f'{service_timeout_sec:g} s; no objects spawned' + ) + + logger = node.get_logger() + total = len(nodes) + for index, data in enumerate(nodes, start=1): + name = node_name(data) + future = client.call_async(SpawnNodeFromString.Request(data=data)) + spin(node, future, timeout_sec=response_timeout_sec) + if not future.done(): + raise SpawnError( + f'{name}: spawn request timed out after {response_timeout_sec:g} s; ' + 'result unknown, no retry' + ) + try: + response = future.result() + except Exception as error: + raise SpawnError(f'{name}: spawn call failed: {error}') from error + if response is None: + raise SpawnError(f'{name}: spawn returned no response') + if not response.success: + raise SpawnError( + f'{name}: Webots rejected the object ({index - 1}/{total} accepted)' + ) + logger.info(f'Spawned {index}/{total}: {name}') + + +def execute_plan( + plan: ScenePlan, + client, + node, + *, + service_timeout_sec: float = DEFAULT_SERVICE_TIMEOUT, + response_timeout_sec: float = DEFAULT_RESPONSE_TIMEOUT, + spin_until_future_complete=None, +) -> None: + """Run a plan and publish readiness only after the final success.""" + try: + spawn_nodes( + node, + client, + plan.nodes, + service_timeout_sec=service_timeout_sec, + response_timeout_sec=response_timeout_sec, + spin_until_future_complete=spin_until_future_complete, + ) + except Exception as error: + plan.readiness.ready = False + plan.readiness.message = ( + f'experiment {plan.experiment_id}: {error}' + ) + raise + plan.readiness.ready = True + plan.readiness.message = ( + f'experiment {plan.experiment_id}: {len(plan.nodes)} objects spawned' + ) + + +class SceneSpawnerNode(Node): + def __init__(self) -> None: + super().__init__('scene_spawner') + default_config = str( + Path(get_package_share_directory('iiwa_experement')) + / 'config' + / 'scenes.yaml' + ) + self.declare_parameter('experiment_id', 0) + self.declare_parameter('scene_config', default_config) + self._readiness = Readiness() + self.create_service(Trigger, READY_SERVICE, self._handle_ready) + + def _handle_ready(self, request, response): + response.success = self._readiness.ready + response.message = self._readiness.message + return response + + def run(self) -> None: + experiment_id = int(self.get_parameter('experiment_id').value) + scene_config = str(self.get_parameter('scene_config').value) + asset_root = ( + Path(get_package_share_directory('iiwa_description')) + / 'objects' + / 'grant' + ) + + self.get_logger().info( + f'Preparing experiment {experiment_id} from {scene_config}' + ) + plan = build_plan(experiment_id, scene_config, asset_root) + self._readiness = plan.readiness + client = self.create_client(SpawnNodeFromString, SPAWN_SERVICE) + execute_plan(plan, client, self) + self.get_logger().info(self._readiness.message) + + +def main(args=None) -> None: + rclpy.init(args=args) + node = SceneSpawnerNode() + exit_code = 0 + try: + node.run() + rclpy.spin(node) + except (KeyboardInterrupt, ExternalShutdownException): + pass + except Exception as error: + node.get_logger().error(f'Scene spawn failed: {error}') + exit_code = 1 + finally: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + if exit_code: + raise SystemExit(exit_code) + + +if __name__ == '__main__': + main() diff --git a/src/iiwa_experement/test/test_scene_spawner.py b/src/iiwa_experement/test/test_scene_spawner.py new file mode 100644 index 0000000..cff4eb2 --- /dev/null +++ b/src/iiwa_experement/test/test_scene_spawner.py @@ -0,0 +1,172 @@ +"""The spawner sends every node once and only signals readiness at the end.""" +from pathlib import Path + +import pytest +from ament_index_python.packages import get_package_share_directory + +from iiwa_experement.scenario import ScenarioError, load_scenario +from iiwa_experement.scene_spawner import ( + Readiness, + ScenePlan, + SpawnError, + build_plan, + execute_plan, + node_name, + spawn_nodes, +) +from iiwa_experement.webots_nodes import build_nodes + + +PACKAGE = Path(__file__).resolve().parents[1] +SCENES = PACKAGE / 'config' / 'scenes.yaml' +ASSET_ROOT = ( + Path(get_package_share_directory('iiwa_description')) / 'objects' / 'grant' +) + + +class FakeLogger: + def __init__(self): + self.messages = [] + + def info(self, message): + self.messages.append(message) + + +class FakeNode: + def __init__(self): + self.logger = FakeLogger() + + def get_logger(self): + return self.logger + + +class FakeFuture: + def __init__(self, behavior): + self.behavior = behavior + self.completed = behavior != 'timeout' + + def done(self): + return self.completed + + def result(self): + if isinstance(self.behavior, Exception): + raise self.behavior + if self.behavior == 'reject': + return type('Response', (), {'success': False})() + return type('Response', (), {'success': True})() + + +class FakeClient: + def __init__(self, behaviors=(), available=True): + self.behaviors = list(behaviors) + self.available = available + self.calls = [] + self.readiness_during_calls = [] + self.readiness = None + + def wait_for_service(self, timeout_sec): + return self.available + + def call_async(self, request): + self.calls.append(request.data) + if self.readiness is not None: + self.readiness_during_calls.append(self.readiness.ready) + index = len(self.calls) - 1 + behavior = self.behaviors[index] if index < len(self.behaviors) else 'ok' + return FakeFuture(behavior) + + +def fake_spin(node, future, timeout_sec): + return None + + +def run_plan(nodes, client, readiness=None, **kwargs): + plan = ScenePlan( + experiment_id=1, + spec=load_scenario(1, SCENES), + nodes=list(nodes), + readiness=readiness or Readiness(), + ) + client.readiness = plan.readiness + execute_plan( + plan, + client, + FakeNode(), + spin_until_future_complete=fake_spin, + **kwargs, + ) + return plan + + +def test_successful_spawn_sends_build_nodes_in_order_and_signals_at_the_end(): + nodes = build_nodes(load_scenario(6, SCENES), ASSET_ROOT) + client = FakeClient() + plan = run_plan(nodes, client) + assert client.calls == nodes + assert client.readiness_during_calls == [False] * len(nodes) + assert plan.readiness.ready is True + assert plan.readiness.message == f'experiment 1: {len(nodes)} objects spawned' + + +def test_node_name_is_used_for_progress_and_errors(): + assert node_name('Solid {\n name "grant_cap"\n}') == 'grant_cap' + assert node_name('Solid {}') == 'unnamed object' + + +def test_missing_supervisor_fails_without_spawning(): + client = FakeClient(available=False) + with pytest.raises(SpawnError, match='unavailable'): + run_plan(['Solid { name "grant_cap" }'], client) + assert client.calls == [] + + +def test_rejected_object_names_the_last_object_and_keeps_readiness_false(): + client = FakeClient(['ok', 'reject']) + readiness = Readiness() + with pytest.raises(SpawnError, match='grant_body'): + run_plan( + ['Solid { name "grant_fixture" }', 'Solid { name "grant_body" }'], + client, + readiness, + ) + assert len(client.calls) == 2 + assert readiness.ready is False + assert 'grant_body' in readiness.message + + +def test_exception_names_the_last_object(): + client = FakeClient(['ok', RuntimeError('service exploded')]) + with pytest.raises(SpawnError, match='grant_body.*service exploded'): + run_plan( + ['Solid { name "grant_fixture" }', 'Solid { name "grant_body" }'], + client, + ) + + +def test_response_timeout_is_not_retried(): + client = FakeClient(['ok', 'timeout', 'ok']) + with pytest.raises(SpawnError, match='grant_body.*timed out.*no retry'): + run_plan( + ['Solid { name "grant_fixture" }', 'Solid { name "grant_body" }'], + client, + ) + assert len(client.calls) == 2 + + +def test_duplicate_name_rejection_is_not_hidden(): + client = FakeClient(['reject']) + with pytest.raises(SpawnError, match='grant_fixture.*rejected'): + run_plan(['Solid { name "grant_fixture" }'], client) + + +def test_build_plan_rejects_an_invalid_id_before_spawning(): + with pytest.raises(ScenarioError): + build_plan(0, SCENES, ASSET_ROOT) + with pytest.raises(ScenarioError): + build_plan(21, SCENES, ASSET_ROOT) + + +def test_build_plan_uses_the_full_build_nodes_list(): + plan = build_plan(19, SCENES, ASSET_ROOT) + assert plan.nodes == build_nodes(plan.spec, ASSET_ROOT) + assert plan.readiness.ready is False