From fe12dd7b33bafd089bbdf69cef00d56df53b3ff6 Mon Sep 17 00:00:00 2001 From: qing <1776177662@qq.com> Date: Fri, 14 Aug 2026 07:52:07 +0800 Subject: [PATCH] =?UTF-8?q?feat(pybot):=20=E8=BF=9B=E7=A8=8B=E5=86=85?= =?UTF-8?q?=E5=AE=9E=E6=97=B6=E9=A2=84=E6=B5=8B=E9=97=AD=E7=8E=AF=EF=BC=88?= =?UTF-8?q?--loop=EF=BC=89=E4=B8=8E=E5=B8=A7=E6=BA=90/=E8=8A=82=E6=8B=8D?= =?UTF-8?q?=E4=BF=AE=E5=A4=8D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - 新增 run_loop 闭环：F 帧+H 步历史双滑窗，冷启动重复同帧+零动作， 每步 采样→推理→执行→回写，hz 节拍控制，可选同步录制 - --loop CKPT/--hz/--loop-steps 参数，按 .hef 后缀选 Hailo/PyTorch 后端， actpred 软依赖（仅 --loop 时 import），保持 pybot 零依赖 - P1-2 帧源修复：闭环改从 camera 守护线程队列取最近 F 帧（低延迟）， 替代 capture() 同步曝光阻塞，framerate/capacity 配置真正生效 - P1-1 overrun 告警：超时步记录实际耗时，汇总 overrun 比例并给降频建议 - 剔除 Speaker.delay 发声阻塞，speaker 补 delay=0 非阻塞 - 新增 tests/test_loop.py 11 用例（冷启动/历史滑动/索引映射/帧源等） --- pybot/README.md | 20 ++- pybot/pybot/main.py | 43 +++++- pybot/pybot/robot.py | 133 ++++++++++++++++++ pybot/tests/test_loop.py | 283 +++++++++++++++++++++++++++++++++++++++ 4 files changed, 477 insertions(+), 2 deletions(-) create mode 100644 pybot/tests/test_loop.py diff --git a/pybot/README.md b/pybot/README.md index b907232..64de207 100644 --- a/pybot/README.md +++ b/pybot/README.md @@ -28,9 +28,27 @@ cat scripts/std.mt | ./_run.sh # 与 bot4cj 相同的管道用法 ./_run.sh --dry-run scripts/std.mt # Mock 总线，动作序列到 stdout ./_run.sh --test roundtrip in.mt out.mt # 解析↔序列化往返自检 ./_run.sh --verbose scripts/std.mt # DEBUG 日志（stderr） +./_run.sh --record data/ep scripts/std.mt # 录制 actpred 格式 episode ``` -`-i` / `-o` 参数位保留（reserved），未来对接 [actpred](../actpred/) 动作预测。 +`-i` / `-o` 参数位保留（reserved），未来对接跨进程 FIFO。 + +### 实时预测闭环（`--loop`） + +`--loop CKPT` 进入进程内闭环：采样 → actpred 推理 → 执行 → 回写历史。 +脚本提供硬件配置与 `@Action` 表头（动作向量索引映射），不执行脚本动作行。 + +```bash +./_run.sh --loop ../actpred/checkpoints/best_inference.pt --hz 10 scripts/std.mt +./_run.sh --loop actpred_pretrain.hef --hz 20 --loop-steps 100 scripts/std.mt # NPU 后端 +``` + +- checkpoint 后缀 `.hef` → Hailo NPU 后端，否则 → PyTorch 后端 +- `hz` 须 ≥ 推理 + 执行耗时（ONNX ~11ms / Hailo ~12ms，hz ≤ 50 安全） +- `@Action` 表头列数须 ≤ `action_dim`（默认 18），闭环中跳过 `spk.delay` 阻塞 +- **帧源**：闭环从 camera 守护线程队列取最近 F 帧（无阻塞），`@Camera` 的 + `framerate` 建议 ≥ 2×`hz` 以保证帧新鲜度；`capacity` 应 ≥ F（帧窗长度）。 + 部署时若日志出现 `loop overrun` 告警，说明 `hz` 超过推理+执行能力，应降低 `hz` ## 目录 diff --git a/pybot/pybot/main.py b/pybot/pybot/main.py index b9a53ed..04ffed2 100644 --- a/pybot/pybot/main.py +++ b/pybot/pybot/main.py @@ -43,6 +43,14 @@ def _build_argparser(): "--dry-run 下用合成帧）") p.add_argument("--action-dim", type=int, default=18, help="动作向量维度（默认 18，与 actpred 一致）") + p.add_argument("--loop", metavar="CKPT", default=None, + help="实时预测闭环：加载 actpred checkpoint，" + "采样→推理→执行→回写（不执行脚本 @Action 行，" + "仅用其表头做动作向量索引映射）") + p.add_argument("--hz", type=float, default=10.0, + help="闭环控制频率 Hz（默认 10，须 ≥ 推理+执行耗时）") + p.add_argument("--loop-steps", type=int, default=None, metavar="N", + help="闭环步数上限（默认无限，调试用）") return p @@ -91,6 +99,12 @@ def main(argv=None): if args.dry_run: mocks = (i2c.use_mock(), spi.use_mock()) + predictor = None + if args.loop: + predictor = _load_predictor(args.loop) + if predictor is None: + return 2 + source = open(args.script, encoding="utf-8") if args.script else sys.stdin try: robot = RobotStruct(source, dry_run=args.dry_run) @@ -100,7 +114,11 @@ def main(argv=None): recorder = EpisodeRecorder(args.record, action_dim=args.action_dim) with robot: - robot.run(recorder=recorder) + if predictor is not None: + robot.run_loop(predictor, hz=args.hz, + max_steps=args.loop_steps, recorder=recorder) + else: + robot.run(recorder=recorder) finally: if args.script: source.close() @@ -110,5 +128,28 @@ def main(argv=None): return 0 +def _load_predictor(checkpoint): + """进程内构建 actpred 推理器；失败返回 None + + pybot 不硬依赖 actpred：仅 --loop 时才尝试 import，actpred 不在 + sys.path 时给出可操作提示。checkpoint 目录名决定后端（.hef → Hailo + NPU，否则 → PyTorch），推理类由 actpred 提供。 + """ + try: + if checkpoint.endswith(".hef"): + from actpred.hailo_infer import HailoPredictor as _P + else: + from actpred.predict import ActionPredictorInference as _P + except ImportError: + logging.error("--loop 需要 actpred 模块（把 blackbird 根目录加入 " + "PYTHONPATH，或在 actpred/ 同级运行）") + return None + try: + return _P(checkpoint) + except Exception: + logging.exception("加载 actpred checkpoint 失败: %s", checkpoint) + return None + + if __name__ == "__main__": sys.exit(main()) diff --git a/pybot/pybot/robot.py b/pybot/pybot/robot.py index a30b486..83a1f04 100644 --- a/pybot/pybot/robot.py +++ b/pybot/pybot/robot.py @@ -11,6 +11,8 @@ import atexit import logging +import time +from collections import deque from . import plugin from .hardvars import (AliasVar, CameraVar, I2CVar, MPU6050Var, ServoVar, @@ -196,6 +198,137 @@ class RobotStruct: path = recorder.flush() log.info("episode saved: %s", path) + # ---------------- 实时预测闭环 ---------------- + + def run_loop(self, predictor, hz=10.0, max_steps=None, recorder=None): + """实时预测闭环：无限循环 采样→推理→执行→回写历史 + + predictor: actpred 推理器（鸭子类型），需具备 + frame_len / history_len / action_dim 属性与 + predict(images, action_history) -> (action_dim,) 方法。 + pybot 不 import 具体推理类，保持零依赖承诺。 + hz: 控制频率（周期 = 1/hz 秒，须 ≥ 推理 + 执行耗时） + max_steps: 步数上限，None 表示无限循环（Ctrl-C 退出） + recorder: 可选 EpisodeRecorder，闭环中同步录制 (frame, action) + + 与 run() 的差异：run() 是脚本驱动一次性执行（行内 delay 分解为 + 10ms 微步插值）；run_loop() 由 predictor 逐步产出动作，节奏由 + hz 主循环掌控，每步直接下发不做行分解（delay 仅作回放兼容， + 闭环中跳过其发声阻塞）。 + + 帧源：闭环从 camera 守护线程的有界队列取最近 F 帧（低延迟， + 让 framerate/capacity 配置生效），而非 capture() 同步曝光 + （会阻塞 ~1/framerate 秒，把 hz 锁死在相机帧率之下）。dry_run + 或队列空时回退 _grab_frame 合成帧。 + """ + import numpy as np + camera_var = self._find_camera() + if camera_var is None and not self.dry_run: + raise ValueError("闭环需要 @Camera 块（或使用 --dry-run 生成合成帧）") + + action = self.find_mt("Action") + if action is None: + raise ValueError("no @Action matrix in script") + columns = self._validate_action(action) + + F, H = predictor.frame_len, predictor.history_len + action_dim = predictor.action_dim + if len(columns) > action_dim: + raise ValueError( + f"@Action 列数 {len(columns)} 超过 predictor.action_dim " + f"{action_dim}，无法按索引对齐") + exec_cols = self._loop_exec_columns(columns) + + frames = deque(maxlen=F) + history = deque(maxlen=H) + self._loop_cold_start(frames, history, camera_var, action_dim) + + dt = 1.0 / hz + step = 0 + overrun = 0 + log.info("closed loop start: hz=%.1f F=%d H=%d action_dim=%d", + hz, F, H, action_dim) + while max_steps is None or step < max_steps: + t0 = time.monotonic() + frames.extend(self._loop_recent_frames(camera_var, F)) # 1 采样 + act = predictor.predict(list(frames), np.array(history)) # 2 推理 + act = np.asarray(act, dtype=np.float32).reshape(-1) + self._execute_action_vec(act, exec_cols) # 3 执行 + history.append(act.copy()) # 4 回写 + if recorder is not None: + recorder.record(frames[-1], + self._build_action_vec(act, recorder.action_dim)) + step += 1 + remain = dt - (time.monotonic() - t0) # 节拍 + if remain > 0: + time.sleep(remain) + else: + overrun += 1 + log.warning("loop overrun: step %d took %.1fms > %.1fms " + "（推理+执行超过周期，实际频率已低于 %.1fHz）", + step, (dt - remain) * 1000, dt * 1000, hz) + if overrun: + log.warning("closed loop overrun %d/%d steps (%.0f%%)，" + "建议降低 --hz 或检查推理后端延迟", + overrun, step, 100.0 * overrun / max(step, 1)) + log.info("closed loop stop after %d steps", step) + + def _loop_recent_frames(self, camera_var, F): + """从 camera 守护线程队列取最近 F 帧（旧→新），无阻塞 + + 队列未填满 F 帧时重复最旧帧兜底；队列完全空（dry_run / 守护线程 + 刚启动）时回退 _grab_frame 合成帧。相比 capture() 同步曝光， + 从队列消费让 framerate/capacity 配置生效，闭环周期不被曝光阻塞。 + """ + if camera_var is None: + return [self._grab_frame(camera_var)] * F + snap = camera_var.camera.snapshot() # [(ts, frame), ...] 旧→新 + if not snap: + return [self._grab_frame(camera_var)] * F + recent = [f for _, f in snap[-F:]] + if len(recent) < F: # 队列未满：重复最旧帧 + recent = [recent[0]] * (F - len(recent)) + recent + return recent + + def _loop_exec_columns(self, columns): + """闭环执行列：剔除 Speaker.delay（发声阻塞会卡死实时节拍） + + 返回 [(action_idx, dev, param)]，action_idx 是动作向量索引（按 + @Action 表头顺序，与 actpred 的 action_dim 对齐）。 + """ + out = [] + for j, (dev, param) in enumerate(columns): + rp = self._resolve_param(param) + if isinstance(self.M.get(dev), SpeakerVar) and rp == "delay": + continue + out.append((j, dev, rp)) + return out + + def _loop_cold_start(self, frames, history, camera_var, action_dim): + """冷启动：F 帧滑窗从队列取最近帧（空则合成帧重复），H 步零动作 + + 帧源与主循环一致（_loop_recent_frames），避免冷启动与运行期帧源 + 不一致。history 用零向量填满是保守初值，避免随机姿态跳变损坏舵机。 + """ + import numpy as np + frames.extend(self._loop_recent_frames(camera_var, frames.maxlen)) + for _ in range(history.maxlen): + history.append(np.zeros(action_dim, dtype=np.float32)) + + def _execute_action_vec(self, act, exec_cols): + """把动作向量按 exec_cols 索引映射到设备并直接下发（不分解微步） + + SpeakerVar.play 强制要求 delay 键；闭环已剔除 delay 列（避免发声 + 阻塞卡死节拍），此处为其补 delay=0 —— 发声但不阻塞，节拍由 hz 掌控。 + """ + row_sets = {} + for j, dev, rp in exec_cols: + row_sets.setdefault(dev, {})[rp] = float(act[j]) + for dev, params in row_sets.items(): + if isinstance(self.M[dev], SpeakerVar) and "delay" not in params: + params["delay"] = 0 + self.M[dev].play(params) + def _resolve_row(self, action, row, n_cols): """解析一行的全部值（占位符继承）""" return [parse_mt(action, row, j) for j in range(n_cols)] diff --git a/pybot/tests/test_loop.py b/pybot/tests/test_loop.py new file mode 100644 index 0000000..c687a0f --- /dev/null +++ b/pybot/tests/test_loop.py @@ -0,0 +1,283 @@ +"""实时预测闭环测试：run_loop() + Mock predictor + Mock camera + +全部在 dry_run / mock I2C 下运行，不碰真实硬件与真实 actpred。 +predictor 用鸭子类型桩（具备 frame_len/history_len/action_dim/predict）， +验证闭环的冷启动、采样→推理→执行→回写、索引映射与节拍。 +""" + +import numpy as np +import pytest + +from pybot.robot import RobotStruct + +# @Camera + @Servo + @Speaker + @Action（含别名 A=angle / F=frequency） +# @Action 表头 4 列：s1.A s2.A spk.F spk.delay → 动作向量 action_dim=4 +LOOP_MT = """ +@Alias +name alias; + A angle; + F frequency; +@ + +@I2C + name path; +i2c-1 /dev/i2c-1; +@ + +@PCA9685 + name I2C address; +pca9685-0 i2c-1 0x40; +@ + +@Servo + name max min offset controller channel; + s1 90 - - pca9685-0 0; + s2 180 - - - 1; +@ + +@Speaker +name channels sample_rate volumn max_f min_f; + spk 2 48000 0.6 5000 -; +@ + +@Camera + name width height framerate capacity; + cam0 320 240 15 4; +@ + +@Action +s1.A s2.A spk.F spk.delay; + 45 0 10 20; +@ +""" + +# 无 @Camera 的脚本（验证闭环缺相机报错） +NO_CAMERA_MT = """ +@Alias +name alias; + A angle; +@ + +@Servo + name max min offset controller channel; + s1 90 - - - 0; +@ + +@Action +s1.A; + 45; +@ +""" + + +class MockPredictor: + """鸭子类型推理器桩：记录每次调用，返回固定/递增动作向量""" + + def __init__(self, frame_len=3, history_len=2, action_dim=4): + self.frame_len = frame_len + self.history_len = history_len + self.action_dim = action_dim + self.calls = [] # [(frames_list, history_array)] + self._step = 0 + + def predict(self, images, action_history): + self.calls.append((list(images), np.array(action_history))) + # 返回可辨识的动作：s1=step+1, s2=step+2, spk.F=step+3, delay=0 + act = np.arange(self._step + 1, self._step + 1 + self.action_dim, + dtype=np.float32) + self._step += 1 + return act + + +def _make_robot(mt_text, mock_i2c): + import io + return RobotStruct(io.StringIO(mt_text), dry_run=True) + + +# ---------------- 冷启动 ---------------- + +def test_cold_start_fills_windows(mock_i2c): + """冷启动：F 帧重复同帧 + H 步历史全零，首步推理前即填满""" + robot = _make_robot(LOOP_MT, mock_i2c) + pred = MockPredictor(frame_len=3, history_len=2, action_dim=4) + robot.active() + robot.run_loop(pred, hz=1000.0, max_steps=1) + robot.deactive() + + # 首次调用 predict：frames 应为 F=3 帧，history 应为 H=2 步全零 + frames, history = pred.calls[0] + assert len(frames) == 3 + assert history.shape == (2, 4) + assert np.allclose(history, 0.0) + + +def test_history_window_slides(mock_i2c): + """回写：每步把 predictor 返回的动作 append 进 history，窗口滑动""" + robot = _make_robot(LOOP_MT, mock_i2c) + pred = MockPredictor(frame_len=3, history_len=2, action_dim=4) + robot.active() + robot.run_loop(pred, hz=1000.0, max_steps=3) + robot.deactive() + + # 第 3 次调用时 history 应含前两步返回的动作（[1..4] 与 [2..5]） + _, history = pred.calls[2] + assert np.allclose(history[0], [1, 2, 3, 4]) + assert np.allclose(history[1], [2, 3, 4, 5]) + + +# ---------------- 动作向量 → 设备映射 ---------------- + +def test_execute_action_vec_maps_by_index(mock_i2c): + """18 维向量按 @Action 表头索引映射；舵机角度写入 PCA9685""" + robot = _make_robot(LOOP_MT, mock_i2c) + fd = robot.M["i2c-1"].play({})["fd"] + robot.active() + mock_i2c.journal.clear() + + pred = MockPredictor(frame_len=3, history_len=2, action_dim=4) + robot.run_loop(pred, hz=1000.0, max_steps=1) + robot.deactive() + + # 首步返回 [1,2,3,4]：s1=1°, s2=2°, spk.F=3 + writes = [(e[2], e[3]) for e in mock_i2c.journal + if e[0] == "write_reg" and e[1] == fd] + # s1=1° → 脉宽 0.5+2/180 → offval = int((0.5+2/180)*4096/20) + off1 = int((0.5 + 2 * 1 / 180) * 4096 / 20) + off2 = int((0.5 + 2 * 2 / 180) * 4096 / 20) + reg = lambda ch: 0x06 + 4 * ch # noqa: E731 + assert (reg(0) + 2, off1 & 0xFF) in writes + assert (reg(1) + 2, off2 & 0xFF) in writes + + +def test_speaker_delay_non_blocking(mock_i2c): + """闭环剔除 spk.delay 阻塞：传给 speaker 的 delay 恒为 0（发声不阻塞）""" + robot = _make_robot(LOOP_MT, mock_i2c) + robot.active() + spk = robot.M["spk"] + played = [] + orig_play = spk.play + spk.play = lambda p: played.append(dict(p)) or orig_play(p) + + pred = MockPredictor(frame_len=3, history_len=2, action_dim=4) + robot.run_loop(pred, hz=1000.0, max_steps=1) + robot.deactive() + + # 每次 play 含 frequency，且 delay 被强制为 0（非阻塞） + assert played, "spk 应被 play（含 frequency）" + for p in played: + assert "frequency" in p + assert p.get("delay") == 0 + + +# ---------------- 校验与边界 ---------------- + +def test_loop_requires_camera(mock_i2c): + """非 dry_run 且无 @Camera → 报错；dry_run 下合成帧可用""" + robot = RobotStruct(__import__("io").StringIO(NO_CAMERA_MT), dry_run=False) + pred = MockPredictor(frame_len=3, history_len=2, action_dim=1) + with pytest.raises(ValueError, match="@Camera"): + robot.run_loop(pred, max_steps=1) + robot.deactive() + + +def test_loop_requires_action_block(mock_i2c): + """缺 @Action 块 → 报错""" + mt = """ +@Camera + name width height framerate capacity; + cam0 320 240 15 4; +@ +""" + robot = _make_robot(mt, mock_i2c) + pred = MockPredictor(frame_len=3, history_len=2, action_dim=4) + with pytest.raises(ValueError, match="@Action"): + robot.run_loop(pred, max_steps=1) + robot.deactive() + + +def test_loop_action_dim_too_small(mock_i2c): + """@Action 列数超过 predictor.action_dim → 报错""" + robot = _make_robot(LOOP_MT, mock_i2c) + pred = MockPredictor(frame_len=3, history_len=2, action_dim=2) # < 4 列 + robot.active() + with pytest.raises(ValueError, match="action_dim"): + robot.run_loop(pred, max_steps=1) + robot.deactive() + + +# ---------------- 节拍 ---------------- + +def test_loop_pacing(mock_i2c): + """节拍：hz 控制周期下界（推理+执行 < 周期时 sleep 补足）""" + import time + robot = _make_robot(LOOP_MT, mock_i2c) + pred = MockPredictor(frame_len=3, history_len=2, action_dim=4) + robot.active() + t0 = time.monotonic() + robot.run_loop(pred, hz=50.0, max_steps=5) # 周期 20ms × 5 = ≥100ms + elapsed = time.monotonic() - t0 + robot.deactive() + assert elapsed >= 0.09 # 允许少量计时误差 + + +# ---------------- 帧源：守护线程队列 ---------------- + +class _SeqGrabber: + """递增序号帧源（dry_run 下闭环注入用）""" + def __init__(self): + self.n = 0 + + def __call__(self): + self.n += 1 + return self.n + + +def test_loop_consumes_daemon_queue(mock_i2c): + """闭环从守护线程队列取最近 F 帧，而非 capture() 同步曝光 + + dry_run 下守护线程不启动，手动 _grab_once 填队列模拟运行期队列。 + """ + robot = _make_robot(LOOP_MT, mock_i2c) + robot.active() + cam = robot.M["cam0"].camera + cam._grabber = _SeqGrabber() + for _ in range(5): # 填 5 帧：队列容量 4，保留最新 [2,3,4,5] + cam._grab_once() + pred = MockPredictor(frame_len=3, history_len=2, action_dim=4) + robot.run_loop(pred, hz=1000.0, max_steps=1) + robot.deactive() + + frames, _ = pred.calls[0] + # 帧来自序号帧源（int），闭环应取队列最近 F=3 帧 [3,4,5]（旧→新递增） + assert all(isinstance(f, int) for f in frames) + assert frames == [3, 4, 5], "应取队列中最近 F=3 帧（旧→新）" + + +def test_loop_queue_underfill_repeats_oldest(mock_i2c): + """队列未满 F 帧时：重复最旧帧兜底""" + robot = _make_robot(LOOP_MT, mock_i2c) + robot.active() + cam = robot.M["cam0"].camera + cam._grabber = _SeqGrabber() + # 不启动守护线程，手动抓 1 帧入队（队列只有 1 帧 < F=3） + cam._grab_once() + pred = MockPredictor(frame_len=3, history_len=2, action_dim=4) + robot.run_loop(pred, hz=1000.0, max_steps=1) + robot.deactive() + + frames, _ = pred.calls[0] + assert frames == [1, 1, 1], "队列只有 1 帧时应重复最旧帧填满 F" + + +def test_loop_empty_queue_falls_back_synthetic(mock_i2c): + """队列完全空（dry_run 无守护线程）时回退合成帧""" + robot = _make_robot(LOOP_MT, mock_i2c) + robot.active() # dry_run：守护线程不启动，队列恒空 + pred = MockPredictor(frame_len=3, history_len=2, action_dim=4) + robot.run_loop(pred, hz=1000.0, max_steps=1) + robot.deactive() + + frames, _ = pred.calls[0] + # dry_run 合成帧是 numpy 数组（HWC uint8），非 int + assert all(isinstance(f, np.ndarray) for f in frames) + assert frames[0].shape == (240, 320, 3) # LOOP_MT @Camera 320x240