feat: 更新右臂和左臂配置,禁用奇异性避免;增强ACT数据采集功能,添加日志记录

This commit is contained in:
2026-08-13 16:58:54 +08:00
parent 2d89fe1820
commit 94e1bf9467
7 changed files with 102 additions and 14 deletions
+2 -2
View File
@@ -45,7 +45,7 @@ left_arm_teleop:
realtime_push_host_ip: 192.168.192.148 realtime_push_host_ip: 192.168.192.148
realtime_push_port: 8089 realtime_push_port: 8089
realtime_push_cycle_ms: 5 realtime_push_cycle_ms: 5
avoid_singularity: 1 avoid_singularity: 0
follow: false follow: false
canfd_trajectory_mode: 2 canfd_trajectory_mode: 2
canfd_radio: 0 canfd_radio: 0
@@ -102,7 +102,7 @@ right_arm_teleop:
realtime_push_host_ip: 192.168.192.148 realtime_push_host_ip: 192.168.192.148
realtime_push_port: 8090 realtime_push_port: 8090
realtime_push_cycle_ms: 5 realtime_push_cycle_ms: 5
avoid_singularity: 1 avoid_singularity: 0
follow: false follow: false
canfd_trajectory_mode: 2 canfd_trajectory_mode: 2
canfd_radio: 0 canfd_radio: 0
+1 -1
View File
@@ -38,7 +38,7 @@ single_arm_velocity_teleop:
realtime_push_host_ip: 192.168.192.148 realtime_push_host_ip: 192.168.192.148
realtime_push_port: 8089 realtime_push_port: 8089
realtime_push_cycle_ms: 5 realtime_push_cycle_ms: 5
avoid_singularity: 1 avoid_singularity: 0
follow: false follow: false
canfd_trajectory_mode: 2 canfd_trajectory_mode: 2
canfd_radio: 0 canfd_radio: 0
+1 -1
View File
@@ -37,7 +37,7 @@ single_arm_velocity_teleop:
realtime_push_host_ip: 192.168.192.148 realtime_push_host_ip: 192.168.192.148
realtime_push_port: 8090 realtime_push_port: 8090
realtime_push_cycle_ms: 5 realtime_push_cycle_ms: 5
avoid_singularity: 1 avoid_singularity: 0
# 厂商 MovejCANFD 示例默认低跟随;高跟随仅在验证规划轨迹后单独开启。 # 厂商 MovejCANFD 示例默认低跟随;高跟随仅在验证规划轨迹后单独开启。
follow: false follow: false
canfd_trajectory_mode: 2 canfd_trajectory_mode: 2
@@ -46,6 +46,14 @@ class LauncherCommandsTest(unittest.TestCase):
"Open ROS Topic/Node List Monitor", "Open ROS Topic/Node List Monitor",
"Open Controller Topic Monitor", "Open Controller Topic Monitor",
], ],
"ACT Data Collection": [
"Right Arm ACT Data Collection Launch",
"XRobotoolkit UDP Bridge (90 Hz)",
"Open ACT Recording Status",
"Open Right Arm ACT Control Sample Hz",
"Open ROS Topic/Node List Monitor",
"Open Controller Topic Monitor",
],
"Diagnostics": [ "Diagnostics": [
"ROS Doctor Report", "ROS Doctor Report",
"XR-RM Bringup Prefix", "XR-RM Bringup Prefix",
@@ -79,6 +87,22 @@ class LauncherCommandsTest(unittest.TestCase):
commands["2. Dual Arm MuJoCo Real Hardware Launch"], commands["2. Dual Arm MuJoCo Real Hardware Launch"],
) )
def test_act_mode_uses_confirmed_right_hardware_topics(self) -> None:
commands = dict(launcher_ui.build_commands_by_mode("ACT Data Collection"))
self.assertIn(
"arm:=right use_mock:=false record_act:=true",
commands["1. Right Arm ACT Data Collection Launch"],
)
self.assertEqual(
commands["3. Open ACT Recording Status"],
"ros2 topic echo /act/recording_status",
)
self.assertEqual(
commands["4. Open Right Arm ACT Control Sample Hz"],
"ros2 topic hz /xr_rm/right_rm75/act_control_sample",
)
def test_cmd_vel_monitor_is_completely_removed(self) -> None: def test_cmd_vel_monitor_is_completely_removed(self) -> None:
self.assertFalse(hasattr(launcher_ui, "CMD_VEL_MONITOR_ACTION")) self.assertFalse(hasattr(launcher_ui, "CMD_VEL_MONITOR_ACTION"))
for mode in launcher_ui.MODES: for mode in launcher_ui.MODES:
+20 -2
View File
@@ -1,7 +1,7 @@
#!/usr/bin/env python3 #!/usr/bin/env python3
"""XR-RM 桌面调试启动器。 """XR-RM 桌面调试启动器。
提供 Tkinter 图形界面,按“仿真/MuJoCo/真机/诊断”组织常用 ROS2 launch、 提供 Tkinter 图形界面,按“仿真/MuJoCo/真机/ACT采集/诊断”组织常用 ROS2 launch、
sample_udp_sender、topic 监控和环境检查命令,降低现场调试时的命令输入成本。 sample_udp_sender、topic 监控和环境检查命令,降低现场调试时的命令输入成本。
""" """
@@ -76,6 +76,7 @@ MODES = [
"Simulation", "Simulation",
"MuJoCo", "MuJoCo",
"Real Hardware", "Real Hardware",
"ACT Data Collection",
"Diagnostics", "Diagnostics",
] ]
@@ -275,6 +276,23 @@ def build_commands_by_mode(mode: str) -> list[tuple[str, str]]:
("Right Gripper Open", _tool_command("right", True)), ("Right Gripper Open", _tool_command("right", True)),
("Right Gripper Close", _tool_command("right", False)), ("Right Gripper Close", _tool_command("right", False)),
] ]
elif mode == "ACT Data Collection":
items = [
(
"Right Arm ACT Data Collection Launch",
"ros2 launch xr_rm_bringup arm_debug.launch.py "
"arm:=right use_mock:=false record_act:=true",
),
("XRobotoolkit UDP Bridge (90 Hz)", _xrobotoolkit_bridge_command()),
(
"Open ACT Recording Status",
"ros2 topic echo /act/recording_status",
),
(
"Open Right Arm ACT Control Sample Hz",
"ros2 topic hz /xr_rm/right_rm75/act_control_sample",
),
]
else: else:
items = [ items = [
("ROS Doctor Report", "ros2 doctor --report"), ("ROS Doctor Report", "ros2 doctor --report"),
@@ -329,7 +347,7 @@ class LauncherApp:
mode_frame, mode_frame,
textvariable=self.mode_var, textvariable=self.mode_var,
values=MODES, values=MODES,
width=16, width=20,
state="readonly", state="readonly",
) )
self.mode_combo.grid(row=0, column=1, sticky="ew") self.mode_combo.grid(row=0, column=1, sticky="ew")
+26 -7
View File
@@ -616,14 +616,19 @@ class _StatusPublisher:
class _Logger: class _Logger:
def info(self, *_args, **_kwargs): def __init__(self):
pass self.infos = []
self.warnings = []
self.errors = []
def warn(self, *_args, **_kwargs): def info(self, message, *_args, **_kwargs):
pass self.infos.append(message)
def error(self, *_args, **_kwargs): def warn(self, message, *_args, **_kwargs):
pass self.warnings.append(message)
def error(self, message, *_args, **_kwargs):
self.errors.append(message)
def _control_message(seq, control_ns, *, grip=True): def _control_message(seq, control_ns, *, grip=True):
@@ -705,7 +710,8 @@ def _recorder_for_test(tmp_path):
recorder._status_pub = _StatusPublisher() recorder._status_pub = _StatusPublisher()
recorder._now_ns = lambda: now_ns recorder._now_ns = lambda: now_ns
recorder._disk_usage = lambda _path: SimpleNamespace(free=5 * 1024**3) recorder._disk_usage = lambda _path: SimpleNamespace(free=5 * 1024**3)
recorder.get_logger = lambda: _Logger() recorder._logger = _Logger()
recorder.get_logger = lambda: recorder._logger
return recorder return recorder
@@ -776,6 +782,14 @@ def test_end_to_end_fake_episode_saves_and_returns_idle(tmp_path):
assert recorder.state is RecordingState.IDLE assert recorder.state is RecordingState.IDLE
assert "SAVING" in recorder._status_pub.messages assert "SAVING" in recorder._status_pub.messages
assert recorder._status_pub.messages[-2:] == ["SAVED", "IDLE"] assert recorder._status_pub.messages[-2:] == ["SAVED", "IDLE"]
assert any(
"ACT录制状态:RECORDINGepisode_0" in message
for message in recorder._logger.infos
)
assert any(
"episode_0.hdf53 samples" in message
for message in recorder._logger.infos
)
@requires_h5py @requires_h5py
@@ -791,6 +805,11 @@ def test_final_publish_rejects_allocated_episode_number_conflict(tmp_path):
assert not (recorder._task_dir / "episode_1.hdf5").exists() assert not (recorder._task_dir / "episode_1.hdf5").exists()
rejected = list((recorder._task_dir / "rejected").glob("*.hdf5")) rejected = list((recorder._task_dir / "rejected").glob("*.hdf5"))
assert len(rejected) == 1 assert len(rejected) == 1
assert any(
str(rejected[0]) in message
and "原因:episode_number_conflict" in message
for message in recorder._logger.warnings
)
with h5py.File(rejected[0], "r") as root: with h5py.File(rejected[0], "r") as root:
assert root.attrs["reject_reason"] == "episode_number_conflict" assert root.attrs["reject_reason"] == "episode_number_conflict"
@@ -1142,6 +1142,13 @@ class ActEpisodeRecorder(Node):
self._on_left_controller, self._on_left_controller,
10, 10,
) )
self.get_logger().info(
f"ACT录制器已启动,输出目录:{self._task_dir}"
)
self.get_logger().info(
"操作提示:右手B准备/结束录制;准备后握住右手Grip开始采样;"
"左手Y长按1秒丢弃;录制中不要按右手A"
)
self._publish_state(RecordingState.IDLE) self._publish_state(RecordingState.IDLE)
@staticmethod @staticmethod
@@ -1182,6 +1189,12 @@ class ActEpisodeRecorder(Node):
message = String() message = String()
message.data = state.value if not reason else f"{state.value}:{reason}" message.data = state.value if not reason else f"{state.value}:{reason}"
self._status_pub.publish(message) self._status_pub.publish(message)
episode = (
f"episode_{self._episode_index}"
if self._episode_index is not None
else ""
)
self.get_logger().info(f"ACT录制状态:{message.data}{episode}")
def _run_preflight(self) -> str | None: def _run_preflight(self) -> str | None:
now_ns = self._now_ns() now_ns = self._now_ns()
@@ -1557,6 +1570,10 @@ class ActEpisodeRecorder(Node):
except FileExistsError: except FileExistsError:
self._reject_closed_partial("episode_number_conflict") self._reject_closed_partial("episode_number_conflict")
return return
self.get_logger().info(
f"ACT数据已保存:{destination}"
f"{report.metrics['sample_count']} samples"
)
self._finish_result(RecordingState.SAVED) self._finish_result(RecordingState.SAVED)
def _reject_closed_partial( def _reject_closed_partial(
@@ -1570,6 +1587,7 @@ class ActEpisodeRecorder(Node):
root.attrs["episode_status"] = "rejected" root.attrs["episode_status"] = "rejected"
root.attrs["reject_reason"] = reason root.attrs["reject_reason"] = reason
root.attrs["interrupted"] = np.bool_(interrupted) root.attrs["interrupted"] = np.bool_(interrupted)
sample_count = int(root["action"].shape[0])
rejected = self._task_dir / "rejected" rejected = self._task_dir / "rejected"
rejected.mkdir(exist_ok=True) rejected.mkdir(exist_ok=True)
safe_reason = re.sub(r"[^a-zA-Z0-9_-]", "_", reason) safe_reason = re.sub(r"[^a-zA-Z0-9_-]", "_", reason)
@@ -1581,6 +1599,10 @@ class ActEpisodeRecorder(Node):
/ f"episode_{episode_index}_{safe_reason}_{timestamp}.hdf5" / f"episode_{episode_index}_{safe_reason}_{timestamp}.hdf5"
) )
publish_without_overwrite(self._partial_path, destination) publish_without_overwrite(self._partial_path, destination)
self.get_logger().warn(
f"ACT数据已拒绝:{destination}{sample_count} samples"
f"原因:{reason}"
)
self._finish_result(RecordingState.REJECTED, reason) self._finish_result(RecordingState.REJECTED, reason)
def _reject_current( def _reject_current(
@@ -1609,11 +1631,16 @@ class ActEpisodeRecorder(Node):
def _discard_current(self) -> None: def _discard_current(self) -> None:
if self._partial_path is None: if self._partial_path is None:
return return
partial = self._partial_path
if self._writer is not None: if self._writer is not None:
self._writer.finish() self._writer.finish()
sample_count = self._store.count if self._store is not None else 0
if self._store is not None: if self._store is not None:
self._store.close() self._store.close()
discard_partial(self._partial_path) discard_partial(partial)
self.get_logger().info(
f"ACT数据已丢弃:{partial}{sample_count} samples"
)
self._finish_result(RecordingState.DISCARDED) self._finish_result(RecordingState.DISCARDED)
def _finish_result( def _finish_result(