feat: 更新右臂和左臂配置,禁用奇异性避免;增强ACT数据采集功能,添加日志记录
This commit is contained in:
@@ -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
|
||||||
|
|||||||
@@ -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
|
||||||
|
|||||||
@@ -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:
|
||||||
|
|||||||
@@ -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")
|
||||||
|
|||||||
@@ -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录制状态:RECORDING,episode_0" in message
|
||||||
|
for message in recorder._logger.infos
|
||||||
|
)
|
||||||
|
assert any(
|
||||||
|
"episode_0.hdf5(3 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(
|
||||||
|
|||||||
Reference in New Issue
Block a user