feat: spawn deterministic grant scene
This commit is contained in:
Даниил Грабарь
@@ -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()
|
||||
@@ -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
|
||||
Reference in New Issue
Block a user