757 lines
22 KiB
Markdown
757 lines
22 KiB
Markdown
# ACT 双相机预览与采样质量判定修正实施计划
|
||
|
||
> **供代理执行者使用：** 必须使用 `superpowers:subagent-driven-development`（推荐）或 `superpowers:executing-plans`，逐项执行本计划。所有步骤使用复选框（`- [ ]`）跟踪。
|
||
|
||
**目标：** 保持 ACT 当前因果时间对齐，同时把真实相机丢帧与软件采样相位漂移分开判定，并在 ACT 数采启动时默认显示全局 D455 和右腕 D405 实时画面。
|
||
|
||
**架构：** 继续由 `act_episode_recorder` 独占两台 RealSense，并按每三个 `90 Hz` 控制周期选择不晚于控制时刻的最新图像。`CameraBuffer` 负责真实采集质量，HDF5 校验只记录 ACT 样本重复/跨帧率；同一节点内新增一个只读 OpenCV 预览线程，复用现有帧缓冲且不进入机器人控制链路。
|
||
|
||
**技术栈：** Ubuntu 22.04、ROS2 Humble、Python 3、rclpy、NumPy、h5py、pyrealsense2、OpenCV、pytest、HDF5。
|
||
|
||
---
|
||
|
||
## 文件结构
|
||
|
||
本次不新建 ROS 包或运行进程，文件职责保持如下：
|
||
|
||
- 修改 `xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py`：真实采集统计、ACT 采样相位指标、质量判定、双路预览及关闭顺序；
|
||
- 修改 `xr_rm_teleop/test/test_act_episode_recorder.py`：指标口径、误拒绝复现、帧号回退和预览隔离测试；
|
||
- 修改 `README.md`：说明 ACT 数采默认预览、显示内容和关闭行为。
|
||
|
||
不修改 `arm_debug.launch.py`、`launcher_ui.py`、ROS 消息、机器人控制节点和相机 YAML。工作区已有的 `xr_rm_teleop/xr_rm_teleop/fun_peripheral.py` 未提交改动属于用户，不加入本任务提交。
|
||
|
||
所有构建和测试命令从工作空间根目录执行：
|
||
|
||
```bash
|
||
cd /home/robot/WS_xr
|
||
source /opt/ros/humble/setup.bash
|
||
```
|
||
|
||
自动化测试不得启动真机 launch、移动机械臂或操作夹爪。
|
||
|
||
### 任务 1：区分真实丢帧与 ACT 采样相位漂移
|
||
|
||
**文件：**
|
||
|
||
- 修改：`xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py:408-579`
|
||
- 修改：`xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py:595-657`
|
||
- 修改：`xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py:1472-1511`
|
||
- 测试：`xr_rm_teleop/test/test_act_episode_recorder.py:273-296`
|
||
- 测试：`xr_rm_teleop/test/test_act_episode_recorder.py:491-607`
|
||
|
||
- [ ] **步骤 1：写异步采样不拒绝和指标测试**
|
||
|
||
在测试导入中加入 `sample_frame_metrics`，并增加：
|
||
|
||
```python
|
||
def test_sample_frame_metrics_separate_repeats_skips_and_regressions():
|
||
repeat, skip, regression = sample_frame_metrics(
|
||
np.asarray((986, 986, 988), dtype=np.uint64)
|
||
)
|
||
|
||
assert repeat == pytest.approx(0.5)
|
||
assert skip == pytest.approx(0.5)
|
||
assert regression == 0
|
||
|
||
|
||
@requires_h5py
|
||
def test_validate_episode_accepts_async_camera_phase_drift(tmp_path):
|
||
path = _valid_episode(tmp_path)
|
||
with h5py.File(path, "r+") as root:
|
||
root["debug/cameras/cam_high_frame_number"][:] = (986, 986, 988)
|
||
|
||
report = validate_episode(path, _quality_limits())
|
||
|
||
assert report.accepted
|
||
assert report.metrics["cam_high_sample_repeat_ratio"] == pytest.approx(0.5)
|
||
assert report.metrics["cam_high_sample_skip_ratio"] == pytest.approx(0.5)
|
||
```
|
||
|
||
在 `_valid_episode()` 写入新增的真实采集属性：
|
||
|
||
```python
|
||
root.attrs["camera_high_frame_number_regression_count"] = 0
|
||
root.attrs["camera_right_wrist_frame_number_regression_count"] = 0
|
||
```
|
||
|
||
- [ ] **步骤 2：写真实帧号回退测试**
|
||
|
||
扩展现有 `CameraBuffer` 测试：
|
||
|
||
```python
|
||
def test_camera_buffer_counts_frame_number_regressions():
|
||
buffer = CameraBuffer(maxlen=4)
|
||
for frame_number in (100, 101, 1, 2):
|
||
buffer.push(
|
||
CameraFrame(
|
||
_image(frame_number),
|
||
frame_number,
|
||
float(frame_number),
|
||
time.monotonic_ns(),
|
||
)
|
||
)
|
||
|
||
stats = buffer.stats()
|
||
|
||
assert stats.frame_number_regression_count == 1
|
||
assert stats.dropped_frames == 0
|
||
```
|
||
|
||
测试文件顶部增加标准库导入：
|
||
|
||
```python
|
||
import time
|
||
```
|
||
|
||
并在稳定拒绝原因参数中增加真实采集帧号回退：
|
||
|
||
```python
|
||
elif mutation == "camera_frame_regression":
|
||
root.attrs["camera_high_frame_number_regression_count"] = 1
|
||
```
|
||
|
||
对应期望：
|
||
|
||
```python
|
||
("camera_frame_regression", "camera_frame_number_regression"),
|
||
```
|
||
|
||
- [ ] **步骤 3：运行新增测试并确认失败**
|
||
|
||
运行：
|
||
|
||
```bash
|
||
cd /home/robot/WS_xr
|
||
source /opt/ros/humble/setup.bash
|
||
pytest src/xr_rm_teleop/test/test_act_episode_recorder.py \
|
||
-k "sample_frame_metrics or async_camera_phase_drift or frame_number_regressions or camera_frame_regression" -v
|
||
```
|
||
|
||
预期：测试因 `sample_frame_metrics` 尚不存在、`CameraStats` 没有回退计数，或旧校验仍以 `camera_sample_drop_ratio` 拒绝而失败。
|
||
|
||
- [ ] **步骤 4：实现最小采样相位指标**
|
||
|
||
在质量校验辅助函数附近增加纯函数：
|
||
|
||
```python
|
||
def sample_frame_metrics(
|
||
frame_numbers: np.ndarray,
|
||
) -> tuple[float, float, int]:
|
||
diffs = np.diff(np.asarray(frame_numbers, dtype=np.int64))
|
||
denominator = max(1, len(diffs))
|
||
return (
|
||
float(np.count_nonzero(diffs == 0) / denominator),
|
||
float(np.count_nonzero(diffs > 1) / denominator),
|
||
int(np.count_nonzero(diffs < 0)),
|
||
)
|
||
```
|
||
|
||
把 `validate_episode()` 中原有“所有 `diff != 1` 都拒绝”的循环替换为：
|
||
|
||
```python
|
||
for camera in ("cam_high", "cam_wrist"):
|
||
frame_numbers = datasets[
|
||
f"debug/cameras/{camera}_frame_number"
|
||
][:]
|
||
repeat_ratio, skip_ratio, regression_count = sample_frame_metrics(
|
||
frame_numbers
|
||
)
|
||
metrics[f"{camera}_sample_repeat_ratio"] = repeat_ratio
|
||
metrics[f"{camera}_sample_skip_ratio"] = skip_ratio
|
||
if regression_count:
|
||
return _quality_failure("camera_frame_number_regression", metrics)
|
||
```
|
||
|
||
这样 `986 → 986 → 988` 只产生统计，不再触发 `camera_sample_drop_ratio`。
|
||
|
||
- [ ] **步骤 5：在采集层统计帧号回退**
|
||
|
||
扩展 `CameraStats`：
|
||
|
||
```python
|
||
@dataclass(frozen=True)
|
||
class CameraStats:
|
||
frame_count: int
|
||
dropped_frames: int
|
||
frame_number_regression_count: int
|
||
first_host_monotonic_ns: int | None
|
||
last_host_monotonic_ns: int | None
|
||
```
|
||
|
||
在 `CameraBuffer.__init__()` 增加：
|
||
|
||
```python
|
||
self._frame_number_regression_count = 0
|
||
```
|
||
|
||
在 `CameraBuffer.push()` 更新帧号前使用互斥分支：
|
||
|
||
```python
|
||
if self._last_frame_number is not None:
|
||
if frame.frame_number <= self._last_frame_number:
|
||
self._frame_number_regression_count += 1
|
||
elif frame.frame_number > self._last_frame_number + 1:
|
||
self._dropped_frames += (
|
||
frame.frame_number - self._last_frame_number - 1
|
||
)
|
||
```
|
||
|
||
在 `stats()` 返回：
|
||
|
||
```python
|
||
frame_number_regression_count=self._frame_number_regression_count,
|
||
```
|
||
|
||
- [ ] **步骤 6：把真实采集回退纳入 episode 属性和拒绝条件**
|
||
|
||
把 `_interval_camera_metrics()` 的返回值扩展为 FPS、真实丢帧率和本 episode 新增的回退数：
|
||
|
||
```python
|
||
@staticmethod
|
||
def _interval_camera_metrics(
|
||
baseline: CameraStats,
|
||
current: CameraStats,
|
||
) -> tuple[float, float, int]:
|
||
frames = max(0, current.frame_count - baseline.frame_count)
|
||
dropped = max(0, current.dropped_frames - baseline.dropped_frames)
|
||
regressions = max(
|
||
0,
|
||
current.frame_number_regression_count
|
||
- baseline.frame_number_regression_count,
|
||
)
|
||
if (
|
||
baseline.last_host_monotonic_ns is None
|
||
or current.last_host_monotonic_ns is None
|
||
):
|
||
return 0.0, 1.0, regressions
|
||
elapsed_ns = (
|
||
current.last_host_monotonic_ns
|
||
- baseline.last_host_monotonic_ns
|
||
)
|
||
fps = frames * 1e9 / elapsed_ns if elapsed_ns > 0 else 0.0
|
||
expected = frames + dropped
|
||
drop_ratio = dropped / expected if expected else 1.0
|
||
return fps, drop_ratio, regressions
|
||
```
|
||
|
||
`_write_camera_metrics()` 写入：
|
||
|
||
```python
|
||
"camera_high_frame_number_regression_count": high[2],
|
||
"camera_right_wrist_frame_number_regression_count": wrist[2],
|
||
```
|
||
|
||
`validate_episode()` 在读取 FPS 和真实丢帧率后要求两个回退属性存在且为零：
|
||
|
||
```python
|
||
for name in (
|
||
"camera_high_frame_number_regression_count",
|
||
"camera_right_wrist_frame_number_regression_count",
|
||
):
|
||
if name not in root.attrs:
|
||
return _quality_failure("camera_stats_missing", metrics)
|
||
value = int(root.attrs[name])
|
||
metrics[name] = value
|
||
if value:
|
||
return _quality_failure("camera_frame_number_regression", metrics)
|
||
```
|
||
|
||
- [ ] **步骤 7：确保采样指标写入保存和拒绝文件**
|
||
|
||
增加读取 HDF5 根节点的辅助函数：
|
||
|
||
```python
|
||
def episode_sample_frame_metrics(root: Any) -> dict[str, int | float]:
|
||
metrics: dict[str, int | float] = {}
|
||
for camera in ("cam_high", "cam_wrist"):
|
||
repeat, skip, regression = sample_frame_metrics(
|
||
root[f"debug/cameras/{camera}_frame_number"][:]
|
||
)
|
||
metrics[f"{camera}_sample_repeat_ratio"] = repeat
|
||
metrics[f"{camera}_sample_skip_ratio"] = skip
|
||
metrics[f"{camera}_sample_frame_number_regression_count"] = regression
|
||
return metrics
|
||
```
|
||
|
||
`validate_episode()` 复用该函数更新 `report.metrics`。在 `_reject_closed_partial()` 已打开 HDF5 后也执行：
|
||
|
||
```python
|
||
for name, value in episode_sample_frame_metrics(root).items():
|
||
root.attrs[name] = value
|
||
```
|
||
|
||
保存路径继续由 `_complete_save()` 把 `report.metrics` 写入属性。保存日志追加紧凑摘要：
|
||
|
||
```python
|
||
self.get_logger().info(
|
||
"ACT相机采样相位："
|
||
f"high重复={report.metrics['cam_high_sample_repeat_ratio']:.2%}, "
|
||
f"high跨帧={report.metrics['cam_high_sample_skip_ratio']:.2%}, "
|
||
f"wrist重复={report.metrics['cam_wrist_sample_repeat_ratio']:.2%}, "
|
||
f"wrist跨帧={report.metrics['cam_wrist_sample_skip_ratio']:.2%}"
|
||
)
|
||
```
|
||
|
||
- [ ] **步骤 8：运行相关测试并确认通过**
|
||
|
||
运行：
|
||
|
||
```bash
|
||
cd /home/robot/WS_xr
|
||
source /opt/ros/humble/setup.bash
|
||
pytest src/xr_rm_teleop/test/test_act_episode_recorder.py \
|
||
-k "camera or validate_episode" -v
|
||
```
|
||
|
||
预期：所有选中测试通过；真实丢帧率仍使用 `camera_drop_ratio` 拒绝，异步重复/跨帧不拒绝。
|
||
|
||
- [ ] **步骤 9：提交任务 1**
|
||
|
||
```bash
|
||
git add src/xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py \
|
||
src/xr_rm_teleop/test/test_act_episode_recorder.py
|
||
git commit -m "fix: 修正ACT相机采样质量判定"
|
||
```
|
||
|
||
### 任务 2：在录制器中增加非阻塞双路预览
|
||
|
||
**文件：**
|
||
|
||
- 修改：`xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py:595-657`
|
||
- 修改：`xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py:1006-1152`
|
||
- 修改：`xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py:1668-1671`
|
||
- 测试：`xr_rm_teleop/test/test_act_episode_recorder.py`
|
||
|
||
- [ ] **步骤 1：写两秒滚动 FPS 测试**
|
||
|
||
扩展 `CameraBuffer` 测试，使用明确的单调时间：
|
||
|
||
```python
|
||
def test_camera_buffer_reports_two_second_rolling_fps():
|
||
buffer = CameraBuffer(maxlen=4)
|
||
start_ns = 10_000_000_000
|
||
for index in range(61):
|
||
buffer.push(
|
||
CameraFrame(
|
||
_image(index),
|
||
index,
|
||
float(index),
|
||
start_ns + index * 33_333_333,
|
||
)
|
||
)
|
||
|
||
assert buffer.stats().rolling_fps == pytest.approx(30.0, rel=0.02)
|
||
```
|
||
|
||
- [ ] **步骤 2：写预览显示异常隔离测试**
|
||
|
||
构造不经过 ROS 初始化的录制器和抛错的 `cv2` 替身：
|
||
|
||
```python
|
||
def test_preview_failure_does_not_change_recording_state():
|
||
recorder = object.__new__(ActEpisodeRecorder)
|
||
recorder._session = _recording_session()
|
||
recorder._preview_stop = threading.Event()
|
||
recorder._preview_thread = None
|
||
recorder.get_logger = lambda: _Logger()
|
||
|
||
class FailingCv2:
|
||
WINDOW_NORMAL = 0
|
||
|
||
@staticmethod
|
||
def namedWindow(*_args):
|
||
raise RuntimeError("no display")
|
||
|
||
recorder._preview_loop(FailingCv2())
|
||
|
||
assert recorder.state is RecordingState.RECORDING
|
||
```
|
||
|
||
`_Logger` 增加 `warns` 收集和 `warn()`：
|
||
|
||
```python
|
||
self.warns = []
|
||
|
||
def warn(self, message):
|
||
self.warns.append(message)
|
||
```
|
||
|
||
- [ ] **步骤 3：写预览关闭顺序测试**
|
||
|
||
验证关闭节点时先停预览，再停相机：
|
||
|
||
```python
|
||
def test_close_stops_preview_before_cameras_and_releases_lock():
|
||
events = []
|
||
recorder = object.__new__(ActEpisodeRecorder)
|
||
recorder._stop_preview = lambda: events.append("preview")
|
||
recorder._high_camera = SimpleNamespace(
|
||
stop=lambda: events.append("high")
|
||
)
|
||
recorder._wrist_camera = SimpleNamespace(
|
||
stop=lambda: events.append("wrist")
|
||
)
|
||
recorder._directory_lock = SimpleNamespace(
|
||
release=lambda: events.append("lock")
|
||
)
|
||
|
||
recorder.close()
|
||
|
||
assert events == ["preview", "high", "wrist", "lock"]
|
||
```
|
||
|
||
- [ ] **步骤 4：运行新增预览测试并确认失败**
|
||
|
||
运行：
|
||
|
||
```bash
|
||
cd /home/robot/WS_xr
|
||
source /opt/ros/humble/setup.bash
|
||
pytest src/xr_rm_teleop/test/test_act_episode_recorder.py \
|
||
-k "rolling_fps or preview or close_stops_preview" -v
|
||
```
|
||
|
||
预期：测试因 `rolling_fps`、`_preview_loop()` 和 `_stop_preview()` 尚不存在而失败。
|
||
|
||
- [ ] **步骤 5：实现两秒滚动 FPS**
|
||
|
||
在 `CameraBuffer.__init__()` 增加：
|
||
|
||
```python
|
||
self._recent_host_monotonic_ns: deque[int] = deque()
|
||
```
|
||
|
||
每次 `push()` 时裁剪两秒窗口：
|
||
|
||
```python
|
||
self._recent_host_monotonic_ns.append(frame.host_monotonic_ns)
|
||
cutoff_ns = frame.host_monotonic_ns - 2_000_000_000
|
||
while (
|
||
self._recent_host_monotonic_ns
|
||
and self._recent_host_monotonic_ns[0] < cutoff_ns
|
||
):
|
||
self._recent_host_monotonic_ns.popleft()
|
||
```
|
||
|
||
`CameraStats` 增加字段和属性：
|
||
|
||
```python
|
||
recent_host_monotonic_ns: tuple[int, ...]
|
||
|
||
@property
|
||
def rolling_fps(self) -> float:
|
||
if len(self.recent_host_monotonic_ns) < 2:
|
||
return 0.0
|
||
elapsed_ns = (
|
||
self.recent_host_monotonic_ns[-1]
|
||
- self.recent_host_monotonic_ns[0]
|
||
)
|
||
return (
|
||
(len(self.recent_host_monotonic_ns) - 1) * 1e9 / elapsed_ns
|
||
if elapsed_ns > 0
|
||
else 0.0
|
||
)
|
||
```
|
||
|
||
`stats()` 使用：
|
||
|
||
```python
|
||
recent_host_monotonic_ns=tuple(self._recent_host_monotonic_ns),
|
||
```
|
||
|
||
- [ ] **步骤 6：实现最小预览渲染方法**
|
||
|
||
在 `ActEpisodeRecorder` 增加固定窗口名：
|
||
|
||
```python
|
||
PREVIEW_WINDOW = "ACT - D455 Global / D405 Right Wrist"
|
||
```
|
||
|
||
增加生成单个画面块的方法。相机帧是 RGB，因此显示前转换成 BGR：
|
||
|
||
```python
|
||
@staticmethod
|
||
def _preview_tile(role: str, frame, stats, cv2):
|
||
if frame is None:
|
||
tile = np.zeros((480, 640, 3), dtype=np.uint8)
|
||
frame_number = "-"
|
||
else:
|
||
tile = cv2.cvtColor(frame.image, cv2.COLOR_RGB2BGR)
|
||
frame_number = str(frame.frame_number)
|
||
tile = tile.copy()
|
||
cv2.rectangle(tile, (0, 0), (640, 74), (0, 0, 0), -1)
|
||
lines = (
|
||
f"{role} FPS {stats.rolling_fps:.1f} Frame {frame_number}",
|
||
f"Received {stats.frame_count} Dropped "
|
||
f"{stats.dropped_frames} ({stats.drop_ratio:.2%})",
|
||
)
|
||
for index, line in enumerate(lines):
|
||
cv2.putText(
|
||
tile,
|
||
line,
|
||
(10, 28 + index * 30),
|
||
cv2.FONT_HERSHEY_SIMPLEX,
|
||
0.65,
|
||
(255, 255, 255),
|
||
1,
|
||
cv2.LINE_AA,
|
||
)
|
||
return tile
|
||
```
|
||
|
||
增加 `_compose_preview()`：读取两个缓冲最新帧，水平拼接，并在底部显示状态：
|
||
|
||
```python
|
||
def _compose_preview(self, cv2):
|
||
high_frames = self._high_camera.buffer.snapshot()
|
||
wrist_frames = self._wrist_camera.buffer.snapshot()
|
||
high = high_frames[-1] if high_frames else None
|
||
wrist = wrist_frames[-1] if wrist_frames else None
|
||
high_stats = self._high_camera.buffer.stats()
|
||
wrist_stats = self._wrist_camera.buffer.stats()
|
||
image = np.hstack(
|
||
(
|
||
self._preview_tile("GLOBAL D455", high, high_stats, cv2),
|
||
self._preview_tile("RIGHT WRIST D405", wrist, wrist_stats, cv2),
|
||
)
|
||
)
|
||
|
||
now_ns = self._now_ns()
|
||
high_age = (
|
||
f"{(now_ns - high.host_monotonic_ns) * 1e-6:.1f} ms"
|
||
if high is not None
|
||
else "-"
|
||
)
|
||
wrist_age = (
|
||
f"{(now_ns - wrist.host_monotonic_ns) * 1e-6:.1f} ms"
|
||
if wrist is not None
|
||
else "-"
|
||
)
|
||
skew = (
|
||
f"{abs(high.host_monotonic_ns - wrist.host_monotonic_ns) * 1e-6:.1f} ms"
|
||
if high is not None and wrist is not None
|
||
else "-"
|
||
)
|
||
episode = (
|
||
f"episode_{self._episode_index}"
|
||
if self._episode_index is not None
|
||
else "-"
|
||
)
|
||
samples = self._store.count if self._store is not None else 0
|
||
footer = np.zeros((80, image.shape[1], 3), dtype=np.uint8)
|
||
lines = (
|
||
f"Age high={high_age} wrist={wrist_age} Camera skew={skew}",
|
||
f"ACT {self.state.value} {episode} Samples {samples}",
|
||
)
|
||
for index, line in enumerate(lines):
|
||
cv2.putText(
|
||
footer,
|
||
line,
|
||
(10, 30 + index * 32),
|
||
cv2.FONT_HERSHEY_SIMPLEX,
|
||
0.7,
|
||
(255, 255, 255),
|
||
1,
|
||
cv2.LINE_AA,
|
||
)
|
||
return np.vstack((image, footer))
|
||
```
|
||
|
||
若尚无帧，对应数值显示 `-`；该方法不修改任何录制器状态。
|
||
|
||
- [ ] **步骤 7：实现预览线程生命周期和故障隔离**
|
||
|
||
在相机成员创建前初始化：
|
||
|
||
```python
|
||
self._preview_stop = threading.Event()
|
||
self._preview_thread: threading.Thread | None = None
|
||
```
|
||
|
||
两台相机成功启动后调用 `_start_preview()`：
|
||
|
||
```python
|
||
if self._camera_start_error is None:
|
||
self._start_preview()
|
||
```
|
||
|
||
实现：
|
||
|
||
```python
|
||
def _start_preview(self) -> None:
|
||
if not (os.environ.get("DISPLAY") or os.environ.get("WAYLAND_DISPLAY")):
|
||
self.get_logger().warn("未检测到桌面显示环境，ACT双相机预览已停用。")
|
||
return
|
||
try:
|
||
import cv2
|
||
except ImportError as exc:
|
||
self.get_logger().warn(f"OpenCV不可用，ACT双相机预览已停用：{exc}")
|
||
return
|
||
self._preview_stop.clear()
|
||
self._preview_thread = threading.Thread(
|
||
target=self._preview_loop,
|
||
args=(cv2,),
|
||
name="act_camera_preview",
|
||
daemon=True,
|
||
)
|
||
self._preview_thread.start()
|
||
|
||
def _preview_loop(self, cv2) -> None:
|
||
try:
|
||
cv2.namedWindow(self.PREVIEW_WINDOW, cv2.WINDOW_NORMAL)
|
||
while not self._preview_stop.is_set():
|
||
cv2.imshow(self.PREVIEW_WINDOW, self._compose_preview(cv2))
|
||
key = cv2.waitKey(1) & 0xFF
|
||
if key in (ord("q"), ord("Q"), 27):
|
||
break
|
||
if cv2.getWindowProperty(
|
||
self.PREVIEW_WINDOW,
|
||
cv2.WND_PROP_VISIBLE,
|
||
) < 1:
|
||
break
|
||
self._preview_stop.wait(0.1)
|
||
except Exception as exc:
|
||
self.get_logger().warn(f"ACT双相机预览已停用：{exc}")
|
||
finally:
|
||
try:
|
||
cv2.destroyWindow(self.PREVIEW_WINDOW)
|
||
except Exception:
|
||
pass
|
||
|
||
def _stop_preview(self) -> None:
|
||
self._preview_stop.set()
|
||
if self._preview_thread is not None:
|
||
self._preview_thread.join(timeout=2.0)
|
||
self._preview_thread = None
|
||
```
|
||
|
||
`close()` 的第一步调用 `self._stop_preview()`，然后保持现有相机停止和目录锁释放顺序。
|
||
|
||
- [ ] **步骤 8：运行预览相关测试并确认通过**
|
||
|
||
运行：
|
||
|
||
```bash
|
||
cd /home/robot/WS_xr
|
||
source /opt/ros/humble/setup.bash
|
||
pytest src/xr_rm_teleop/test/test_act_episode_recorder.py \
|
||
-k "rolling_fps or preview or close_stops_preview" -v
|
||
```
|
||
|
||
预期：所有选中测试通过，测试过程不打开真实窗口或相机。
|
||
|
||
- [ ] **步骤 9：运行 ACT 录制器完整单元测试**
|
||
|
||
运行：
|
||
|
||
```bash
|
||
cd /home/robot/WS_xr
|
||
source /opt/ros/humble/setup.bash
|
||
pytest src/xr_rm_teleop/test/test_act_episode_recorder.py -v
|
||
```
|
||
|
||
预期：全部通过。
|
||
|
||
- [ ] **步骤 10：提交任务 2**
|
||
|
||
```bash
|
||
git add src/xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py \
|
||
src/xr_rm_teleop/test/test_act_episode_recorder.py
|
||
git commit -m "feat: 添加ACT双相机实时预览"
|
||
```
|
||
|
||
### 任务 3：更新使用说明并完成工作空间验证
|
||
|
||
**文件：**
|
||
|
||
- 修改：`README.md:144-169`
|
||
|
||
- [ ] **步骤 1：更新 ACT 数采说明**
|
||
|
||
在 ACT episode 启动命令后补充：
|
||
|
||
```markdown
|
||
`record_act:=true` 启动后默认显示全局 D455 和右腕 D405 双路画面，并显示实时
|
||
FPS、真实丢帧率、帧龄、双相机时间差、录制状态、episode 编号和样本数。按
|
||
`Q`、`Esc` 或关闭窗口只会停止预览，ACT 相机采集和录制继续运行；没有桌面环境
|
||
或 OpenCV 显示失败时也不会影响录制。
|
||
|
||
相机采集线程观察到的真实掉帧仍会拒绝 episode。独立 `30 Hz` 控制和相机时钟
|
||
造成的 ACT 样本重复/跨帧只写入 HDF5 质量指标，不再误报为相机丢包。
|
||
```
|
||
|
||
- [ ] **步骤 2：运行文档和 Python 静态检查**
|
||
|
||
运行：
|
||
|
||
```bash
|
||
cd /home/robot/WS_xr
|
||
source /opt/ros/humble/setup.bash
|
||
python -m py_compile \
|
||
src/xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py \
|
||
src/xr_rm_teleop/test/test_act_episode_recorder.py
|
||
git diff --check
|
||
```
|
||
|
||
预期：命令返回码为 `0`，没有语法或空白错误。
|
||
|
||
- [ ] **步骤 3：运行项目要求的工作空间构建**
|
||
|
||
运行：
|
||
|
||
```bash
|
||
cd /home/robot/WS_xr
|
||
source /opt/ros/humble/setup.bash
|
||
colcon build --symlink-install
|
||
```
|
||
|
||
预期：所有包构建成功。不得在 `/home/robot/WS_xr/src` 中运行该命令。
|
||
|
||
- [ ] **步骤 4：构建后再次运行 ACT 录制器测试**
|
||
|
||
运行：
|
||
|
||
```bash
|
||
cd /home/robot/WS_xr
|
||
source /opt/ros/humble/setup.bash
|
||
source install/setup.bash
|
||
pytest src/xr_rm_teleop/test/test_act_episode_recorder.py -v
|
||
```
|
||
|
||
预期：全部通过。
|
||
|
||
- [ ] **步骤 5：确认提交范围并提交任务 3**
|
||
|
||
运行：
|
||
|
||
```bash
|
||
git status --short
|
||
git diff -- README.md \
|
||
src/xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py \
|
||
src/xr_rm_teleop/test/test_act_episode_recorder.py
|
||
git add README.md
|
||
git commit -m "docs: 更新ACT相机预览说明"
|
||
```
|
||
|
||
不得暂存或提交 `xr_rm_teleop/xr_rm_teleop/fun_peripheral.py`。
|
||
|
||
## 现场验证
|
||
|
||
自动化实施结束后，由用户在确认现场安全条件后运行现有 ACT 数采入口：
|
||
|
||
```bash
|
||
cd /home/robot/WS_xr
|
||
source /opt/ros/humble/setup.bash
|
||
source install/setup.bash
|
||
ros2 launch xr_rm_bringup arm_debug.launch.py \
|
||
arm:=right use_mock:=false record_act:=true
|
||
```
|
||
|
||
该命令会连接右臂真机，代理不得自行执行。现场确认：
|
||
|
||
1. 双路画面默认出现且角色正确；
|
||
2. FPS、真实丢帧率、帧龄、双相机时间差和 ACT 状态持续更新；
|
||
3. 关闭窗口后录制状态和 HDF5 写入继续；
|
||
4. 正常异步重复/跨帧的 episode 能保存；
|
||
5. HDF5 属性包含真实采集质量与两路采样重复/跨帧指标。
|