caowangrenbo
5e2017de69
- s10_robot.py: protocol framing, mTLS client, heartbeat, status and control APIs - demo_stand_rotate_lie.py: stand, rotate in place, lie down with safe recovery - status_probe.py: read-only status subscription - tests: protocol unit tests and local fake robot - docs: remote control command reference
242 lines
8.3 KiB
Python
242 lines
8.3 KiB
Python
#!/usr/bin/env python3
|
||
"""演示：起立 → 原地旋转一段时间 → 停止 → 趴下恢复。
|
||
|
||
用法::
|
||
|
||
uv run python demo_stand_rotate_lie.py --seconds 5 --yaw 0.3
|
||
|
||
# 指定证书目录和机器人地址
|
||
uv run python demo_stand_rotate_lie.py --certs /path/to/certs --host 10.21.33.103
|
||
|
||
流程与《山猫S10 软件开发指南》的遥控时序一致：
|
||
|
||
1. mTLS 连接 30003/TCP，开启 2Hz 心跳（机器人随后开始主动上报状态）
|
||
2. 切到常规模式
|
||
3. 下发站立，等待进入 RL 控制状态（ASDU 站立完成后会自动进入）
|
||
4. 切到基础步态（必须在完全停止时切换）
|
||
5. 以 10Hz 持续下发 Yaw 轴指令，原地旋转指定秒数
|
||
6. 下发全 0 停止，再下发趴下，等待回到趴卧/待机状态
|
||
|
||
任何异常、Ctrl-C 或超时都会走安全收尾：先发全 0 停止，若仍处于站立/RL
|
||
状态则下发趴下，最后关闭连接。
|
||
"""
|
||
|
||
from __future__ import annotations
|
||
|
||
import argparse
|
||
import logging
|
||
import os
|
||
import signal
|
||
import sys
|
||
import threading
|
||
import time
|
||
from pathlib import Path
|
||
|
||
from s10_robot import (
|
||
Gait,
|
||
Mode,
|
||
MotionState,
|
||
S10Client,
|
||
S10CommandError,
|
||
S10Error,
|
||
S10Timeout,
|
||
)
|
||
|
||
log = logging.getLogger("demo")
|
||
|
||
ABORT = threading.Event()
|
||
|
||
|
||
def _handle_signal(signum, _frame):
|
||
log.warning("收到信号 %s，准备安全收尾", signal.Signals(signum).name)
|
||
ABORT.set()
|
||
|
||
|
||
def resolve_certs(explicit: str | None) -> Path:
|
||
candidates = []
|
||
if explicit:
|
||
candidates.append(Path(explicit))
|
||
if os.environ.get("S10_CERTS"):
|
||
candidates.append(Path(os.environ["S10_CERTS"]))
|
||
candidates.append(Path("certs"))
|
||
candidates.append(
|
||
Path.home()
|
||
/ ".zcode/workspace/draft-183385f3-4f14-43ad-ab22-e5deace0e05c/backup/certs"
|
||
)
|
||
for path in candidates:
|
||
if (path / "client_chain.crt").is_file() and (path / "client.key").is_file():
|
||
return path
|
||
raise SystemExit(
|
||
"找不到证书目录，请用 --certs 指定包含 client_chain.crt / client.key / root_ca.crt 的目录"
|
||
)
|
||
|
||
|
||
def describe(robot: S10Client) -> str:
|
||
basic = robot.basic_status
|
||
return (
|
||
f"MotionState={basic.get('MotionState')} Gait={basic.get('Gait')} "
|
||
f"Mode={basic.get('ControlUsageMode')} HES={basic.get('HES')}"
|
||
)
|
||
|
||
|
||
def check_alive(robot: S10Client) -> None:
|
||
basic = robot.basic_status
|
||
if basic.get("HES") == 1:
|
||
raise S10Error("硬急停已触发，终止运动")
|
||
errors = robot.active_errors
|
||
for err in errors:
|
||
if max(err.get("Severities") or [0]) >= 5:
|
||
raise S10Error(f"出现 FATAL 故障: {err.get('Name')} ({err.get('Code')})")
|
||
|
||
|
||
def rotate_in_place(robot: S10Client, yaw: float, seconds: float, rate_hz: float) -> None:
|
||
"""以固定频率持续下发 Yaw 轴指令，旋转指定时间。"""
|
||
period = 1.0 / rate_hz
|
||
deadline = time.monotonic() + seconds
|
||
next_tick = time.monotonic()
|
||
sent = 0
|
||
log.info("开始原地旋转: yaw=%+.2f, %g 秒, %g Hz", yaw, seconds, rate_hz)
|
||
while time.monotonic() < deadline:
|
||
if ABORT.is_set():
|
||
log.warning("旋转被中断")
|
||
break
|
||
check_alive(robot)
|
||
state = robot.basic_status.get("MotionState")
|
||
if state != MotionState.RL:
|
||
raise S10Error(f"旋转过程中运动状态异常: MotionState={state}（预期 {MotionState.RL}）")
|
||
robot.move(yaw=yaw)
|
||
sent += 1
|
||
next_tick += period
|
||
time.sleep(max(0.0, next_tick - time.monotonic()))
|
||
log.info("旋转结束，共下发 %d 帧轴指令", sent)
|
||
|
||
|
||
def safe_recover(robot: S10Client) -> None:
|
||
"""异常/中断后的安全收尾：停止移动，必要时趴下。"""
|
||
if not robot.connected:
|
||
return
|
||
try:
|
||
robot.stop()
|
||
except S10Error as exc:
|
||
log.warning("发送停止指令失败: %s", exc)
|
||
state = robot.basic_status.get("MotionState")
|
||
if state in (MotionState.STANDING, MotionState.RL, MotionState.JOINT_DAMP):
|
||
try:
|
||
log.info("安全收尾：下发趴下")
|
||
robot.lie_down()
|
||
robot.wait_motion_state({MotionState.LYING, MotionState.DEFAULT}, timeout=15)
|
||
log.info("已趴下: %s", describe(robot))
|
||
except (S10Error, S10Timeout) as exc:
|
||
log.error("趴下失败，请手动接管: %s", exc)
|
||
|
||
|
||
def run(args: argparse.Namespace) -> int:
|
||
certs = resolve_certs(args.certs)
|
||
log.info("证书目录: %s", certs)
|
||
|
||
robot = S10Client(
|
||
host=args.host,
|
||
port=args.port,
|
||
cert=str(certs / "client_chain.crt"),
|
||
key=str(certs / "client.key"),
|
||
ca=str(certs / "root_ca.crt"),
|
||
verify=args.verify,
|
||
heartbeat_hz=args.heartbeat_hz,
|
||
request_timeout=args.timeout,
|
||
)
|
||
|
||
try:
|
||
robot.connect()
|
||
log.info("等待首次状态上报…")
|
||
try:
|
||
robot.wait_basic("MotionState", range(-2, 0x1002), timeout=5.0)
|
||
except S10Timeout:
|
||
log.warning("5 秒内未收到状态上报，继续尝试下发指令")
|
||
log.info("初始状态: %s", describe(robot))
|
||
|
||
if ABORT.is_set():
|
||
return 130
|
||
|
||
log.info("① 切换到常规模式")
|
||
robot.set_mode(Mode.NORMAL)
|
||
try:
|
||
robot.wait_mode({Mode.NORMAL}, timeout=3)
|
||
except S10Timeout:
|
||
log.warning("模式回读超时，继续")
|
||
|
||
log.info("② 下发站立")
|
||
robot.stand()
|
||
log.info(" 等待进入 RL 控制状态（最多 25 秒）…")
|
||
robot.wait_motion_state({MotionState.RL}, timeout=25)
|
||
log.info(" 已站立并进入 RL 控制: %s", describe(robot))
|
||
|
||
log.info("③ 切换到基础步态 0x%04X", Gait.BASIC)
|
||
robot.set_gait(Gait.BASIC)
|
||
time.sleep(0.3)
|
||
|
||
log.info("④ 原地旋转")
|
||
rotate_in_place(robot, args.yaw, args.seconds, args.rate)
|
||
|
||
log.info("⑤ 停止移动，等待完全静止")
|
||
robot.stop()
|
||
robot.stop()
|
||
time.sleep(0.5)
|
||
|
||
log.info("⑥ 下发趴下")
|
||
robot.lie_down()
|
||
state = robot.wait_motion_state({MotionState.LYING, MotionState.DEFAULT}, timeout=20)
|
||
log.info(" 已恢复: MotionState=%s（%s）", state, describe(robot))
|
||
return 0
|
||
|
||
except S10CommandError as exc:
|
||
log.error("机器人拒绝指令: %s", exc)
|
||
return 2
|
||
except (S10Error, S10Timeout) as exc:
|
||
log.error("执行失败: %s", exc)
|
||
return 1
|
||
finally:
|
||
safe_recover(robot)
|
||
robot.close()
|
||
log.info("连接已关闭")
|
||
|
||
|
||
def main(argv: list[str] | None = None) -> int:
|
||
parser = argparse.ArgumentParser(
|
||
description="山猫 S10：起立 → 原地旋转 → 趴下 恢复演示",
|
||
formatter_class=argparse.ArgumentDefaultsHelpFormatter,
|
||
)
|
||
parser.add_argument("--host", default="10.21.33.103", help="机器人地址")
|
||
parser.add_argument("--port", type=int, default=30003, help="TLS 端口")
|
||
parser.add_argument("--certs", default=None, help="证书目录（含 client_chain.crt/client.key/root_ca.crt）")
|
||
parser.add_argument("--yaw", type=float, default=0.3,
|
||
help="偏航比例 [-1,1]，正值为逆时针")
|
||
parser.add_argument("--seconds", type=float, default=5.0, help="旋转时长（秒）")
|
||
parser.add_argument("--rate", type=float, default=10.0, help="轴指令下发频率（Hz）")
|
||
parser.add_argument("--heartbeat-hz", type=float, default=2.0, help="心跳频率（Hz）")
|
||
parser.add_argument("--timeout", type=float, default=5.0, help="单条指令响应超时（秒）")
|
||
parser.add_argument("--verify", action="store_true", help="严格校验服务端证书链")
|
||
parser.add_argument("-v", "--verbose", action="store_true", help="输出调试日志")
|
||
args = parser.parse_args(argv)
|
||
|
||
if not -1.0 <= args.yaw <= 1.0:
|
||
parser.error("--yaw 必须在 [-1, 1] 之间")
|
||
if args.seconds <= 0:
|
||
parser.error("--seconds 必须大于 0")
|
||
if args.rate <= 0:
|
||
parser.error("--rate 必须大于 0")
|
||
|
||
logging.basicConfig(
|
||
level=logging.DEBUG if args.verbose else logging.INFO,
|
||
format="%(asctime)s.%(msecs)03d %(levelname)-7s %(name)s: %(message)s",
|
||
datefmt="%H:%M:%S",
|
||
)
|
||
|
||
signal.signal(signal.SIGINT, _handle_signal)
|
||
signal.signal(signal.SIGTERM, _handle_signal)
|
||
|
||
return run(args)
|
||
|
||
|
||
if __name__ == "__main__":
|
||
sys.exit(main())
|