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
Step 1: 写失败测试

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

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())

Step 2: 确认测试因功能缺失而失败

在工作空间根目录运行：

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。

Step 3: 实现最小统计逻辑

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

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

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

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 + "]"

满窗后输出并清空：

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 和发送调用采样，并在关节命令处理完成后记录总耗时。早退周期不进入统计窗口，现有控制和安全逻辑保持不变。

Step 4: 运行测试确认通过

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

预期：全部通过。

Step 5: 完整验证

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

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

5.2 KiB Raw Blame History

RM75 Control Timing Stats Implementation Plan

Task 1: 控制周期统计

5.2 KiB

Raw Blame History