From 910ab8aa384dbbf46c1ad9967650a4b707f3c223 Mon Sep 17 00:00:00 2001 From: qing <1776177662@qq.com> Date: Mon, 17 Aug 2026 21:59:25 +0800 Subject: [PATCH] =?UTF-8?q?feat(pybot):=20=E9=97=AD=E7=8E=AF=E5=BC=82?= =?UTF-8?q?=E6=AD=A5=E6=8E=A8=E7=90=86=E2=80=94=E2=80=94=E6=89=A7=E8=A1=8C?= =?UTF-8?q?=E4=B8=8A=E4=B8=80=E6=AD=A5=E5=8A=A8=E4=BD=9C=E4=B8=8E=20NPU=20?= =?UTF-8?q?=E6=8E=A8=E7=90=86=E5=B9=B6=E8=A1=8C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit run_loop 自动检测 predictor.predict_async 走异步路径：提交推理后并行 执行上一步动作（滞后一拍，10Hz 下 100ms），隐藏真实 I2C 执行耗时； 同步路径不变。pybot 97 测试全过（新增异步路径 + 滞后一拍执行验证）。 --- pybot/pybot/robot.py | 38 +++++++++++++++++++++++++-------- pybot/tests/test_loop.py | 46 ++++++++++++++++++++++++++++++++++++++++ 2 files changed, 75 insertions(+), 9 deletions(-) diff --git a/pybot/pybot/robot.py b/pybot/pybot/robot.py index 64d0e05..d53280c 100644 --- a/pybot/pybot/robot.py +++ b/pybot/pybot/robot.py @@ -287,21 +287,41 @@ class RobotStruct: 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) + # 异步模式：predictor 声明 predict_async 时启用——推理期间并行执行 + # 上一步动作（滞后一拍执行，动作与状态错位一拍，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: while max_steps is None or step < max_steps: t0 = time.monotonic() frames.extend(self._loop_recent_frames(camera_var, F)) # 1 采样 imu = self._read_imus(imu_vars) - if use_imu: # 2 推理 - act = predictor.predict(list(frames), np.array(history), - imu=self._pad_imu( - imu, predictor.imu_dim)) + if use_async: # 2 推理（异步） + if use_imu: + job = predictor.predict_async( + 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: - 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 执行 + if use_imu: + act = predictor.predict(list(frames), np.array(history), + 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 回写 if recorder is not None: recorder.record( diff --git a/pybot/tests/test_loop.py b/pybot/tests/test_loop.py index ff1775c..b44ead4 100644 --- a/pybot/tests/test_loop.py +++ b/pybot/tests/test_loop.py @@ -104,6 +104,14 @@ def _make_robot(mt_text, mock_i2c): 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): @@ -230,6 +238,44 @@ def test_loop_pacing(mock_i2c): 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: