# RM75 Control Timing Stats 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:** 在 Grip 激活期间每约 5 秒向 `arm_debug.launch.py` 终端输出一次控制链路耗时统计。

**Architecture:** 在现有 `SingleArmVelocityTeleop` 控制回调内使用单调高精度时钟记录实际周期、控制路径总耗时、QP、关节发送和反馈年龄。节点保存一个固定长度样本窗口，满窗后用 NumPy 计算 mean/P95/P99/max，输出一条 ROS 日志并清空窗口。

**Tech Stack:** Python 3.10、ROS2 Humble `rclpy`、NumPy、pytest。

---

### Task 1: 控制周期统计

**Files:**
- Modify: `xr_rm_teleop/test/test_joint_control.py`
- Modify: `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`

- [x] **Step 1: 写失败测试**

在 `test_joint_control.py` 添加确定性两样本窗口测试：

```python
def test_timing_stats_logs_summary_and_clears_window() -> None:
    messages = []
    teleop = object.__new__(SingleArmVelocityTeleop)
    teleop._arm_name = "right_rm75"
    teleop._dt = 0.008
    teleop._timing_stats_window = 2
    teleop._timing_samples = {
        name: []
        for name in ("period", "total", "qp", "send", "feedback_age")
    }
    teleop.get_logger = lambda: SimpleNamespace(
        info=lambda message: messages.append(message)
    )

    teleop._record_timing_sample(7.0, 6.0, 1.0, 0.5, 3.0)
    assert messages == []

    teleop._record_timing_sample(9.0, 10.0, 2.0, 0.7, 4.0)

    assert len(messages) == 1
    assert "right_rm75 timing n=2 deadline=8.000 ms" in messages[0]
    assert "period[n=2 mean=8.000 p95=8.900 p99=8.980 max=9.000 ms overruns=1]" in messages[0]
    assert "total[n=2 mean=8.000 p95=9.800 p99=9.960 max=10.000 ms overruns=1]" in messages[0]
    assert "qp[n=2" in messages[0]
    assert "send[n=2" in messages[0]
    assert "feedback_age[n=2" in messages[0]
    assert all(not samples for samples in teleop._timing_samples.values())
```

- [x] **Step 2: 确认测试因功能缺失而失败**

在工作空间根目录运行：

```bash
source /opt/ros/humble/setup.bash
PYTHONPATH=src/xr_rm_teleop pytest -q \
  src/xr_rm_teleop/test/test_joint_control.py::test_timing_stats_logs_summary_and_clears_window
```

预期：失败并提示 `SingleArmVelocityTeleop` 没有 `_record_timing_sample`。

- [x] **Step 3: 实现最小统计逻辑**

在节点初始化中创建约 5 秒的窗口：

```python
self._timing_stats_window = max(1, int(round(5.0 / self._dt)))
self._timing_samples = {
    name: []
    for name in ("period", "total", "qp", "send", "feedback_age")
}
self._last_control_tick_started_ns: int | None = None
```

为每组样本计算统计摘要：

```python
def _timing_summary(
    self,
    name: str,
    samples: list[float],
    deadline_ms: float | None = None,
) -> str:
    values = np.asarray(samples)
    result = (
        f"{name}[n={len(samples)} mean={np.mean(values):.3f} "
        f"p95={np.percentile(values, 95):.3f} "
        f"p99={np.percentile(values, 99):.3f} "
        f"max={np.max(values):.3f} ms"
    )
    if deadline_ms is not None:
        result += f" overruns={np.count_nonzero(values > deadline_ms)}"
    return result + "]"
```

满窗后输出并清空：

```python
def _record_timing_sample(
    self,
    period_ms: float | None,
    total_ms: float,
    qp_ms: float,
    send_ms: float,
    feedback_age_ms: float,
) -> None:
    if period_ms is not None:
        self._timing_samples["period"].append(period_ms)
    self._timing_samples["total"].append(total_ms)
    self._timing_samples["qp"].append(qp_ms)
    self._timing_samples["send"].append(send_ms)
    self._timing_samples["feedback_age"].append(feedback_age_ms)
    if len(self._timing_samples["total"]) < self._timing_stats_window:
        return

    deadline_ms = self._dt * 1000.0
    summaries = [
        self._timing_summary("period", self._timing_samples["period"], deadline_ms),
        self._timing_summary("total", self._timing_samples["total"], deadline_ms),
        self._timing_summary("qp", self._timing_samples["qp"]),
        self._timing_summary("send", self._timing_samples["send"]),
        self._timing_summary("feedback_age", self._timing_samples["feedback_age"]),
    ]
    self.get_logger().info(
        f"{self._arm_name} timing n={len(self._timing_samples['total'])} "
        f"deadline={deadline_ms:.3f} ms | " + " | ".join(summaries)
    )
    for samples in self._timing_samples.values():
        samples.clear()
```

在 `_control_tick()` 中围绕 QP 和发送调用采样，并在关节命令处理完成后记录总耗时。早退周期不进入统计窗口，现有控制和安全逻辑保持不变。

- [x] **Step 4: 运行测试确认通过**

```bash
source /opt/ros/humble/setup.bash
PYTHONPATH=src/xr_rm_teleop pytest -q src/xr_rm_teleop/test/test_joint_control.py
```

预期：全部通过。

- [x] **Step 5: 完整验证**

```bash
source /opt/ros/humble/setup.bash
pytest -q src/xr_rm_teleop/test/test_orientation_control.py
colcon build --symlink-install
```

预期：姿态测试和工作空间构建全部通过。根据仓库规则，不自动提交 Git。