168 lines
4.8 KiB
Markdown
168 lines
4.8 KiB
Markdown
# RM75 Feedback Thread Timing 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:** 在不改变反馈轮询和机械臂控制行为的前提下，统计 `rm_get_joint_degree()` 调用耗时与成功反馈更新间隔。
|
|
|
|
**Architecture:** `RealManAdapter` 在反馈读取边界测量时间，并把可选计时值随 `JointStateSnapshot` 放入现有缓存。`SingleArmVelocityTeleop` 复用现有 timing 窗口，只对新的反馈时间戳记录一次并输出汇总。
|
|
|
|
**Tech Stack:** Python 3.10、ROS2 Humble、pytest、NumPy、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: 写失败测试**
|
|
|
|
在 `test_joint_feedback_is_cached_in_radians` 中通过 `monkeypatch` 固定
|
|
`perf_counter_ns()` 和 `monotonic()`，连续读取两次，并验证：
|
|
|
|
```python
|
|
assert first.read_duration_ms == pytest.approx(2.0)
|
|
assert first.update_interval_ms is None
|
|
assert second.read_duration_ms == pytest.approx(3.0)
|
|
assert second.update_interval_ms == pytest.approx(11.0)
|
|
```
|
|
|
|
- [ ] **Step 2: 确认测试失败**
|
|
|
|
在 `/home/robot/WS_xr` 执行：
|
|
|
|
```bash
|
|
source /opt/ros/humble/setup.bash
|
|
python3 -m pytest -q src/xr_rm_teleop/test/test_initial_joint_pose.py::test_joint_feedback_is_cached_in_radians
|
|
```
|
|
|
|
预期：因 `JointStateSnapshot` 尚无计时字段而失败。
|
|
|
|
- [ ] **Step 3: 最小实现反馈计时**
|
|
|
|
给快照增加可选字段，保持现有两参数构造兼容：
|
|
|
|
```python
|
|
@dataclass(frozen=True)
|
|
class JointStateSnapshot:
|
|
positions: list[float]
|
|
received_at: float
|
|
read_duration_ms: float | None = None
|
|
update_interval_ms: float | None = None
|
|
```
|
|
|
|
在 `_read_joint_state_once()` 中只包围 SDK 调用测量 `read_duration_ms`；数据校验
|
|
成功后取得 `received_at`，并在缓存锁内根据上一快照计算
|
|
`update_interval_ms`。`get_latest_joint_state()` 同步复制两个字段。Mock 使用
|
|
字段默认值，不伪造计时。
|
|
|
|
- [ ] **Step 4: 确认局部测试通过**
|
|
|
|
重复 Step 2 命令。预期：PASS。
|
|
|
|
### Task 2: 将唯一反馈样本加入现有 timing 汇总
|
|
|
|
**Files:**
|
|
- Modify: `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
|
|
- Test: `xr_rm_teleop/test/test_joint_control.py`
|
|
|
|
- [ ] **Step 1: 写失败测试**
|
|
|
|
扩展 `test_timing_stats_logs_summary_and_clears_window`：在三个控制样本中传入
|
|
“快照 A、重复快照 A、快照 B”，并断言日志包含：
|
|
|
|
```python
|
|
assert "feedback_read[n=2" in messages[0]
|
|
assert "feedback_interval[n=1" in messages[0]
|
|
```
|
|
|
|
这样同时验证新反馈只计一次、重复缓存不重复计数、首次反馈无更新间隔。
|
|
|
|
- [ ] **Step 2: 确认测试失败**
|
|
|
|
在 `/home/robot/WS_xr` 执行：
|
|
|
|
```bash
|
|
source /opt/ros/humble/setup.bash
|
|
python3 -m pytest -q src/xr_rm_teleop/test/test_joint_control.py::test_timing_stats_logs_summary_and_clears_window
|
|
```
|
|
|
|
预期：因 timing 样本尚不支持新字段而失败。
|
|
|
|
- [ ] **Step 3: 最小实现唯一反馈统计**
|
|
|
|
在节点初始化时：
|
|
|
|
```python
|
|
self._timing_samples = {
|
|
name: []
|
|
for name in (
|
|
"period",
|
|
"total",
|
|
"qp",
|
|
"send",
|
|
"feedback_age",
|
|
"feedback_read",
|
|
"feedback_interval",
|
|
)
|
|
}
|
|
self._last_timing_feedback_received_at: float | None = None
|
|
```
|
|
|
|
让 `_record_timing_sample()` 接收当前 `JointStateSnapshot`。仅当
|
|
`received_at` 与 `_last_timing_feedback_received_at` 不同时，追加非 `None`
|
|
的读取耗时和更新间隔。汇总时仅输出非空的新数组，避免 mock 模式对空数组
|
|
求百分位数。控制循环把已有 `snapshot` 传入该函数。
|
|
|
|
- [ ] **Step 4: 确认相关测试通过**
|
|
|
|
```bash
|
|
source /opt/ros/humble/setup.bash
|
|
python3 -m pytest -q src/xr_rm_teleop/test/test_joint_control.py src/xr_rm_teleop/test/test_initial_joint_pose.py
|
|
```
|
|
|
|
预期：全部 PASS。
|
|
|
|
### Task 3: 回归验证
|
|
|
|
**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
|
|
```
|
|
|
|
预期：只包含设计、计划、反馈计时实现及相关测试。按仓库规则不自动提交。
|