feat: spawn deterministic grant scene

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()

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