commit 6dd1fa7e240d008781882e08e1cbde76ef710711 Author: deepseek Date: Sun Oct 25 17:53:11 2026 +0800 release: v1.0 - WL-PRO 轮组机器人遥控脚本 反向实现设备 WebSocket 控制协议,提供单次命令与常驻长连接两种模式。 核心内容: - robot-ctl.py 单次命令客户端,纯标准库无依赖 - robot-daemon.py 常驻守护进程,维持唯一长连接, 解决断开丢尾帧导致的速度残留 - rc.sh 向守护进程注入动作,内置 stable=1 硬锁 - docs/PROTOCOL.md 协议逆向与踩坑记录 - tests/fake-robot-ws.js 离线验证用假设备 实测标定 (50% 速度): - 前进/后退 ~8 cm/s - 左转/右转 ~35 deg/s 已知限制: - 设备为 Arduino WebSocketsServer,纯单向,不回传状态 - 仅摇杆与跳跃有效,linear/angular/方向键无效 - 网页与脚本不可同时连接,会互相覆盖帧 diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..323b1ab --- /dev/null +++ b/.gitignore @@ -0,0 +1,22 @@ +# 运行时产物 +daemon.log +daemon.pid +daemon-state.json +*.log + +# Python +__pycache__/ +*.py[cod] +*.pyo +.venv/ +venv/ + +# Node +node_modules/ + +# 编辑器 / 系统 +.vscode/ +.idea/ +.DS_Store +*.swp +.git-credentials diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..9dfb2f3 --- /dev/null +++ b/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2026 WL-PRO Robot Control contributors + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/README.md b/README.md new file mode 100644 index 0000000..77a9a8d --- /dev/null +++ b/README.md @@ -0,0 +1,157 @@ +# WL-PRO 轮组机器人遥控 + +用脚本驱动 WL-PRO 轮组机器人,绕开网页界面。 + +**版本**:1.0 + +--- + +## 这是什么 + +WL-PRO 的网页遥控页(`http:///`)通过 WebSocket 向设备端口 **81** 发 JSON 帧。 +本项目直接复现这个协议,让你能从命令行、脚本或自动化流程里控制机器人。 + +包含两种工作模式: + +| 模式 | 文件 | 适用 | +|---|---|---| +| 单次命令 | `robot-ctl.py` | 局域网直连、一次性动作 | +| 常驻守护 | `robot-daemon.py` + `rc.sh` | 远程 / 需要持续保持连接 | + +**远程场景请用常驻模式**,原因见 [docs/PROTOCOL.md](docs/PROTOCOL.md) 的「为什么必须常驻」。 + +--- + +## 快速开始 + +### 局域网直连(最简单) + +```bash +python3 robot-ctl.py --host 192.168.4.1 joy 0 50 1000 +``` + +### 远程常驻(推荐) + +```bash +# 1. 启动守护进程(保持唯一长连接) +setsid nohup python3 robot-daemon.py --host <公网IP> --port <映射端口> \ + > daemon.log 2>&1 < /dev/null & +echo $! > daemon.pid + +# 2. 下发动作 +bash rc.sh go 50 1000 # 前进 1 秒 +bash rc.sh jump # 跳一下 +bash rc.sh stop # 立即停 +``` + +--- + +## 协议要点 + +设备只接受**摇杆**和**跳跃**,其他字段无效(实测)。 + +每帧完整 JSON: + +```json +{ + "dir": "stop", "mode": "basic", "roll": 0, "height": 38, + "linear": 0, "angular": 0, "stable": 1, + "joy_x": 0, "joy_y": 50 +} +``` + +| 字段 | 范围 | 说明 | +|---|---|---| +| `joy_y` | -100 ~ 100 | 正=前进,负=后退 | +| `joy_x` | -100 ~ 100 | 正=右转,负=左转 | +| `dir` | `stop` / `jump` | 只有 `jump` 有效 | +| `stable` | 0 / 1 | 主动保持平衡,**必须保持 1** | +| `height` | 32 ~ 85 | 站高 mm | +| `roll` | -30 ~ 30 | 设备侧无效,固定 0 | +| `linear` / `angular` | — | 设备侧无效,固定 0 | + +--- + +## 实测标定值(50% 速度) + +设备响应**非线性**,以下仅在 `joy` 绝对值 50 附近有效。 + +| 动作 | 参数 | 实测 | +|---|---|---| +| 前进 / 后退 | `joy_y = ±50` | **≈ 8 cm/s** | +| 左转 / 右转 | `joy_x = ±50` | **≈ 35 °/s** | +| 跳跃 | `dir: jump` | 点动 300ms | + +**距离换算** + +| 目标 | 时长 | +|---|---| +| 5 cm | 600 ms | +| 10 cm | 1200 ms | +| 20 cm | 2500 ms | +| 30 cm | 3800 ms | +| 50 cm | 6000 ms | + +**角度换算** + +| 目标 | 时长 | +|---|---| +| 15° | 430 ms | +| 35° | 1000 ms | +| 45° | 1300 ms | +| 90° | 2600 ms | +| 180° | 5100 ms(建议分两段) | + +> 速度低于约 50% 时推进力不足,机器人基本不动。超过 80% 明显更快但更难控。 +> 单次动作超过 3 秒,轮子打滑和地面不平会让误差累积,长距离建议分段执行并中途校正。 + +--- + +## 安全约束 + +本项目内置以下硬约束,防止误操作: + +1. **`stable` 恒为 1** —— 主动保持平衡不可通过脚本关闭 +2. **`height` 恒为 38 / `roll` 恒为 0** —— 动作帧不会意外改变姿态 +3. **动作帧自动补停止帧** —— 到期立即归零 + +第 1 条在 `rc.sh` 里有显式拒绝分支: + +```bash +$ bash rc.sh stable 0 +[rc] 拒绝:主动保持平衡不允许关闭(用户明确指令)。 +``` + +--- + +## 文件说明 + +| 文件 | 作用 | +|---|---| +| `robot-ctl.py` | 单次命令客户端,纯标准库,无依赖 | +| `robot-daemon.py` | 常驻守护进程,维持唯一长连接 | +| `rc.sh` | 向守护进程注入动作(写状态文件) | +| `docs/PROTOCOL.md` | 协议逆向细节与踩坑记录 | +| `docs/device-page.html` | 设备原始网页快照(协议参考) | +| `tests/fake-robot-ws.js` | 假设备,用于离线验证帧格式 | + +--- + +## 依赖 + +- Python 3.7+(`robot-ctl.py` / `robot-daemon.py` 仅用标准库) +- Node.js 18+(仅 `tests/fake-robot-ws.js` 需要,需 `ws` 包) + +--- + +## 已知限制 + +1. **设备不回传状态** —— 固件是 Arduino WebSocketsServer,纯单向,无法确认动作是否生效,只能靠肉眼观察 +2. **同一时刻只能一个客户端** —— 网页和脚本同时连会互相覆盖指令 +3. **无位置反馈** —— 不能闭环控制,长距离移动会累积误差 + +--- + +## 许可 + +MIT,见 [LICENSE](LICENSE)。 diff --git a/VERSION b/VERSION new file mode 100644 index 0000000..d3827e7 --- /dev/null +++ b/VERSION @@ -0,0 +1 @@ +1.0 diff --git a/docs/PROTOCOL.md b/docs/PROTOCOL.md new file mode 100644 index 0000000..31c925d --- /dev/null +++ b/docs/PROTOCOL.md @@ -0,0 +1,181 @@ +# 协议逆向笔记 + +记录 WL-PRO 轮组机器人的通信协议,以及踩过的坑。内容全部来自实测,不是推测。 + +--- + +## 设备识别 + +WebSocket 握手响应头泄露了固件身份: + +``` +HTTP/1.1 101 Switching Protocols +Server: arduino-WebSocketsServer +Upgrade: websocket +Connection: Upgrade +Sec-WebSocket-Version: 13 +Sec-WebSocket-Accept: ... +``` + +结论:固件基于 **Arduino WebSocketsServer** 库(ESP32 / Arduino 平台)。 + +这个库的关键特性:**纯单向**。 + +- 握手完成后不发送任何数据 +- 收到客户端帧后不返回任何 ack 或状态 +- 因此**永远无法确认动作是否执行**,只能靠肉眼观察 + +--- + +## 端点 + +| 用途 | 地址 | +|---|---| +| 遥控页 | `http:///` | +| 控制通道 | `ws://:81/` | +| WiFi 设置 | `http:///wifi` | + +控制通道固定 **81 端口**,路径为 `/`。 + +--- + +## 帧格式 + +完整的 JSON 文本帧,每次发送必须包含全部字段: + +```json +{ + "dir": "stop", + "mode": "basic", + "roll": 0, + "height": 38, + "linear": 0, + "angular": 0, + "stable": 1, + "joy_x": 0, + "joy_y": 50 +} +``` + +### 字段详解 + +| 字段 | 范围 | 有效 | 说明 | +|---|---|---|---| +| `joy_y` | -100 ~ 100 | ✅ | 纵向摇杆。正值前进,负值后退 | +| `joy_x` | -100 ~ 100 | ✅ | 横向摇杆。正值右转,负值左转 | +| `dir` | 字符串 | ✅ 仅 `jump` | `jump` 触发跳跃;其余值(含 `forward`/`back`/`left`/`right`)**无效** | +| `stable` | 0 / 1 | ✅ | 主动保持平衡开关。1 = 开启 | +| `height` | 32 ~ 85 | ✅ | 站高,单位 mm | +| `roll` | -30 ~ 30 | ❌ | 滑块存在但设备侧无响应 | +| `linear` | -200 ~ 200 | ❌ | 单位 mm/s,设备侧无响应 | +| `angular` | -100 ~ 100 | ❌ | 单位 °/s,设备侧无响应 | +| `mode` | 字符串 | — | 固定 `basic` | + +**只有摇杆(`joy_x`/`joy_y`)、跳跃(`dir:"jump"`)、`stable`、`height` 真正生效。** + +`roll` / `linear` / `angular` 三个滑块在网页上有 UI,但拖动了机器人没反应——用户确认过,不要浪费时间试。 + +--- + +## 为什么必须常驻连接 + +这是本项目最重要的一条经验。 + +### 现象 + +早期实现是「每次命令新建连接 → 推几百毫秒帧 → 断开」。结果: + +``` +17:32 执行 joy_y=-60 持续 600ms,命令已退出 +17:36 用户报告「还在退」 +``` + +**命令结束四分钟后,机器人还在移动。** + +### 原因 + +设备对短连接的处理不可靠:客户端推完动作帧后立刻发 WebSocket close 帧,**归零帧可能随连接关闭一起丢失**,设备保留最后一次收到的速度值。 + +设备是纯单向的,不会告诉我们它记住了什么。 + +### 解法 + +保持**单条长连接永不主动断开**: + +- 空闲时每 120ms 推一帧全零(`dir:"stop"`, `joy:0,0`),与网页空闲时的行为一致 +- 动作通过状态文件注入,带到期时间,到期自动归零 +- 断开前先补 12 帧零,再发 close + +`robot-daemon.py` 实现的就是这个模式。 + +--- + +## 网页抢帧冲突 + +网页里这个定时器是隐形杀手: + +```js +setInterval(function () { + send_data(); // dir 恒为 "stop",joy_x/joy_y 取摇杆当前位置 +}, 150); +``` + +不动摇杆时摇杆自动回中,所以它**每 150ms 发一帧 `joy=0,0`**。 + +如果网页和脚本同时连着设备,两个独立 WS 客户端的帧会交错: + +``` +脚本: joy_y = -60 +网页: joy_y = 0 ← 150ms 后 +脚本: joy_y = -60 +网页: joy_y = 0 +``` + +设备收到的是来回跳变的矛盾指令,表现就是"几乎没动"或原地抖动。 + +**结论:操作前必须关掉网页标签页**(关闭,不是最小化)。 + +同理,网页的方向键是 `mousedown` 发一帧 `dir`、`mouseup` 发一帧 `stop`,中间还有 150ms 定时器插一帧 `stop`——**实际效果是 ≤150ms 的瞬间点动**,不是持续移动。 + +早期我把 `dir:"forward"` 连推 1 秒(7 帧),等于连下 7 次动作指令不给停止,姿态控制被打乱,机器人直接摔倒。 + +--- + +## 非线性响应 + +速度与 `joy` 值不是线性的: + +| `joy` 绝对值 | 实测表现 | +|---|---| +| 20 | 几乎不动 | +| 30 | 前进 3 秒变化很小 | +| 50 | ≈ 8 cm/s(可用基准) | +| 80 | 明显更快 | + +**低于约 50% 推进力不足**。标定表只在 50% 附近有效,换速度需重新标定。 + +--- + +## 远程访问 + +设备通常藏在局域网里。远程控制需要一条端口隧道: + +- frpc 客户端在局域网机器上 +- 代理类型必须选 **tcp**(`http`/`https` 是 frps 的虚拟主机模式,需要 `vhostHTTPPort`,通常没开) +- `localIP` 必须填**设备真实 IP**,不能填 `127.0.0.1`(那是客户端自己) +- `localPort` = 81,`remotePort` = 云服务器上已放行的端口 + +云服务器安全组必须放行 `remotePort`。被 DROP 的端口表现为连接超时(不是拒绝),很容易误判成网络问题。 + +--- + +## 调试技巧 + +判断隧道是否真的打通,看 frps 的**出向流量**: + +``` +in=579B / out=0B ← 后端没收到,或连错了对象 +in=2155B / out=964B ← 通了 +``` + +`in` 是我们发出的字节,`out` 是设备返回的字节。纯单向设备握手后不再说话,但**握手响应本身**会让 `out` 增长。`out=0` 说明数据根本没到达设备。 diff --git a/docs/device-page.html b/docs/device-page.html new file mode 100644 index 0000000..679ed1c --- /dev/null +++ b/docs/device-page.html @@ -0,0 +1,696 @@ + + + + + + + legged wheel robot web ctrl + + + + + +

WL-PRO WiFi遥控模式

+ +
+ Robot Go! +
+
+ +
+
+
+ + + +
+
+
+
+ + +
+
+ + +
+ +
+ + +
+
+ + +
+
+ +
+ + + + + +
+ + + + + + + + + + + diff --git a/rc.sh b/rc.sh new file mode 100755 index 0000000..f2fd949 --- /dev/null +++ b/rc.sh @@ -0,0 +1,93 @@ +#!/usr/bin/env bash +# 通过常驻守护进程注入动作(只写状态文件,不自己开连接) +# +# 前提:robot-daemon.py 正在运行,保持唯一长连接。 +# +# 用法: +# bash rc.sh go -40 800 # 后退 40% 速度 0.8 秒 +# bash rc.sh go 40 800 # 前进 +# bash rc.sh strafe 40 800 # 右转(负值左转) +# bash rc.sh joy 30 -40 800 # 任意摇杆组合 +# bash rc.sh jump # 跳一下 +# bash rc.sh stop # 立即归零 +# bash rc.sh status # 看当前注入状态 + 守护进程是否活着 +# bash rc.sh hold # 持续注入零帧(确保静止,默认 5 秒) + +set -u +D="$(cd "$(dirname "$0")" && pwd)" +STATE="${RC_STATE:-$D/daemon-state.json}" +PIDF="$D/daemon.pid" + +die() { echo "[rc] $*" >&2; exit 1; } + +alive() { + [ -f "$PIDF" ] || return 1 + local pid; pid="$(cat "$PIDF" 2>/dev/null || true)" + [ -n "$pid" ] || return 1 + kill -0 "$pid" 2>/dev/null || return 1 + return 0 +} + +write_state() { # joy_x joy_y dir seconds + local jx="$1" jy="$2" dr="$3" secs="$4" + local until + if [ "${secs:-0}" = "0" ]; then + until=0 + else + until="$(python3 -c "import time;print(f'{time.time()+float($secs):.3f}')")" + fi + python3 - "$STATE" "$jx" "$jy" "$dr" "$until" <<'PY' +import json, sys, os, tempfile +path, jx, jy, dr, until = sys.argv[1], int(sys.argv[2]), int(sys.argv[3]), sys.argv[4], float(sys.argv[5]) +data = {"joy_x": jx, "joy_y": jy, "dir": dr, "until": until} +d = os.path.dirname(os.path.abspath(path)) +fd, tmp = tempfile.mkstemp(dir=d) +with os.fdopen(fd, "w") as f: + json.dump(data, f) +os.replace(tmp, path) +PY +} + +CMD="${1:-}"; shift || true +alive || die "守护进程未运行。先启动:setsid nohup python3 $D/robot-daemon.py > $D/daemon.log 2>&1 &" + +# 平衡开关硬锁:拒绝关闭 +case "$CMD" in + stable|balance) + die "拒绝:主动保持平衡不允许关闭(用户明确指令)。" + ;; +esac + +case "$CMD" in + go) + Y="${1:-50}"; MS="${2:-800}" + write_state 0 "$Y" stop "$(python3 -c "print(float($MS)/1000)")" + echo "[rc] 摇杆 y=$Y 持续 ${MS}ms" ;; + strafe) + X="${1:-50}"; MS="${2:-800}" + write_state "$X" 0 stop "$(python3 -c "print(float($MS)/1000)")" + echo "[rc] 摇杆 x=$X 持续 ${MS}ms" ;; + joy) + X="${1:-0}"; Y="${2:-0}"; MS="${3:-800}" + write_state "$X" "$Y" stop "$(python3 -c "print(float($MS)/1000)")" + echo "[rc] 摇杆 x=$X y=$Y 持续 ${MS}ms" ;; + jump) + MS="${1:-300}" + write_state 0 0 jump "$(python3 -c "print(float($MS)/1000)")" + echo "[rc] 跳 ${MS}ms" ;; + stop) + write_state 0 0 stop 0 + echo "[rc] 已归零" ;; + hold) + MS="${1:-5000}" + write_state 0 0 stop "$(python3 -c "print(float($MS)/1000)")" + echo "[rc] 保持零帧 ${MS}ms" ;; + status) + echo "[rc] 守护进程 PID: $(cat "$PIDF")" + echo "[rc] 状态文件: $STATE" + cat "$STATE" 2>/dev/null || echo "(无)" + echo + echo "[rc] 最近日志:"; tail -6 "$D/daemon.log" 2>/dev/null ;; + *) + sed -n '2,16p' "$0" | sed 's/^# \{0,1\}//'; exit 1 ;; +esac diff --git a/robot-ctl.js b/robot-ctl.js new file mode 100644 index 0000000..225a696 --- /dev/null +++ b/robot-ctl.js @@ -0,0 +1,244 @@ +#!/usr/bin/env node +/** + * WL-PRO 轮组机器人 遥控脚本(无外部依赖,Node >= 22 自带 WebSocket) + * + * 控制通道与网页一致的协议:ws://:81/ 发送 JSON 状态帧: + * {dir, mode, roll, height, linear, angular, stable, joy_x, joy_y} + * dir: stop | forward | back | left | right | jump + * + * 用法示例: + * node robot-ctl.js --host 203.0.113.10 --port 81 stop + * node robot-ctl.js --host 192.168.4.1 forward --ms 800 + * node robot-ctl.js jump # 原地跳一下 + * node robot-ctl.js joy 0 100 --ms 1200 # 摇杆:x=0 y=100 持续 1.2s + * node robot-ctl.js height 60 # 站高 60mm(32~85) + * node robot-ctl.js roll 10 # 横滚 10°(-30~30) + * node robot-ctl.js linear 100 --ms 500 # 线速度 100mm/s + * node robot-ctl.js seq "height 50; forward 800; stop" + * node robot-ctl.js probe # 只连接并打印收到的帧 + * node robot-ctl.js raw '{"dir":"jump"}' + * + * 通用参数:--host --port --timeout --rate --wet + * --wet:允许真正发出动作(不写它也能动,仅为显式确认用法保留) + */ + +const DEFAULT_STATE = { + roll: 0, + height: 38, + linear: 0, + angular: 0, + stable: 0, + mode: 'basic', + dir: 'stop', + joy_x: 0, + joy_y: 0, +}; + +function parseArgs(argv) { + const opts = { host: '192.168.4.1', port: 81, timeout: 6000, rate: 150, ms: null, wet: false }; + const tokens = []; + for (let i = 0; i < argv.length; i++) { + const a = argv[i]; + if (a === '--host') opts.host = argv[++i]; + else if (a === '--port') opts.port = Number(argv[++i]); + else if (a === '--timeout') opts.timeout = Number(argv[++i]); + else if (a === '--rate') opts.rate = Number(argv[++i]); + else if (a === '--ms') opts.ms = Number(argv[++i]); + else if (a === '--wet') opts.wet = true; + else tokens.push(a); + } + opts.cmd = tokens[0]; + opts.args = tokens.slice(1); + return opts; +} + +class Robot { + constructor(opts) { + this.opts = opts; + this.state = { ...DEFAULT_STATE }; + this.ws = null; + this.log = (...m) => console.log('[robot]', ...m); + } + + url() { + return `ws://${this.opts.host}:${this.opts.port}/`; + } + + connect() { + if (typeof WebSocket === 'undefined') { + return Promise.reject(new Error('当前 Node 无全局 WebSocket,需要 Node >= 22')); + } + return new Promise((resolve, reject) => { + const ws = new WebSocket(this.url()); + this.ws = ws; + const timer = setTimeout(() => { + try { ws.close(); } catch {} + reject(new Error(`连接超时 ${this.url()}`)); + }, this.opts.timeout); + + ws.onopen = () => { + clearTimeout(timer); + this.log('已连接', this.url()); + resolve(); + }; + ws.onerror = (e) => { + clearTimeout(timer); + reject(new Error(`连接失败 ${this.url()} (${(e && (e.message || e.type)) || 'error'})`)); + }; + ws.onclose = (e) => { + this.log('连接关闭', e?.code ?? '', e?.reason ?? ''); + }; + ws.onmessage = (m) => { + const s = typeof m.data === 'string' ? m.data : '[binary]'; + this.log('收到:', s.slice(0, 400)); + }; + }); + } + + send(extra = {}) { + if (!this.ws || this.ws.readyState !== 1) throw new Error('未连接'); + Object.assign(this.state, extra); + const frame = { + dir: this.state.dir, + mode: this.state.mode, + roll: this.state.roll, + height: this.state.height, + linear: this.state.linear, + angular: this.state.angular, + stable: this.state.stable, + joy_x: this.state.joy_x, + joy_y: this.state.joy_y, + }; + this.ws.send(JSON.stringify(frame)); + return frame; + } + + sleep(ms) { return new Promise((r) => setTimeout(r, ms)); } + + /** 推一段时间状态帧(用于持续动作/摇杆) */ + async stream(extra, ms, rate) { + const step = Math.max(30, rate || this.opts.rate); + const t0 = Date.now(); + this.send(extra); + while (Date.now() - t0 < ms) { + await this.sleep(Math.min(step, Math.max(0, ms - (Date.now() - t0)))); + if (Date.now() - t0 >= ms) break; + this.send(extra); + } + } + + stop() { + if (this.ws && this.ws.readyState === 1) { + this.send({ dir: 'stop', joy_x: 0, joy_y: 0 }); + } + } + + close() { + try { this.stop(); } catch {} + try { if (this.ws) this.ws.close(); } catch {} + } +} + +const DIRS = new Set(['forward', 'back', 'left', 'right', 'jump']); + +async function runOne(robot, cmd, args, opts) { + const num = (v, d) => (v === undefined || v === '' ? d : Number(v)); + switch (cmd) { + case 'stop': + robot.send({ dir: 'stop', joy_x: 0, joy_y: 0 }); + robot.log('停止'); + return; + case 'probe': { + const ms = Number(args[0] ?? 3000); + robot.send({ dir: 'stop' }); + robot.log(`已同步状态,监听 ${ms}ms 内机器人回传…`); + await robot.sleep(ms); + return; + } + case 'raw': { + const json = args.join(' '); + const obj = JSON.parse(json); + const frame = robot.send(obj); + robot.log('已发:', JSON.stringify(frame)); + return; + } + case 'height': + robot.send({ height: num(args[0], 38) }); + robot.log(`站高 = ${robot.state.height}mm`); + return; + case 'roll': + robot.send({ roll: num(args[0], 0) }); + robot.log(`横滚 = ${robot.state.roll}°`); + return; + case 'linear': + case 'angular': { + const v = num(args[0], 0); + const ms = opts.ms ?? num(args[1], 800); + await robot.stream({ [cmd]: v }, ms, opts.rate); + robot.send({ [cmd]: 0 }); + robot.log(`${cmd} = ${v} 持续 ${ms}ms 后归零`); + return; + } + case 'stable': + robot.send({ stable: num(args[0], 0) ? 1 : 0 }); + robot.log(`stable = ${robot.state.stable}`); + return; + case 'joy': { + const x = num(args[0], 0), y = num(args[1], 0); + const ms = opts.ms ?? num(args[2], 800); + await robot.stream({ joy_x: x, joy_y: y }, ms, opts.rate); + robot.send({ joy_x: 0, joy_y: 0, dir: 'stop' }); + robot.log(`摇杆 x=${x} y=${y} 持续 ${ms}ms 后回中`); + return; + } + case 'seq': { + const script = args.join(' '); + for (const part of script.split(';').map((s) => s.trim()).filter(Boolean)) { + const seg = part.split(/\s+/); + await runOne(robot, seg[0], seg.slice(1), opts); + await robot.sleep(120); + } + return; + } + default: { + if (DIRS.has(cmd)) { + const ms = opts.ms ?? (cmd === 'jump' ? 300 : 800); + await robot.stream({ dir: cmd }, ms, opts.rate); + robot.send({ dir: 'stop' }); + robot.log(`${cmd} 持续 ${ms}ms 后停止`); + return; + } + throw new Error(`未知命令: ${cmd}(可用: stop forward back left right jump joy height roll linear angular stable seq raw probe)`); + } + } +} + +(async () => { + const opts = parseArgs(process.argv.slice(2)); + if (!opts.cmd) { + console.log('用法: node robot-ctl.js [--host IP] [--port 81] [--ms N] <命令> [参数…]'); + console.log('命令: stop | forward | back | left | right | jump | joy [ms] |'); + console.log(' height | roll | linear [ms] | angular [ms] |'); + console.log(' stable <0|1> | seq "; " | raw | probe [ms]'); + process.exit(1); + } + + const robot = new Robot(opts); + let code = 0; + const cleanup = () => { try { robot.close(); } catch {} }; + process.on('SIGINT', () => { cleanup(); process.exit(130); }); + process.on('uncaughtException', (e) => { console.error('[robot] 错误:', e.message); cleanup(); process.exit(1); }); + + try { + await robot.connect(); + await runOne(robot, opts.cmd, opts.args, opts); + await robot.sleep(120); + } catch (e) { + console.error('[robot] 失败:', e.message); + code = 1; + } finally { + cleanup(); + await new Promise((r) => setTimeout(r, 150)); + process.exit(code); + } +})(); diff --git a/robot-ctl.py b/robot-ctl.py new file mode 100644 index 0000000..018a29b --- /dev/null +++ b/robot-ctl.py @@ -0,0 +1,350 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +WL-PRO 轮组机器人 遥控脚本(纯标准库,无第三方依赖,Windows/macOS/Linux 通用) + +协议与网页 index.html 完全一致:ws://:81/ ,发送 JSON 状态帧: + {"dir":..., "mode":"basic", "roll":0, "height":38, "linear":0, + "angular":0, "stable":0, "joy_x":0, "joy_y":0} + dir 取值:stop | forward | back | left | right | jump + +用法: + python robot-ctl.py --host 192.168.4.1 forward # 前进 0.8s 后停 + python robot-ctl.py jump # 跳一下 + python robot-ctl.py --host 192.168.4.1 joy 0 100 # 摇杆 前推到底 0.8s + python robot-ctl.py joy -60 0 --ms 500 # 左移 + python robot-ctl.py height 60 # 站高 60mm(32~85) + python robot-ctl.py roll 10 # 横滚 10°(-30~30) + python robot-ctl.py linear 150 --ms 600 # 线速度 150mm/s + python robot-ctl.py seq "height 60; forward 800; stop" + python robot-ctl.py stop # 急停 + python robot-ctl.py probe # 只连不动作,打印回传 + +参数:--host(默认 192.168.4.1) --port(默认 81) --ms(动作持续毫秒) + --rate(推帧间隔毫秒,默认 150,与网页一致) --timeout(连接超时秒) +""" + +import argparse +import base64 +import hashlib +import json +import os +import socket +import struct +import sys +import time + +DEFAULT_STATE = { + "roll": 0, "height": 38, "linear": 0, "angular": 0, + "stable": 0, "mode": "basic", "dir": "stop", "joy_x": 0, "joy_y": 0, +} +DIRS = {"forward", "back", "left", "right", "jump"} +GUID = "258EAFA5-E914-47DA-95CA-C5AB0DC85B11" + + +class WS: + """极简 WebSocket 客户端(仅文本帧 + ping/pong/close)""" + + def __init__(self, host, port, timeout=8.0, verbose=True): + self.host, self.port, self.timeout, self.verbose = host, port, timeout, verbose + self.sock = None + self.buf = b"" + + def log(self, *a): + if self.verbose: + print("[robot]", *a, flush=True) + + def connect(self): + s = socket.create_connection((self.host, self.port), timeout=self.timeout) + s.settimeout(self.timeout) + key = base64.b64encode(os.urandom(16)).decode() + req = ( + "GET / HTTP/1.1\r\n" + f"Host: {self.host}:{self.port}\r\n" + "Upgrade: websocket\r\nConnection: Upgrade\r\n" + f"Sec-WebSocket-Key: {key}\r\nSec-WebSocket-Version: 13\r\n" + f"Origin: http://{self.host}:{self.port}\r\n\r\n" + ) + s.sendall(req.encode()) + # 读响应头 + head = b"" + while b"\r\n\r\n" not in head: + chunk = s.recv(4096) + if not chunk: + raise RuntimeError("握手时连接被关闭") + head += chunk + if len(head) > 65536: + raise RuntimeError("响应头过大") + header, rest = head.split(b"\r\n\r\n", 1) + lines = header.decode("utf-8", "replace").split("\r\n") + status = lines[0] + if "101" not in status: + raise RuntimeError(f"设备未接受 WebSocket 升级:{status}") + hdrs = {} + for ln in lines[1:]: + if ":" in ln: + k, v = ln.split(":", 1) + hdrs[k.strip().lower()] = v.strip() + expect = base64.b64encode(hashlib.sha1((key + GUID).encode()).digest()).decode() + if hdrs.get("sec-websocket-accept") != expect: + raise RuntimeError("Sec-WebSocket-Accept 校验失败") + self.sock, self.buf = s, rest + self.log("已连接", f"ws://{self.host}:{self.port}/") + return self + + def send_text(self, text): + if self.sock is None: + raise RuntimeError("未连接") + payload = text.encode() + n = len(payload) + header = bytearray([0x81]) + if n < 126: + header.append(0x80 | n) + elif n < 65536: + header.append(0x80 | 126) + header += struct.pack("!H", n) + else: + header.append(0x80 | 127) + header += struct.pack("!Q", n) + mask = os.urandom(4) + header += mask + masked = bytes(b ^ mask[i % 4] for i, b in enumerate(payload)) + self.sock.sendall(bytes(header) + masked) + + def _recv_more(self): + chunk = self.sock.recv(4096) + if not chunk: + raise ConnectionError("连接已关闭") + self.buf += chunk + + def recv_frame(self, timeout=0.2): + """返回 (opcode, payload_bytes) 或 None(超时)""" + self.sock.settimeout(timeout) + try: + while True: + if len(self.buf) >= 2: + b0, b1 = self.buf[0], self.buf[1] + opcode = b0 & 0x0F + masked = b1 & 0x80 + ln = b1 & 0x7F + off = 2 + if ln == 126: + if len(self.buf) < 4: + self._recv_more(); continue + ln = struct.unpack("!H", self.buf[2:4])[0]; off = 4 + elif ln == 127: + if len(self.buf) < 10: + self._recv_more(); continue + ln = struct.unpack("!Q", self.buf[2:10])[0]; off = 10 + if masked: + if len(self.buf) < off + 4: + self._recv_more(); continue + mk = self.buf[off:off + 4]; off += 4 + else: + mk = None + if len(self.buf) < off + ln: + self._recv_more(); continue + payload = self.buf[off:off + ln] + self.buf = self.buf[off + ln:] + if mk: + payload = bytes(b ^ mk[i % 4] for i, b in enumerate(payload)) + if opcode == 9: # ping -> pong + self._send_control(0x0A, payload); continue + if opcode == 10: + continue + return opcode, payload + self._recv_more() + except socket.timeout: + return None + except (BlockingIOError, InterruptedError): + return None + + def _send_control(self, opcode, payload=b""): + mask = os.urandom(4) + masked = bytes(b ^ mask[i % 4] for i, b in enumerate(payload)) + self.sock.sendall(bytes([0x80 | opcode, 0x80 | len(payload)]) + mask + masked) + + def drain(self, seconds): + """收一段时间的回传(设备可能会回状态/心跳)""" + end = time.time() + seconds + got = 0 + while time.time() < end: + f = self.recv_frame(timeout=min(0.2, max(0.02, end - time.time()))) + if f is None: + continue + op, payload = f + if op == 8: + break + if op == 1: + got += 1 + try: + txt = payload.decode("utf-8", "replace") + except Exception: + txt = repr(payload) + self.log("收到:", txt[:400]) + return got + + def close(self): + if self.sock: + try: + self._send_control(0x08, struct.pack("!H", 1000)) + except Exception: + pass + try: + self.sock.close() + except Exception: + pass + self.sock = None + + +class Robot: + def __init__(self, ws, rate=150, base=None): + self.ws, self.rate = ws, rate + self.state = dict(DEFAULT_STATE) + if base: + # 基线:先用设备当前的真实姿态覆盖默认值,避免首个动作帧改掉姿态 + for k, v in base.items(): + if v is not None and k in self.state: + self.state[k] = v + + def send(self, **extra): + self.state.update(extra) + frame = { + "dir": self.state["dir"], "mode": self.state["mode"], + "roll": self.state["roll"], "height": self.state["height"], + "linear": self.state["linear"], "angular": self.state["angular"], + "stable": self.state["stable"], + "joy_x": self.state["joy_x"], "joy_y": self.state["joy_y"], + } + self.ws.send_text(json.dumps(frame, separators=(",", ":"))) + return frame + + def stream(self, extra, ms): + step = max(30, self.rate) + t0 = time.time() + self.send(**extra) + while (time.time() - t0) * 1000.0 < ms: + left = ms / 1000.0 - (time.time() - t0) + if left <= 0: + break + time.sleep(min(step / 1000.0, left)) + if (time.time() - t0) * 1000.0 >= ms: + break + self.send(**extra) + + +def run(robot, cmd, args, opts): + def num(v, d): + return d if v is None or v == "" else float(v) if "." in str(v) else int(v) + + if cmd == "stop": + robot.send(dir="stop", joy_x=0, joy_y=0) + robot.ws.log("已停止") + return + if cmd == "probe": + robot.send(dir="stop") + got = robot.ws.drain(float(args[0]) if args else 3.0) + robot.ws.log(f"监听结束,收到 {got} 帧") + return + if cmd == "raw": + obj = json.loads(" ".join(args)) + robot.send(**obj) + robot.ws.log("已发:", json.dumps(obj, ensure_ascii=False)) + return + if cmd == "height": + robot.send(height=num(args[0] if args else None, 38)) + robot.ws.log(f"站高 = {robot.state['height']}mm") + return + if cmd == "roll": + robot.send(roll=num(args[0] if args else None, 0)) + robot.ws.log(f"横滚 = {robot.state['roll']}°") + return + if cmd in ("linear", "angular"): + v = num(args[0] if args else None, 0) + ms = opts.ms or (num(args[1], 800) if len(args) > 1 else 800) + robot.stream({cmd: v}, ms) + robot.send(**{cmd: 0}) + robot.ws.log(f"{cmd} = {v} 持续 {ms}ms 后归零") + return + if cmd == "stable": + robot.send(stable=1 if num(args[0] if args else None, 0) else 0) + robot.ws.log(f"stable = {robot.state['stable']}") + return + if cmd == "joy": + x = num(args[0] if args else None, 0) + y = num(args[1], 0) if len(args) > 1 else 0 + ms = opts.ms or (num(args[2], 800) if len(args) > 2 else 800) + robot.stream({"joy_x": x, "joy_y": y}, ms) + robot.send(joy_x=0, joy_y=0, dir="stop") + robot.ws.log(f"摇杆 x={x} y={y} 持续 {ms}ms 后回中") + return + if cmd == "seq": + script = " ".join(args) + for part in [p.strip() for p in script.split(";") if p.strip()]: + seg = part.split() + run(robot, seg[0], seg[1:], opts) + time.sleep(0.12) + return + if cmd in DIRS: + ms = opts.ms or (300 if cmd == "jump" else 800) + robot.stream({"dir": cmd}, ms) + robot.send(dir="stop") + robot.ws.log(f"{cmd} 持续 {ms}ms 后停止") + return + raise SystemExit( + "未知命令: %s\n可用: stop forward back left right jump joy height roll " + "linear angular stable seq raw probe" % cmd + ) + + +def main(): + ap = argparse.ArgumentParser(add_help=True, description="WL-PRO 轮组机器人遥控") + ap.add_argument("--host", default="192.168.4.1") + ap.add_argument("--port", type=int, default=81) + ap.add_argument("--ms", type=int, default=0) + ap.add_argument("--rate", type=int, default=150) + ap.add_argument("--timeout", type=float, default=8.0) + # 设备当前真实姿态(设备不回传状态,必须由人告知,否则动作帧会改掉姿态) + ap.add_argument("--height", type=float, default=None, help="设备当前站高 mm") + ap.add_argument("--roll", type=float, default=None, help="设备当前横滚 deg") + ap.add_argument("--stable", type=int, default=None, help="设备当前 stable 0/1") + ap.add_argument("--linear", type=float, default=None, help="基线线速度,一般留空") + ap.add_argument("--angular", type=float, default=None, help="基线角速度,一般留空") + ap.add_argument("cmd", nargs="?", default=None) + ap.add_argument("args", nargs="*") + opts = ap.parse_args() + + if not opts.cmd: + ap.print_help() + sys.exit(1) + + ws = WS(opts.host, opts.port, timeout=opts.timeout) + code = 0 + try: + ws.connect() + base = { + "height": opts.height, "roll": opts.roll, "stable": opts.stable, + "linear": opts.linear, "angular": opts.angular, + } + robot = Robot(ws, rate=opts.rate, base=base) + if opts.height is None and opts.cmd in ("forward", "back", "left", "right", "jump", "joy", "stop", "seq"): + print("[robot] 注意: 未用 --height 指定设备当前站高,将按默认 38mm 发送。" + "若设备实际站高不是 38mm,本命令会把它改到 38mm。", file=sys.stderr) + run(robot, opts.cmd, opts.args, opts) + time.sleep(0.15) + except KeyboardInterrupt: + code = 130 + print("\n[robot] 已中断") + except Exception as e: + print(f"[robot] 失败: {e}", file=sys.stderr) + code = 1 + finally: + try: + ws.close() + except Exception: + pass + sys.exit(code) + + +if __name__ == "__main__": + main() diff --git a/robot-daemon.py b/robot-daemon.py new file mode 100755 index 0000000..f89e50e --- /dev/null +++ b/robot-daemon.py @@ -0,0 +1,191 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +常驻长连接驱动:保持单条 WebSocket 不断开,持续推零帧,按需注入动作。 + +设计要点(针对 Arduino WebSocketsServer + 短连接丢尾帧的问题): + * 全程只开一条连接,运行期间绝不关闭 → 避免"断开瞬间丢归零帧 → 速度残留" + * 空闲时每 120ms 推一帧全零(dir=stop, joy=0,0),与网页空闲行为一致 + * 动作靠写状态文件注入,带硬性到期时间,到期自动归零 + * stable 恒为 1(主动保持平衡),本进程拒绝任何关闭请求 + * height 恒为 38,roll 恒为 0 + +状态文件(默认 ./daemon-state.json): + {"joy_x":0, "joy_y":0, "dir":"stop", "until": } + 控制脚本只写这个文件;本进程读取并执行。 + +用法: + python3 robot-daemon.py [--host H] [--port P] [--state FILE] [--interval 0.12] + # 后台运行建议 nohup + setsid +""" + +import argparse +import json +import os +import signal +import sys +import time + +sys.path.insert(0, os.path.dirname(os.path.abspath(__file__))) +from importlib.machinery import SourceFileLoader + +_ctl = SourceFileLoader("rctl", os.path.join(os.path.dirname(os.path.abspath(__file__)), "robot-ctl.py")).load_module() + +HEIGHT = 38 +ROLL = 0 +STABLE = 1 + +ZERO = {"joy_x": 0, "joy_y": 0, "dir": "stop"} + + +def log(*a): + print(time.strftime("[%H:%M:%S]"), *a, flush=True) + + +def frame_of(joy_x, joy_y, direction): + return { + "dir": direction, + "mode": "basic", + "roll": ROLL, + "height": HEIGHT, + "linear": 0, + "angular": 0, + "stable": STABLE, + "joy_x": joy_x, + "joy_y": joy_y, + } + + +def read_state(path): + """读状态文件;不存在或损坏则返回零状态""" + try: + with open(path, "r", encoding="utf-8") as f: + d = json.load(f) + jx = int(d.get("joy_x", 0) or 0) + jy = int(d.get("joy_y", 0) or 0) + dr = str(d.get("dir", "stop") or "stop") + until = float(d.get("until", 0) or 0) + jx = max(-100, min(100, jx)) + jy = max(-100, min(100, jy)) + if dr not in ("stop", "jump"): + dr = "stop" + return {"joy_x": jx, "joy_y": jy, "dir": dr, "until": until} + except Exception: + return {"joy_x": 0, "joy_y": 0, "dir": "stop", "until": 0} + + +class Daemon: + def __init__(self, host, port, state_path, interval): + self.host, self.port = host, port + self.state_path = state_path + self.interval = interval + self.ws = None + self.running = True + self.last_key = None + self.sent = 0 + self.connects = 0 + + def connect(self): + while self.running: + try: + self.ws = _ctl.WS(self.host, self.port, timeout=8, verbose=False) + self.ws.connect() + self.connects += 1 + log(f"已连接 {self.host}:{self.port}(第 {self.connects} 次)") + return True + except Exception as e: + log(f"连接失败: {e},2 秒后重试") + time.sleep(2) + return False + + def run(self): + if not self.connect(): + return 1 + log(f"常驻开始 | 间隔 {int(self.interval*1000)}ms | stable={STABLE} height={HEIGHT}") + last_report = time.time() + try: + while self.running: + st = read_state(self.state_path) + now = time.time() + + # 到期 → 归零 + if st["until"] and now >= st["until"]: + st = {"joy_x": 0, "joy_y": 0, "dir": "stop", "until": 0} + + f = frame_of(st["joy_x"], st["joy_y"], st["dir"]) + payload = json.dumps(f, separators=(",", ":")) + + try: + self.ws.send_text(payload) + self.sent += 1 + except Exception as e: + log(f"发送失败: {e} → 重连") + try: + self.ws.close() + except Exception: + pass + self.ws = None + if not self.connect(): + break + continue + + # 状态变化时打日志 + key = (st["joy_x"], st["joy_y"], st["dir"]) + if key != self.last_key: + log(f"状态 → joy_x={st['joy_x']} joy_y={st['joy_y']} dir={st['dir']}" + + (f" 剩余 {st['until']-now:.1f}s" if st["until"] else "")) + self.last_key = key + + if time.time() - last_report >= 30: + log(f"心跳 | 已发 {self.sent} 帧 | 状态 {key}") + last_report = time.time() + + time.sleep(self.interval) + except KeyboardInterrupt: + pass + finally: + self.shutdown() + return 0 + + def shutdown(self): + """退出前归零:多推几帧再关,尽量不让设备残留速度""" + log("开始收尾:推零帧") + try: + zero = json.dumps(frame_of(0, 0, "stop"), separators=(",", ":")) + for _ in range(12): + try: + self.ws.send_text(zero) + except Exception: + break + time.sleep(0.12) + time.sleep(0.5) + except Exception as e: + log(f"收尾异常: {e}") + try: + if self.ws: + self.ws.close() + except Exception: + pass + log("已断开") + + +def main(): + ap = argparse.ArgumentParser() + ap.add_argument("--host", default="203.0.113.10") + ap.add_argument("--port", type=int, default=4782) + ap.add_argument("--state", default=os.path.join(os.path.dirname(os.path.abspath(__file__)), "daemon-state.json")) + ap.add_argument("--interval", type=float, default=0.12) + opts = ap.parse_args() + + d = Daemon(opts.host, opts.port, opts.state, opts.interval) + + def on_sig(signum, _frame): + d.running = False + + signal.signal(signal.SIGTERM, on_sig) + signal.signal(signal.SIGINT, on_sig) + sys.exit(d.run()) + + +if __name__ == "__main__": + main() diff --git a/tests/fake-robot-ws.js b/tests/fake-robot-ws.js new file mode 100644 index 0000000..96cc9bb --- /dev/null +++ b/tests/fake-robot-ws.js @@ -0,0 +1,15 @@ +// 用 ws 库实现的假机器人,仅用于本地验证 robot-ctl.js 的帧格式 +const { WebSocketServer } = require('/root/.local/share/pnpm/global/5/.pnpm/ws@8.21.3/node_modules/ws'); +const port = Number(process.argv[2] || 8905); +const wss = new WebSocketServer({ port, host: '127.0.0.1' }); +let n = 0; +wss.on('connection', (ws) => { + console.log('CONNECTED'); + ws.on('message', (buf) => { + n++; + const txt = buf.toString(); + console.log('FRAME#' + n, txt); + ws.send(JSON.stringify({ ok: true, n, echo: JSON.parse(txt) })); + }); +}); +console.log('fake robot ws on', port);