feat(pybot): 进程内实时预测闭环（--loop）与帧源/节拍修复
- 新增 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 用例（冷启动/历史滑动/索引映射/帧源等）
This commit is contained in:
qing committed
1 parent
5af863a2c5
commit
fe12dd7b33
4 files changed
+477
-2
No files matched your search
+19
-1
@@ -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`
|
||||
|
||||
## 目录
|
||||
|
||||
|
||||
+42
-1
@@ -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())
|
||||
@@ -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)]
|
||||
|
||||
@@ -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
|
||||
Reference in new issue
Block a user