File size: 4,002 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 105 106 107 108 109 110 111 112 113 114 115 | #!/usr/bin/env python3
"""Inspect or explicitly release Unitree high-level motion control for H2.
The default operation is read-only. Releasing a motion mode is intentionally
double-gated because it changes robot state and must remain an operator action.
"""
from __future__ import annotations
import argparse
import json
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 unitree_sdk2py.comm.motion_switcher.motion_switcher_client import MotionSwitcherClient
from unitree_sdk2py.core.channel import ChannelFactoryInitialize
from runtime.motion_attestation import (
ATTESTATION_PATH,
motion_switcher_released,
write_attestation,
)
RELEASE_ACK = "RELEASE-H2-HIGH-LEVEL-CONTROL"
def _mode(client: MotionSwitcherClient) -> tuple[int, dict]:
status, result = client.CheckMode()
motion_switcher_released(status, result)
return int(status), result
def _released(result: dict) -> bool:
return motion_switcher_released(0, result)
def main() -> None:
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("--network-interface", required=True)
parser.add_argument("--release", action="store_true", help="Release the active high-level mode.")
parser.add_argument(
"--write-attestation",
action="store_true",
help="After a successful CheckMode, write the short-lived token required by the runner.",
)
parser.add_argument(
"--ack-release",
default="",
help=f"Required with --release; exact value: {RELEASE_ACK}",
)
args = parser.parse_args()
# A new check always invalidates any earlier token before contacting the
# robot, so a failed/active result cannot leave an old pass consumable.
try:
ATTESTATION_PATH.unlink()
except FileNotFoundError:
pass
interface = str(args.network_interface)
state_path = Path("/sys/class/net") / interface / "operstate"
if not state_path.is_file():
raise RuntimeError(f"network interface does not exist: {interface}")
state = state_path.read_text(encoding="utf-8").strip()
if state != "up":
raise RuntimeError(f"network interface {interface} is not UP (operstate={state!r})")
if args.release and args.ack_release != RELEASE_ACK:
raise RuntimeError(f"--release requires --ack-release {RELEASE_ACK}")
ChannelFactoryInitialize(0, interface)
client = MotionSwitcherClient()
client.SetTimeout(5.0)
client.Init()
_, result = _mode(client)
print("[MOTION MODE] " + json.dumps(result, sort_keys=True, ensure_ascii=False))
if not args.release:
if not _released(result):
raise SystemExit(
"active high-level motion mode detected; inspect it, then rerun with the explicit release gate"
)
print("[MOTION MODE PASS] no active high-level motion mode")
if args.write_attestation:
write_attestation(interface)
print(f"[MOTION ATTESTATION] wrote owner-only token {ATTESTATION_PATH}")
return
for attempt in range(1, 4):
if _released(result):
break
status, _ = client.ReleaseMode()
if status != 0:
raise RuntimeError(f"MotionSwitcher ReleaseMode failed on attempt {attempt}: status={status}")
time.sleep(1.0)
_, result = _mode(client)
print(
f"[MOTION MODE] post-release attempt={attempt} "
+ json.dumps(result, sort_keys=True, ensure_ascii=False)
)
if not _released(result):
raise RuntimeError("high-level motion mode remains active after three release attempts")
print("[MOTION MODE PASS] high-level motion control is released")
if args.write_attestation:
write_attestation(interface)
print(f"[MOTION ATTESTATION] wrote owner-only token {ATTESTATION_PATH}")
if __name__ == "__main__":
main()
|