feat: Implement absolute scheduling for feedback loop and enhance tool frame configuration
This commit is contained in:
@@ -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 文档。按仓库规则不自动提交。
|
||||
Reference in New Issue
Block a user