110 lines
3.3 KiB
Markdown
110 lines
3.3 KiB
Markdown
# 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 文档。按仓库规则不自动提交。
|