| |
| """Read-only readiness window for original RL; never claims a command writer.""" |
| from __future__ import annotations |
|
|
| import argparse |
| from pathlib import Path |
| import sys |
| import time |
|
|
| ROOT = Path(__file__).resolve().parents[1] |
| if str(ROOT) not in sys.path: |
| sys.path.insert(0, str(ROOT)) |
|
|
| from runtime.h2_shm_bridge import H2SharedMemoryBridge, SHM_NAME |
| from scripts.inspect_shm_state import validate_snapshot |
| from scripts.inspect_mocap_pipeline import EXPECTED_PACKET_LEN, InspectionFailure, validate_planner_packet |
|
|
|
|
| class ReadinessWindow: |
| def __init__(self, interface, origin): |
| self.interface, self.origin = interface, origin |
| self.reset() |
|
|
| def reset(self): |
| self.since = None |
| self.last = None |
| self.counts = [0, 0, 0] |
| self.command_id = None |
|
|
| def observe(self, state, now): |
| result = validate_snapshot(state, require_planner=True, require_disarmed=True, |
| expected_interface=self.interface, now_ns=int(now*1e9)) |
| if not result.passed: |
| raise InspectionFailure("; ".join(result.failures)) |
| |
| packet = validate_planner_packet(state.tt_data[:EXPECTED_PACKET_LEN], now_monotonic_s=now, |
| expected_origin_m=self.origin, require_pose_alignment=False, |
| require_ball_source_freshness=False) |
| stamps = (state.state_stamp_ns, state.tt_stamp_ns, packet.robot_source_stamp) |
| if self.last is not None: |
| if any(current < old for current, old in zip(stamps, self.last)): |
| raise InspectionFailure("startup source timestamp moved backwards") |
| if packet.command_id < self.command_id: |
| raise InspectionFailure("startup planner command id moved backwards") |
| if self.since is None: |
| self.since = now |
| for i, stamp in enumerate(stamps): |
| if self.last is None or stamp > self.last[i]: |
| self.counts[i] += 1 |
| self.last, self.command_id = stamps, packet.command_id |
| return now-self.since >= 0.2-1e-9 and min(self.counts) >= 3 |
|
|
|
|
| def wait_ready(interface, origin, *, timeout_s=10.0, clock=time.monotonic, |
| sleep=time.sleep, factory=H2SharedMemoryBridge): |
| started = clock() |
| deadline = started + timeout_s |
| window = ReadinessWindow(interface, origin) |
| bridge = None |
| reason = "waiting for H2BR" |
| last_report = -float("inf") |
| print(f"[STARTUP] communication readiness begin timeout_s={timeout_s:g}", flush=True) |
| try: |
| while clock() < deadline: |
| try: |
| if bridge is None: |
| bridge = factory(SHM_NAME) |
| state = bridge.read_state() |
| now = clock() |
| if window.observe(state, now): |
| print(f"[STARTUP] communication ready elapsed_s={now-started:.3f} " |
| f"unique_state/planner/robot={window.counts}", flush=True) |
| return |
| reason = "waiting for 0.2s of healthy updates (at least 3 per source)" |
| except (FileNotFoundError, InspectionFailure) as error: |
| window.reset() |
| reason = str(error) |
| now = clock() |
| if now-last_report >= 1.0: |
| print(f"[STARTUP] communication not ready: {reason}", flush=True) |
| last_report = now |
| sleep(min(0.02, max(0.0, deadline-clock()))) |
| raise RuntimeError(f"communication readiness timed out after {timeout_s:g}s: {reason}") |
| finally: |
| if bridge is not None: |
| bridge.close() |
|
|
|
|
| def main(): |
| parser = argparse.ArgumentParser(description=__doc__) |
| parser.add_argument("--network-interface", required=True) |
| parser.add_argument("--origin", type=float, nargs=3, default=(-1.87, 0.0, -0.76)) |
| args = parser.parse_args() |
| try: |
| wait_ready(args.network_interface, args.origin) |
| except (OSError, RuntimeError, ValueError) as error: |
| print(f"[STARTUP ERROR] {error}", flush=True) |
| return 1 |
| return 0 |
|
|
|
|
| if __name__ == "__main__": |
| raise SystemExit(main()) |
|
|
