File size: 4,192 Bytes
d8290eb | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 | #!/usr/bin/env python3
"""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))
# H2BR reserves 32 doubles; planner-v3 occupies the first 27.
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())
|