155 lines
5.2 KiB
Markdown
155 lines
5.2 KiB
Markdown
# 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。
|