feat: Implement absolute scheduling for feedback loop and enhance tool frame configuration

This commit is contained in:
2026-07-29 09:44:51 +08:00
parent 2a12eea4d5
commit 687a0b401a
11 changed files with 876 additions and 21 deletions
@@ -0,0 +1,109 @@
# RM75 Feedback Absolute Scheduling Implementation Plan
> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking.
**Goal:** 将关节反馈线程从“读取后固定等待 8 ms”改为无历史周期补跑的绝对起始周期调度。
**Architecture:** `RealManAdapter._feedback_loop()` 保留现有读取、告警和停止结构,只把固定 `Event.wait(feedback_period)` 替换为下一截止时间计算。读取提前完成时等待剩余时间;读取超期时重置调度基准并立即进入下一周期。
**Tech Stack:** Python 3.10、threading、time.monotonic、pytest、ROS2 Humble、colcon
---
### Task 1: 反馈绝对周期调度
**Files:**
- Modify: `xr_rm_teleop/xr_rm_teleop/realman_adapter.py`
- Test: `xr_rm_teleop/test/test_initial_joint_pose.py`
- [ ] **Step 1: 写失败测试**
用确定性的 FakeTime 和 FakeStopEvent 运行 `_feedback_loop()` 三次读取:
- 第一次读取在 5 ms 完成,应只等待剩余 3 ms;
- 第二次在 18 ms 完成,超过 16 ms 截止时间,应不等待并把基准重置为
18 ms
- 第三次在 23 ms 完成,应等待到新基准的 26 ms,即再次等待 3 ms。
断言读取三次且 `wait()` 参数为 `[0.003, 0.003]`。该结果同时证明没有补跑
旧的 8 ms 和 16 ms 截止点。
- [ ] **Step 2: 确认测试失败**
```bash
source /opt/ros/humble/setup.bash
python3 -m pytest -q src/xr_rm_teleop/test/test_initial_joint_pose.py
```
预期:当前固定等待实现记录 `[0.008, 0.008, 0.008]`,测试失败。
- [ ] **Step 3: 写最小实现**
`_feedback_loop()` 改为:
```python
def _feedback_loop(self) -> None:
next_read_at = time.monotonic()
while not self._feedback_stop.is_set():
try:
self._read_joint_state_once()
self._feedback_fault_logged = False
except Exception as exc:
if not self._feedback_fault_logged:
self._log_warn(f"RealMan 关节反馈读取失败:{exc}")
self._feedback_fault_logged = True
next_read_at += self._feedback_period
remaining = next_read_at - time.monotonic()
if remaining <= 0.0:
next_read_at = time.monotonic()
continue
self._feedback_stop.wait(remaining)
```
- [ ] **Step 4: 确认局部测试通过**
重复 Step 2 命令。预期:全部 PASS。
### Task 2: 调度回归验证
**Files:**
- Verify: `xr_rm_teleop`
- Verify: ROS2 workspace
- [ ] **Step 1: 运行遥操作包测试**
```bash
source /opt/ros/humble/setup.bash
python3 -m pytest -q src/xr_rm_teleop/test
```
预期:全部 PASS。
- [ ] **Step 2: 运行姿态控制指定测试**
```bash
source /opt/ros/humble/setup.bash
python3 -m pytest -q src/xr_rm_teleop/test/test_orientation_control.py
```
预期:全部 PASS。
- [ ] **Step 3: 构建工作空间**
```bash
source /opt/ros/humble/setup.bash
colcon build --symlink-install
```
预期:四个包构建成功。
- [ ] **Step 4: 检查最终差异**
```bash
git diff --check
git status --short
```
预期:只包含已确认的统计、工具坐标系幂等修复、反馈绝对周期调度、对应测试
及 Superpowers 文档。按仓库规则不自动提交。