feat(pybot): 闭环异步推理——执行上一步动作与 NPU 推理并行
run_loop 自动检测 predictor.predict_async 走异步路径:提交推理后并行 执行上一步动作(滞后一拍,10Hz 下 100ms),隐藏真实 I2C 执行耗时; 同步路径不变。pybot 97 测试全过(新增异步路径 + 滞后一拍执行验证)。
This commit is contained in:
1 parent
8de7f6696c
commit
910ab8aa38
2 files changed
+75
-9
No files matched your search
+29
-9
@@ -287,21 +287,41 @@ class RobotStruct:
|
|||||||
dt = 1.0 / hz
|
dt = 1.0 / hz
|
||||||
step = 0
|
step = 0
|
||||||
overrun = 0
|
overrun = 0
|
||||||
log.info("closed loop start: hz=%.1f F=%d H=%d action_dim=%d",
|
# 异步模式:predictor 声明 predict_async 时启用——推理期间并行执行
|
||||||
hz, F, H, action_dim)
|
# 上一步动作(滞后一拍执行,动作与状态错位一拍,10Hz 下 = 100ms,
|
||||||
|
# 对微动作步态影响可忽略),省去执行耗时(实测 HEF 步耗时 ~16ms → ~12ms)
|
||||||
|
use_async = hasattr(predictor, "predict_async")
|
||||||
|
prev_act = None
|
||||||
|
log.info("closed loop start: hz=%.1f F=%d H=%d action_dim=%d%s",
|
||||||
|
hz, F, H, action_dim,
|
||||||
|
" async" if use_async else "")
|
||||||
try:
|
try:
|
||||||
while max_steps is None or step < max_steps:
|
while max_steps is None or step < max_steps:
|
||||||
t0 = time.monotonic()
|
t0 = time.monotonic()
|
||||||
frames.extend(self._loop_recent_frames(camera_var, F)) # 1 采样
|
frames.extend(self._loop_recent_frames(camera_var, F)) # 1 采样
|
||||||
imu = self._read_imus(imu_vars)
|
imu = self._read_imus(imu_vars)
|
||||||
if use_imu: # 2 推理
|
if use_async: # 2 推理(异步)
|
||||||
act = predictor.predict(list(frames), np.array(history),
|
if use_imu:
|
||||||
imu=self._pad_imu(
|
job = predictor.predict_async(
|
||||||
imu, predictor.imu_dim))
|
list(frames), np.array(history),
|
||||||
|
imu=self._pad_imu(imu, predictor.imu_dim))
|
||||||
|
else:
|
||||||
|
job = predictor.predict_async(
|
||||||
|
list(frames), np.array(history))
|
||||||
|
if prev_act is not None:
|
||||||
|
self._execute_action_vec(prev_act, exec_cols) # 3 执行(∥ NPU)
|
||||||
|
act = np.asarray(job.result(timeout=10.0),
|
||||||
|
dtype=np.float32).reshape(-1)
|
||||||
|
prev_act = act.copy()
|
||||||
else:
|
else:
|
||||||
act = predictor.predict(list(frames), np.array(history))
|
if use_imu:
|
||||||
act = np.asarray(act, dtype=np.float32).reshape(-1)
|
act = predictor.predict(list(frames), np.array(history),
|
||||||
self._execute_action_vec(act, exec_cols) # 3 执行
|
imu=self._pad_imu(
|
||||||
|
imu, predictor.imu_dim))
|
||||||
|
else:
|
||||||
|
act = predictor.predict(list(frames), np.array(history))
|
||||||
|
act = np.asarray(act, dtype=np.float32).reshape(-1)
|
||||||
|
self._execute_action_vec(act, exec_cols) # 3 执行
|
||||||
history.append(act.copy()) # 4 回写
|
history.append(act.copy()) # 4 回写
|
||||||
if recorder is not None:
|
if recorder is not None:
|
||||||
recorder.record(
|
recorder.record(
|
||||||
|
|||||||
@@ -104,6 +104,14 @@ def _make_robot(mt_text, mock_i2c):
|
|||||||
return RobotStruct(io.StringIO(mt_text), dry_run=True)
|
return RobotStruct(io.StringIO(mt_text), dry_run=True)
|
||||||
|
|
||||||
|
|
||||||
|
class AsyncMockPredictor(MockPredictor):
|
||||||
|
"""带 predict_async 的鸭子类型桩:异步返回与同步相同的可辨识动作"""
|
||||||
|
|
||||||
|
def predict_async(self, images, action_history):
|
||||||
|
act = self.predict(images, action_history)
|
||||||
|
return type("Job", (), {"result": lambda self, timeout=None: act})()
|
||||||
|
|
||||||
|
|
||||||
# ---------------- 冷启动 ----------------
|
# ---------------- 冷启动 ----------------
|
||||||
|
|
||||||
def test_cold_start_fills_windows(mock_i2c):
|
def test_cold_start_fills_windows(mock_i2c):
|
||||||
@@ -230,6 +238,44 @@ def test_loop_pacing(mock_i2c):
|
|||||||
assert elapsed >= 0.09 # 允许少量计时误差
|
assert elapsed >= 0.09 # 允许少量计时误差
|
||||||
|
|
||||||
|
|
||||||
|
# ---------------- 异步推理路径 ----------------
|
||||||
|
|
||||||
|
def test_loop_async_uses_predict_async(mock_i2c):
|
||||||
|
"""predictor 声明 predict_async 时闭环走异步路径(执行滞后一拍)"""
|
||||||
|
robot = _make_robot(LOOP_MT, mock_i2c)
|
||||||
|
pred = AsyncMockPredictor(frame_len=3, history_len=2, action_dim=4)
|
||||||
|
robot.active()
|
||||||
|
robot.run_loop(pred, hz=1000.0, max_steps=3)
|
||||||
|
robot.deactive()
|
||||||
|
# 3 步闭环 → 3 次异步推理(calls 由桩内 predict 记录)
|
||||||
|
assert len(pred.calls) == 3
|
||||||
|
# history 回写与同步一致:第 3 步历史末步 = 第 2 步预测动作 [2,3,4,5]
|
||||||
|
_, hist2 = pred.calls[2]
|
||||||
|
np.testing.assert_allclose(hist2[-1], [2, 3, 4, 5])
|
||||||
|
|
||||||
|
|
||||||
|
def test_loop_async_executes_prev_action(mock_i2c):
|
||||||
|
"""异步模式执行滞后一拍:第 k 步执行第 k-1 步预测的动作"""
|
||||||
|
robot = _make_robot(LOOP_MT, mock_i2c)
|
||||||
|
fd = robot.M["i2c-1"].play({})["fd"]
|
||||||
|
robot.active()
|
||||||
|
mock_i2c.journal.clear()
|
||||||
|
pred = AsyncMockPredictor(frame_len=3, history_len=2, action_dim=4)
|
||||||
|
robot.run_loop(pred, hz=1000.0, max_steps=3)
|
||||||
|
robot.deactive()
|
||||||
|
|
||||||
|
writes = [(e[2], e[3]) for e in mock_i2c.journal
|
||||||
|
if e[0] == "write_reg" and e[1] == fd]
|
||||||
|
# 预测序列:step0→[1,2,3,4], step1→[2,3,4,5], step2→[3,4,5,6]
|
||||||
|
# 执行序列(滞后一拍):step1 执行 [1,2,3,4],step2 执行 [2,3,4,5]
|
||||||
|
off = lambda deg: int((0.5 + 2 * deg / 180) * 4096 / 20) # noqa: E731
|
||||||
|
off_reg = lambda ch: 0x06 + 4 * ch + 2 # noqa: E731 —— LEDx_OFF_L
|
||||||
|
s1_vals = [v for r, v in writes if r == off_reg(0)] # s1 ch0
|
||||||
|
s2_vals = [v for r, v in writes if r == off_reg(1)] # s2 ch1
|
||||||
|
assert s1_vals == [off(1) & 0xFF, off(2) & 0xFF] # step0/step1 预测的 s1
|
||||||
|
assert s2_vals == [off(2) & 0xFF, off(3) & 0xFF] # step0/step1 预测的 s2
|
||||||
|
|
||||||
|
|
||||||
# ---------------- 帧源:守护线程队列 ----------------
|
# ---------------- 帧源:守护线程队列 ----------------
|
||||||
|
|
||||||
class _SeqGrabber:
|
class _SeqGrabber:
|
||||||
|
|||||||
Reference in new issue
Block a user