meiqi/TG3
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: add head control and joint data decoder

Browse Source
This commit is contained in:
meiqi
2026-08-11 19:10:35 +08:00
parent 4cceebfa5f
commit ae0b1dcc85
21 changed files with 1263 additions and 76 deletions
Download Patch File Download Diff File Expand all files Collapse all files

40
verify.sh
View File

@@ -14,7 +14,8 @@ for executable in \
"$repo_dir/tg3_local_teleop/wait_ros_ready.sh" \
"$repo_dir/tg3_local_teleop/home.sh" \
"$repo_dir/tg3_local_teleop/status.sh" \
"$repo_dir/tg3_data_collection/data_get_sync.py"; do
"$repo_dir/tg3_data_collection/data_get_sync.py" \
"$repo_dir/tg3_data_collection/decode_joint_data.py"; do
if [[ ! -x "$executable" ]]; then
echo "Required executable bit is missing: $executable" >&2
exit 1
@@ -25,14 +26,17 @@ python3 -m py_compile \
"$repo_dir/tg3_omnisocket_transport/omnisocket_xtele_sender.py" \
"$repo_dir/tg3_local_teleop/tg3_local_teleop.py" \
"$repo_dir/tg3_local_teleop/gesture_toggle.py" \
"$repo_dir/tg3_local_teleop/head_control.py" \
"$repo_dir/tg3_local_teleop/data_collection.py" \
"$repo_dir/tg3_local_teleop/data_recorder_protocol.py" \
"$repo_dir/tg3_local_teleop/data_recorder_node.py" \
"$repo_dir/tg3_local_teleop/delete_ready_episode.py" \
"$repo_dir/tg3_data_collection/data_get_sync.py"
"$repo_dir/tg3_data_collection/data_get_sync.py" \
"$repo_dir/tg3_data_collection/decode_joint_data.py"
python3 "$repo_dir/tg3_omnisocket_transport/test_session_gate.py"
python3 "$repo_dir/tg3_local_teleop/test_session_gate.py"
python3 "$repo_dir/tg3_local_teleop/test_gesture_toggle.py"
python3 "$repo_dir/tg3_local_teleop/test_head_control.py"
python3 "$repo_dir/tg3_local_teleop/test_idle_session_refresh.py"
python3 "$repo_dir/tg3_local_teleop/test_data_collection.py"
python3 "$repo_dir/tg3_local_teleop/test_data_recorder_protocol.py"
@@ -55,34 +59,34 @@ head_rgbd = {
"/ob_camera_head/depth/camera_info",
"/ob_camera_head/depth/metadata",
}
waist_rgbd = {
"/ob_camera_waist/color/image_raw/compressed",
"/ob_camera_waist/color/camera_info",
"/ob_camera_waist/color/metadata",
"/ob_camera_waist/depth/image_raw/compressedDepth",
"/ob_camera_waist/depth/camera_info",
"/ob_camera_waist/depth/metadata",
}
assert head_rgbd <= topics, "head RGB-D topics must stay in the recording whitelist"
assert waist_rgbd <= topics, "waist RGB-D topics must stay in the recording whitelist"
assert head_rgbd.isdisjoint(required), "head RGB-D must not block recording"
assert waist_rgbd.isdisjoint(required), "waist RGB-D must not block recording"
assert not any(topic.startswith("/ob_camera_waist/") for topic in topics), (
"waist camera topics must stay out of the recording whitelist"
)
assert "/tf_static" in topics and "/tf_static" not in required
assert section.get("minimum_topic_rates_hz", {}) == {}
groups = section["optional_topic_groups"]
assert set(groups) == {"head_rgbd", "waist_rgbd"}
assert set(groups) == {"head_rgbd"}
assert set(groups["head_rgbd"]["topics"]) == head_rgbd
assert set(groups["waist_rgbd"]["topics"]) == waist_rgbd
assert groups["head_rgbd"]["minimum_topic_rates_hz"] == {
"/ob_camera_head/color/image_raw/compressed": 20.0,
"/ob_camera_head/depth/image_raw/compressedDepth": 20.0,
}
assert groups["waist_rgbd"]["minimum_topic_rates_hz"] == {
"/ob_camera_waist/color/image_raw/compressed": 20.0,
"/ob_camera_waist/depth/image_raw/compressedDepth": 20.0,
}
assert section["minimum_free_gib"] >= 100.0
assert section["retain_failed_episodes"] is False
head = document["head"]
assert head["enabled"] is True
assert head["command_topic"] == "/head/cmd"
assert head["state_topic"] == "/robot_state"
assert head["pitch_motor_id"] == 2
assert head["mode"] == 0
assert 0.0 < head["max_current_a"] <= 1.0
assert -0.2617994 <= head["min_pitch_rad"] < head["max_pitch_rad"] <= 1.012291
assert 0.0 < head["max_pitch_speed_rad_s"] <= 0.2
assert 0.0 < head["max_pitch_accel_rad_s2"] <= 0.5
assert 0.0 <= head["joystick_deadzone"] < 1.0
assert "/head/cmd" in topics and "/head/cmd" not in required
print("config.toml parse passed")
PY