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

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 文档。按仓库规则不自动提交。
@@ -0,0 +1,167 @@
+# 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
+```
+
+预期：只包含设计、计划、反馈计时实现及相关测试。按仓库规则不自动提交。
@@ -0,0 +1,107 @@
+# RM75 Idempotent Tool Frame 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:** 让 RealMan 工具坐标系在首次启动时创建、后续启动时更新，并检查所有相关 SDK 返回值。
+
+**Architecture:** `fun_peripheral.py` 增加一个只负责工具坐标系的内部函数，先查询名称列表，再选择创建或更新，最后切换。现有 `peripheral_cfg()` 继续负责 IO 和夹爪初始化，只把原来的两次无检查调用替换为该函数。
+
+**Tech Stack:** Python 3.10、RealMan Python API2、pytest、ROS2 Humble、colcon
+
+---
+
+### Task 1: 工具坐标系幂等配置
+
+**Files:**
+- Modify: `xr_rm_teleop/xr_rm_teleop/fun_peripheral.py`
+- Test: `xr_rm_teleop/test/test_initial_joint_pose.py`
+
+- [ ] **Step 1: 写失败测试**
+
+导入新的 `_configure_tool_frame`，用 FakeArm 分别返回包含和不包含 `omnipic`
+的名称列表。断言不存在时调用 `rm_set_manual_tool_frame`，存在时调用
+`rm_update_tool_frame`，两条路径最后都调用 `rm_change_tool_frame`。
+
+再用参数化失败返回码验证查询、创建/更新和切换失败均抛出包含 SDK 操作名称
+的 `RuntimeError`。
+
+- [ ] **Step 2: 确认测试失败**
+
+```bash
+source /opt/ros/humble/setup.bash
+python3 -m pytest -q src/xr_rm_teleop/test/test_initial_joint_pose.py
+```
+
+预期：因 `_configure_tool_frame` 尚不存在而在测试收集阶段失败。
+
+- [ ] **Step 3: 写最小实现**
+
+在 `fun_peripheral.py` 增加：
+
+```python
+def _check_sdk_return(result: Any, operation: str) -> None:
+    if result != 0:
+        raise RuntimeError(f"{operation} failed with code {result}: {result!r}")
+
+
+def _configure_tool_frame(robot, tool_frame, tool_name: str) -> None:
+    frames = robot.rm_get_total_tool_frame()
+    if not isinstance(frames, dict):
+        raise RuntimeError(
+            f"rm_get_total_tool_frame returned invalid data: {frames!r}"
+        )
+    _check_sdk_return(
+        frames.get("return_code"),
+        "rm_get_total_tool_frame",
+    )
+    tool_names = frames.get("tool_names")
+    if not isinstance(tool_names, (list, tuple)):
+        raise RuntimeError(
+            f"rm_get_total_tool_frame returned invalid tool_names: {tool_names!r}"
+        )
+
+    if tool_name in tool_names:
+        operation = "rm_update_tool_frame"
+        result = robot.rm_update_tool_frame(frame=tool_frame)
+    else:
+        operation = "rm_set_manual_tool_frame"
+        result = robot.rm_set_manual_tool_frame(frame=tool_frame)
+    _check_sdk_return(result, operation)
+    _check_sdk_return(
+        robot.rm_change_tool_frame(tool_name),
+        "rm_change_tool_frame",
+    )
+```
+
+在 `peripheral_cfg()` 中用
+`_configure_tool_frame(robot, tool_frame, tool_name)` 替换原来的创建和切换
+调用。
+
+- [ ] **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
+colcon build --symlink-install
+```
+
+预期：四个包构建成功。
+