From ae0b1dcc85ba289d75b775ed63a3bd95c627cae2 Mon Sep 17 00:00:00 2001 From: meiqi <2510105031@mails.szu.edu.cn> Date: Tue, 11 Aug 2026 19:10:35 +0800 Subject: [PATCH] feat: add head control and joint data decoder --- README.md | 9 +- docs/天工3.0同构臂遥操与数采跨设备迁移手册.md | 14 +- docs/天工3.0本地同构臂遥操迁移部署指南.md | 39 +- tg3_data_collection/README.md | 26 ++ tg3_data_collection/decode_joint_data.py | 377 ++++++++++++++++++ tg3_local_teleop/README.md | 25 +- tg3_local_teleop/config.toml | 58 ++- tg3_local_teleop/head_control.py | 226 +++++++++++ .../src/ros2_bridge_msgs/CMakeLists.txt | 7 + .../src/ros2_bridge_msgs/msg/HeadCtrl.msg | 5 + .../src/ros2_bridge_msgs/msg/HeadStatus.msg | 4 + .../src/ros2_bridge_msgs/msg/ImuStatus.msg | 20 + .../src/ros2_bridge_msgs/msg/LegStatus.msg | 4 + .../src/ros2_bridge_msgs/msg/MotorCtrl.msg | 8 + .../src/ros2_bridge_msgs/msg/RobotState.msg | 6 + .../src/ros2_bridge_msgs/msg/WaistStatus.msg | 4 + tg3_local_teleop/test_head_control.py | 190 +++++++++ tg3_local_teleop/test_idle_session_refresh.py | 3 + tg3_local_teleop/test_session_gate.py | 29 +- tg3_local_teleop/tg3_local_teleop.py | 245 +++++++++++- verify.sh | 40 +- 21 files changed, 1263 insertions(+), 76 deletions(-) create mode 100755 tg3_data_collection/decode_joint_data.py create mode 100644 tg3_local_teleop/head_control.py create mode 100644 tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/HeadCtrl.msg create mode 100644 tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/HeadStatus.msg create mode 100644 tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/ImuStatus.msg create mode 100644 tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/LegStatus.msg create mode 100644 tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/MotorCtrl.msg create mode 100644 tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/RobotState.msg create mode 100644 tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/WaistStatus.msg create mode 100644 tg3_local_teleop/test_head_control.py diff --git a/README.md b/README.md index 64ed837..98ccd43 100644 --- a/README.md +++ b/README.md @@ -47,6 +47,9 @@ TOML 解析检查。若还要核对外置 OmniSocketGo 版本，可设置任务 稳定松开，再重新长按 3 秒。pending 和活动会话都不会自动重试或自动恢复。 - 遥操开启且双摇杆先回中过一次后，左摇杆上下直接控制 HBWALK 前进/后退。 - 遥操开启且双摇杆先回中过一次后，右摇杆左右直接控制 HBWALK 原地转向。 +- 遥操开启且右摇杆纵轴先回中过一次后，按住左 Z + 右摇杆上下调整 + 头部相机视角：上推抬头、下推低头；无需 R3。按住 Z 时会屏蔽右摇杆水平 + 转向，避免调整视角时误转。软件工作区为 `-10°～+50°`，最大 `0.2 rad/s`。 - 右 B 连续 1 秒：左右手同时进入厂商“单食指”姿态；松开至少 0.5 秒后再次连续 1 秒退出。手指动作继续受 `400 units/s` 限速。 - 按住右 A：右手限速摆到目标位置 `[428,735,500,77,77,72]`；松开 A 后恢复 @@ -57,9 +60,9 @@ TOML 解析检查。若还要核对外置 OmniSocketGo 版本，可设置任务 ## 数采保存 Nvidia 独立服务录制明确白名单内的机器人实测状态、双臂/双手命令与反馈、行走、 -IMU、电源状态、xTELE 应用帧以及头/腰 Orbbec 的 RGB-D。RGB 使用标准 JPEG 压缩流， -16 位深度使用无损 compressedDepth PNG，并同时保存相机内参与静态 TF。头部和腰部 -相机都是可选流：服务运行就自动采集，未运行不阻塞核心 episode；manifest 会分别标记 +IMU、电源状态、xTELE 应用帧以及头部 Orbbec 的 RGB-D。RGB 使用标准 JPEG 压缩流， +16 位深度使用无损 compressedDepth PNG，并同时保存相机内参与静态 TF。头部相机 +是可选流：服务运行就自动采集，未运行不阻塞核心 episode；manifest 会标记 `absent/healthy/partial/low_rate`。它不接管或停止厂家 `/record_bag_node`，也不录点云。正常结束后先校验 MCAP、生成 SHA-256 `manifest.json` 和 `READY`，再由 diff --git a/docs/天工3.0同构臂遥操与数采跨设备迁移手册.md b/docs/天工3.0同构臂遥操与数采跨设备迁移手册.md index 420209d..5aff0d1 100644 --- a/docs/天工3.0同构臂遥操与数采跨设备迁移手册.md +++ b/docs/天工3.0同构臂遥操与数采跨设备迁移手册.md @@ -488,8 +488,9 @@ joint_goal_rad = [<新机器人实测的14个弧度值>] - `[hands].closed_normalized` 和单食指姿态； - `[control]` 关节限位是否仍对应当前型号和 SDK； - `[locomotion]` 的 Topic、方向和厂家速度上限； +- `[head]` 的 `/head/cmd`、`/robot_state`、俯仰方向、工作区、速度/加速度和外部命令租约； - `base_directory` 是否仍是 `/home/nvidia/tg3_data_collection`； -- 相机 Topic 是否仍为 `/ob_camera_head/...` 和 `/ob_camera_waist/...`。 +- 头部相机 Topic 是否仍为 `/ob_camera_head/...`。 ### 6.5 新机器人必须重新采集 Home @@ -568,7 +569,8 @@ restart 本机同步服务。 | 换 TS1P | EAI 原厂标定、EAI Peer、机器人 `expected_iarm_id`，必要时方向和按键复核 | | 换灵巧手型号 | 必须改代码和消息映射；不能只换 Topic 字符串 | | 相机未安装或未启动 | 不阻塞数采；manifest 标记 `absent` | -| 头/腰相机启动 | 下一条数采自动包含对应压缩 RGB-D，并记录质量状态 | +| 头部相机启动 | 下一条数采自动包含头部压缩 RGB-D，并记录质量状态 | +| 腰部相机启动 | 不录制；当前白名单不含 `/ob_camera_waist/...` | ## 9. 统一启动顺序与现场验收 @@ -618,7 +620,7 @@ python3 -m json.tool Data_Get/sync_status.json - EAI 本地 5003/5001 数据持续更新； - 待机时 EAI 不发送业务帧； - PS 同步无错误且 pending 为 0； -- 头/腰相机未启动不影响 recorder 服务 active。 +- 头部相机未启动不影响 recorder 服务 active；腰部相机不在数采白名单。 ### 9.3 逐级运动验收 @@ -631,8 +633,10 @@ python3 -m json.tool Data_Get/sync_status.json 5. 长按右 B 1 秒验证左右手同时单食指，再松开 0.5 秒、长按 1 秒退出； 6. 确认双摇杆回中后，直接小幅推动左摇杆验证前后，回中应立即零速； 7. 直接小幅横推右摇杆验证原地转向，回中应立即零速； -8. L3 长按 1 秒开始短数采，再次长按 1 秒结束； -9. 长按 Z+C 3 秒结束遥操，确认限速回新机器人的 Home。 +8. 右摇杆纵轴回中后，按住左 Z 再小幅上推/下推，确认头部相机视角 + 抬高/降低；同时小幅横向偏移不应让机器人转向，松开 Z 或回中后停止更新； +9. L3 长按 1 秒开始短数采，再次长按 1 秒结束； +10. 长按 Z+C 3 秒结束遥操，确认限速回新机器人的 Home。 不要用本项目发布 `gotoHBWALK`，不要向厂家 `/proc_manager/config/notify` 发送伪进程启停 JSON，也不要为了迁移删除厂家限位、电流、碰撞或急停保护。 diff --git a/docs/天工3.0本地同构臂遥操迁移部署指南.md b/docs/天工3.0本地同构臂遥操迁移部署指南.md index f51fe70..1163cf4 100644 --- a/docs/天工3.0本地同构臂遥操迁移部署指南.md +++ b/docs/天工3.0本地同构臂遥操迁移部署指南.md @@ -926,12 +926,15 @@ systemctl --user enable --now tg3-omnisocket-sender.service 11. 仅在 `[locomotion].enabled=true` 且相应链路断流保护已经验证后：先确认双摇杆回中， 再把左摇杆小幅向前推出死区，确认下一个 50 Hz 周期立即响应；回中后确认立即零速停止； 12. 在同一行走前提下，直接把右摇杆小幅横推，确认机器人原地转向；回中应立即清零角速度； -13. 再次长按 Z+C 停止，观察双臂限速回到新机器人保存的 Home； -14. 仅在 OmniSocket 模式、机器人已静止且急停可用时测试 Hub 断线：EAI sender 的 KCP +13. 确认右摇杆纵轴回中后，按住左 Z 再小幅上推/下推，验证头部相机抬头/低头； + 按 Z 期间右摇杆水平轴不应让机器人转向。检查角度始终位于 `-10°～+50°`，且 + 外部 `/head/cmd` 活跃时本项目会暂停头部输出； +14. 再次长按 Z+C 停止，观察双臂限速回到新机器人保存的 Home； +15. 仅在 OmniSocket 模式、机器人已静止且急停可用时测试 Hub 断线：EAI sender 的 KCP 反馈超时约为 500 ms，触发后会作废 session；机器人若持续约 2 秒收不到活动帧，会另行 重建我们的接收桥。不应期待断线后透明续控；恢复后先确认未武装，稳定松开 Z+C 后再重新长按 启动。纯 ZMQ 模式在完成本地断流门控前禁止做运动中的断链测试； -15. 记录 SSH 地址、Home、行走限速、手部端点和单食指姿态到设备档案；OmniSocket 模式 +16. 记录 SSH 地址、Home、行走限速、手部端点和单食指姿态到设备档案；OmniSocket 模式 另记 Peer ID/Hub，纯 ZMQ 模式改记 EAI 固定局域网 IP、TCP 5003 防火墙范围和断流门控参数。 迁移时确认目标机提供 `/hric/robot/cmd_vel`（`geometry_msgs/msg/TwistStamped`），并保持 @@ -1026,6 +1029,7 @@ ZMQ 专用断流门控前不得启用运动发布。 ```text tg3_local_teleop.py +head_control.py data_collection.py data_recorder_protocol.py data_recorder_node.py @@ -1052,38 +1056,37 @@ systemctl --user restart tg3-local-teleop.service ``` 录制脚本必须依次加载 `/opt/ros/jazzy`、`/home/nvidia/xos` 和 -`/opt/robot_tele_server/install`，但**不能**再加载本项目 `ros2_py/install`。后者是桥接 -Python 进程专用的最小 `ArmStatus` 绑定，会遮蔽厂家完整 `ros2_bridge_msgs` C++ 库，导致 +`/opt/robot_tele_server/install`，但**不能**再加载本项目 `ros2_py/install`。后者只是遥操桥 +Python 进程专用的消息绑定（含 `ArmStatus`/`RobotState`/`HeadCtrl`），并不是 +厂家完整 `ros2_bridge_msgs` 包；它会遮蔽厂家 C++ 库，导致 rosbag 加载 `RobotState/ArmCtrl` 时出现 undefined symbol。`run.sh` 仍需项目 overlay； `run_data_recorder.sh` 只用厂家完整消息 overlay。 还需 Python 3 的 PyYAML（Ubuntu 包 `python3-yaml`）解析 rosbag metadata。 迁移时在 `config.toml [data_collection]` 核对 `base_directory`、100 GiB 余量、30 分钟 上限、明确 topic 白名单和 required topics。不要改成 `ros2 bag record -a`，点云需另行 -估算带宽和磁盘后再加入。当前白名单已经包含头/腰 Orbbec 的 JPEG RGB、无损 PNG 深度、 -CameraInfo、Metadata 和 `/tf_static`。头、腰相机都是 optional：服务已启动时 rosbag 自动 +估算带宽和磁盘后再加入。当前白名单只包含头部 Orbbec 的 JPEG RGB、无损 PNG 深度、 +CameraInfo、Metadata 和 `/tf_static`。头部相机是 optional：服务已启动时 rosbag 自动 发现并采集；未启动时不影响核心 episode 开始或进入 READY。收尾后 manifest 会分别把 -两台相机标成 `absent`、`healthy`、`partial` 或 `low_rate`，后两种附带质量警告但不阻塞 +头部相机标成 `absent`、`healthy`、`partial` 或 `low_rate`，后两种附带质量警告但不阻塞 核心数据保存。 -需要采集某台相机的完整 episode 时，先确认相机服务和发布者；不需要相机时可保持服务 +需要采集头部相机的完整 episode 时，先确认相机服务和发布者；不需要相机时可保持服务 停止，L3 数采仍可正常启动： ```bash -systemctl is-active orbbec_waist.service orbbec_head.service -sudo systemctl enable --now orbbec_waist.service -# 同时需要头部 RGB-D 时： +systemctl is-active orbbec_head.service sudo systemctl enable --now orbbec_head.service -ros2 topic info /ob_camera_waist/color/image_raw/compressed -ros2 topic info /ob_camera_waist/depth/image_raw/compressedDepth +ros2 topic info /ob_camera_head/color/image_raw/compressed +ros2 topic info /ob_camera_head/depth/image_raw/compressedDepth ``` -希望采集腰部图像时，两条腰部话题应显示 `Publisher count: 1`；头部同理。原始 +希望釆头部图像时，两条话题都应显示 `Publisher count: 1`。原始 1280x720 RGB+深度约 `8.3 GB/min/相机`，所以默认不重复录 raw，而是录标准 image_transport 压缩流；现场 -单相机约 `1.3–1.5 GB/min`，实际随画面变化。头部和腰部都运行时应按约两倍预留空间与 -同步时间。`minimum_free_gib=100` 是停止下限，不是整条 episode 的容量预算；双相机 -长录制前至少预留“100 GiB + 预计数据量”，并在短录包中先确认实际带宽。 +单相机约 `1.3–1.5 GB/min`，实际随画面变化。`minimum_free_gib=100` 是停止下限， +不是整条 episode 的容量预算；长录制前至少预留“100 GiB + 预计数据量”，并在短录包中 +先确认实际带宽。 ### 11.2 PS 本机 diff --git a/tg3_data_collection/README.md b/tg3_data_collection/README.md index 37e140a..68443e5 100644 --- a/tg3_data_collection/README.md +++ b/tg3_data_collection/README.md @@ -116,6 +116,32 @@ NOREPLACE 语义原子发布、fsync 父目录，最后写入并 fsync `VERIFIED 刚创建的失败 payload，因此机器人不会长期保留失败 MCAP。若以后改回 `true` 以保留故障 诊断，必须另建独立验真与回收协议，不能整目录清空。 +## 解码双臂关节数据 + +MCAP 内的自定义 TG3 消息使用 CDR 编码。解码器读取项目保存的消息定义，并把实际反馈、 +遥操目标、厂家桥后命令和 xTELE 原始目标分别导出为长表 CSV。首次使用在独立虚拟环境 +安装依赖： + +```bash +python3 -m venv /tmp/tg3-mcap-venv +/tmp/tg3-mcap-venv/bin/pip install mcap==1.4.0 mcap-ros2-support==0.5.7 + +/tmp/tg3-mcap-venv/bin/python tg3_data_collection/decode_joint_data.py \ + Data_Get/ +``` + +输出位于 `Data_Get//decoded_joints/`： + +- `robot_arm_feedback.csv`：`/robot_state`、`/freq_change/arm_status` 和 + `/data_logger/arm_status` 的实测位置、速度、电流、温度及错误码； +- `teleop_joint_target.csv`：`/encoder_identical_joint` 的 14 轴遥操目标； +- `vendor_arm_command.csv`：`/arm/cmd` 的厂家桥后控制参数； +- `iarm_source_joint.csv`：`/tg3/data_collection/iarm_frame` 中的 TS1P 原始目标； +- `summary.json`：关节顺序、Motor ID、单位和行数。 + +关节顺序为左臂 7 轴后右臂 7 轴，Motor ID 分别为 `11..17` 和 `21..27`；位置单位 +为 rad，速度单位为 rad/s。输出目录必须不存在，避免误覆盖已经分析或标注过的数据。 + 迁移后需要同步修改 service 中的本机项目路径和机器人 SSH 地址。如果 Nvidia 的 `192.168.41.2` 改变，只改 unit 的 `--remote` 并重新部署 helper；它不在机器人 `config.toml` 或 OmniSocket Peer 设置中。启用自动删除时远端路径必须保持固定的 diff --git a/tg3_data_collection/decode_joint_data.py b/tg3_data_collection/decode_joint_data.py new file mode 100755 index 0000000..1ba9e82 --- /dev/null +++ b/tg3_data_collection/decode_joint_data.py @@ -0,0 +1,377 @@ +#!/usr/bin/env python3 +"""Decode TG3 joint streams from one locally stored MCAP episode into CSV.""" + +from __future__ import annotations + +import argparse +import csv +import json +from pathlib import Path +from typing import Any, Callable + + +ARM_MOTOR_IDS = [*range(11, 18), *range(21, 28)] +JOINT_NAMES = [ + *(f"left_joints_{index}" for index in range(7)), + *(f"right_joints_{index}" for index in range(7)), +] +CUSTOM_TOPICS = { + "/robot_state": "ros2_bridge_msgs/msg/RobotState", + "/freq_change/arm_status": "ros2_bridge_msgs/msg/ArmStatus", + "/data_logger/arm_status": "ros2_bridge_msgs/msg/ArmStatus", + "/arm/cmd": "ros2_bridge_msgs/msg/ArmCtrl", +} +TOPICS = [ + "/robot_state", + "/encoder_identical_joint", + "/freq_change/arm_status", + "/data_logger/arm_status", + "/arm/cmd", + "/tg3/data_collection/iarm_frame", +] + + +def _message_directory() -> Path: + return ( + Path(__file__).resolve().parents[1] + / "tg3_local_teleop" + / "ros2_py" + / "src" + / "ros2_bridge_msgs" + / "msg" + ) + + +def _read_definition(name: str) -> str: + path = _message_directory() / f"{name}.msg" + if not path.is_file(): + raise RuntimeError(f"required message definition is missing: {path}") + return path.read_text(encoding="utf-8") + + +def _definition_bundle(root_name: str, dependencies: list[tuple[str, str]]) -> str: + if root_name == "ArmCtrl": + text = ( + "std_msgs/Header header\n" + "uint8 mode\n" + "uint8 label\n" + "uint8 reserved\n" + "ros2_bridge_msgs/MotorCtrl[] ctrl\n" + ) + else: + text = _read_definition(root_name) + for full_name, definition in dependencies: + text += f"\n===\nMSG: {full_name}\n{definition}" + return text + + +def _custom_decoders() -> dict[str, Callable[[bytes], Any]]: + try: + from mcap_ros2._dynamic import generate_dynamic + except ImportError as exc: + raise RuntimeError( + "missing MCAP decoder; install with: pip install mcap mcap-ros2-support" + ) from exc + + header = "builtin_interfaces/Time stamp\nstring frame_id\n" + motor_status = _read_definition("MotorStatus") + common = [ + ("std_msgs/msg/Header", header), + ("ros2_bridge_msgs/msg/MotorStatus", motor_status), + ] + robot_dependencies = common + [ + (f"ros2_bridge_msgs/msg/{name}", _read_definition(name)) + for name in ( + "HeadStatus", + "WaistStatus", + "LegStatus", + "ArmStatus", + "ImuStatus", + ) + ] + schemas = { + "ros2_bridge_msgs/msg/RobotState": _definition_bundle( + "RobotState", robot_dependencies + ), + "ros2_bridge_msgs/msg/ArmStatus": _definition_bundle( + "ArmStatus", common + ), + "ros2_bridge_msgs/msg/ArmCtrl": _definition_bundle( + "ArmCtrl", + [ + ("std_msgs/msg/Header", header), + ("ros2_bridge_msgs/msg/MotorCtrl", _read_definition("MotorCtrl")), + ], + ), + } + return { + type_name: generate_dynamic(type_name, text)[type_name] + for type_name, text in schemas.items() + } + + +def _stamp_ns(header: Any) -> int: + return int(header.stamp.sec) * 1_000_000_000 + int(header.stamp.nanosec) + + +def _side_and_index(motor_id: int) -> tuple[str, int]: + if 11 <= motor_id <= 17: + return "left", motor_id - 11 + if 21 <= motor_id <= 27: + return "right", motor_id - 21 + return "unknown", -1 + + +def _open_csv(path: Path, columns: list[str]) -> tuple[Any, csv.DictWriter]: + stream = path.open("x", encoding="utf-8", newline="") + writer = csv.DictWriter(stream, fieldnames=columns) + writer.writeheader() + return stream, writer + + +def decode_episode(episode: Path, output: Path) -> dict[str, int]: + try: + from mcap.reader import make_reader + from mcap_ros2.decoder import DecoderFactory + except ImportError as exc: + raise RuntimeError( + "missing MCAP decoder; install with: pip install mcap mcap-ros2-support" + ) from exc + + bag_files = sorted((episode / "bag").glob("*.mcap")) + if not bag_files: + raise RuntimeError(f"no MCAP files found under {episode / 'bag'}") + output.mkdir(parents=True, exist_ok=False) + + feedback_columns = [ + "bag_time_ns", + "ros_time_ns", + "topic", + "side", + "joint_index", + "motor_id", + "position_rad", + "speed_rad_s", + "current_a", + "temperature_c", + "mos_temperature_c", + "error", + ] + target_columns = [ + "bag_time_ns", + "ros_time_ns", + "joint_index", + "joint_name", + "motor_id", + "side", + "position_rad", + "velocity_rad_s", + "effort", + ] + command_columns = [ + "bag_time_ns", + "ros_time_ns", + "mode", + "label", + "motor_id", + "side", + "joint_index", + "kp", + "kd", + "position_rad", + "speed_rad_s", + "torque_feedforward", + "current_limit_a", + ] + source_columns = [ + "bag_time_ns", + "source_time_ms", + "joint_index", + "joint_name", + "motor_id", + "side", + "position_rad", + ] + streams: list[Any] = [] + feedback_stream, feedback_writer = _open_csv( + output / "robot_arm_feedback.csv", feedback_columns + ) + target_stream, target_writer = _open_csv( + output / "teleop_joint_target.csv", target_columns + ) + command_stream, command_writer = _open_csv( + output / "vendor_arm_command.csv", command_columns + ) + source_stream, source_writer = _open_csv( + output / "iarm_source_joint.csv", source_columns + ) + streams.extend((feedback_stream, target_stream, command_stream, source_stream)) + + counts = { + "robot_arm_feedback_rows": 0, + "teleop_joint_target_rows": 0, + "vendor_arm_command_rows": 0, + "iarm_source_joint_rows": 0, + } + custom_decoders = _custom_decoders() + standard_factory = DecoderFactory() + try: + for bag_file in bag_files: + with bag_file.open("rb") as bag_stream: + reader = make_reader(bag_stream) + for schema, channel, message in reader.iter_messages(topics=TOPICS): + topic = channel.topic + if topic in CUSTOM_TOPICS: + decoded = custom_decoders[CUSTOM_TOPICS[topic]](message.data) + else: + decoder = standard_factory.decoder_for( + channel.message_encoding, schema + ) + if decoder is None: + raise RuntimeError( + f"no decoder for {topic} ({schema.name if schema else 'no schema'})" + ) + decoded = decoder(message.data) + + if topic in ( + "/robot_state", + "/freq_change/arm_status", + "/data_logger/arm_status", + ): + status_message = decoded.arm if topic == "/robot_state" else decoded + for motor in status_message.status: + motor_id = int(motor.name) + side, joint_index = _side_and_index(motor_id) + feedback_writer.writerow( + { + "bag_time_ns": message.log_time, + "ros_time_ns": _stamp_ns(decoded.header), + "topic": topic, + "side": side, + "joint_index": joint_index, + "motor_id": motor_id, + "position_rad": motor.pos, + "speed_rad_s": motor.speed, + "current_a": motor.current, + "temperature_c": motor.temperature, + "mos_temperature_c": motor.mos_temperature, + "error": motor.error, + } + ) + counts["robot_arm_feedback_rows"] += 1 + elif topic == "/encoder_identical_joint": + for index, position in enumerate(decoded.position): + motor_id = ARM_MOTOR_IDS[index] if index < 14 else -1 + side, _ = _side_and_index(motor_id) + target_writer.writerow( + { + "bag_time_ns": message.log_time, + "ros_time_ns": _stamp_ns(decoded.header), + "joint_index": index, + "joint_name": decoded.name[index] + if index < len(decoded.name) + else "", + "motor_id": motor_id, + "side": side, + "position_rad": position, + "velocity_rad_s": decoded.velocity[index] + if index < len(decoded.velocity) + else "", + "effort": decoded.effort[index] + if index < len(decoded.effort) + else "", + } + ) + counts["teleop_joint_target_rows"] += 1 + elif topic == "/arm/cmd": + for motor in decoded.ctrl: + motor_id = int(motor.name) + side, joint_index = _side_and_index(motor_id) + command_writer.writerow( + { + "bag_time_ns": message.log_time, + "ros_time_ns": _stamp_ns(decoded.header), + "mode": decoded.mode, + "label": decoded.label, + "motor_id": motor_id, + "side": side, + "joint_index": joint_index, + "kp": motor.kp, + "kd": motor.kd, + "position_rad": motor.pos, + "speed_rad_s": motor.spd, + "torque_feedforward": motor.tor, + "current_limit_a": motor.cur, + } + ) + counts["vendor_arm_command_rows"] += 1 + elif topic == "/tg3/data_collection/iarm_frame": + payload = json.loads(decoded.data) + positions = payload.get("arm", {}).get("position", {}) + values = [ + *positions.get("left", []), + *positions.get("right", []), + ] + if len(values) != 14: + continue + source_time = payload.get("timestamp", "") + for index, position in enumerate(values): + motor_id = ARM_MOTOR_IDS[index] + side, _ = _side_and_index(motor_id) + source_writer.writerow( + { + "bag_time_ns": message.log_time, + "source_time_ms": source_time, + "joint_index": index, + "joint_name": JOINT_NAMES[index], + "motor_id": motor_id, + "side": side, + "position_rad": position, + } + ) + counts["iarm_source_joint_rows"] += 1 + finally: + for stream in streams: + stream.close() + + (output / "summary.json").write_text( + json.dumps( + { + "episode": episode.name, + "joint_order": JOINT_NAMES, + "motor_ids": ARM_MOTOR_IDS, + "units": { + "position": "rad", + "speed": "rad/s", + "current": "A", + "temperature": "degC", + }, + "rows": counts, + }, + ensure_ascii=False, + indent=2, + ) + + "\n", + encoding="utf-8", + ) + return counts + + +def main() -> int: + parser = argparse.ArgumentParser() + parser.add_argument("episode", type=Path, help="Data_Get episode directory") + parser.add_argument( + "--output", + type=Path, + help="new output directory (default: /decoded_joints)", + ) + args = parser.parse_args() + episode = args.episode.resolve() + output = (args.output or episode / "decoded_joints").resolve() + counts = decode_episode(episode, output) + print(json.dumps({"output": str(output), "rows": counts}, indent=2)) + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/tg3_local_teleop/README.md b/tg3_local_teleop/README.md index 37b4e88..a9eebda 100644 --- a/tg3_local_teleop/README.md +++ b/tg3_local_teleop/README.md @@ -15,6 +15,8 @@ TS1P 同构臂 -> 天工 3.0 BrainCo Revo2 双灵巧手 -> /hric/robot/cmd_vel（左摇杆前后 / 右摇杆转向；50 Hz TwistStamped） -> 天工 3.0 HBWALK 行走 + -> /head/cmd（左 Z + 右摇杆上下；Head Pitch 位置模式） + -> 天工 3.0 头部相机上下视角 ``` ## 自动运行与操作 @@ -38,6 +40,13 @@ TS1P 同构臂 `0.8 rad/s`；死区 `0.2`，输出频率 `50 Hz`。 - xTELE 0.1.2 的摇杆数组顺序是“纵轴、横轴”；左纵轴映射到 `linear.x`，右横轴 映射到 `angular.z`。`TwistStamped.header.frame_id` 按二次开放文档设置为 `pelvis`。 +- 头部视角：遥操已启动且右摇杆纵轴先回中过一次后，按住左 Z，右摇杆上推使 + Head Pitch 向负值移动（抬头），下推使其向正值移动（低头）；不需要 R3。按 Z + 期间右摇杆水平转向会被屏蔽，避免调整视角时误转。控制只使用 `mode=0` 绝对 + 位置、`cur=1.0 A`，从 `/robot_state` 的真实 `name=2` 反馈起步，限制在 `-10°～+50°`、 + 最大 `0.2 rad/s`、加速度 `0.5 rad/s²`。摇杆回中、输入/反馈失联或头电机报错时停止 + 更新目标。检测到 `control_forward`、`patrol_agent`、`joystick_bridge` 等外部 + `/head/cmd` 实际消息后，本项目暂停头部输出，静默 1 秒并再次观察摇杆回中后才恢复。 - 当前安装的 xTELE 0.1.2 没有注册同侧右 `C+A` 组合，因此 `C+A` 不执行动作；不能 在不清楚厂商语义的情况下把它擅自绑定为状态切换。 - 同构臂当前的一维手部开合量以机器人实测默认打开姿态为 0 端点，并按厂家 xTELE 的 @@ -73,6 +82,10 @@ HBWALK 状态等运行时保护仍持续检查。厂家 `freq_change_tg3_node` 或右摇杆横向越过死区时发布。摇杆回中后立即发布零速，并补发 10 帧零速。公网断线、退出遥操、 回 Home 或机器人退出 HBWALK 也走同一停止逻辑。 +头部控制只在同一次遥操武装期间启用，不改变 Head Yaw，也不改变厂家头部机械限位、 +电流、高温或碰撞保护。服务重启或新会话不会把软件累计角度当真实位置，必须重新读取 +`/robot_state` 的 Head Pitch 反馈。 + 灵巧手仅在同一次长按武装后发布；以机器人真实手指位置为起点，并按每秒最多 400 个 位置单位平滑跟随。BrainCo 状态 `0`（空闲）、`1`（运动）、`2`（接触/堵转或到限位） 和 `3`（持续力）均为厂家定义的正常运行状态；仅状态失联、未知/非法状态 @@ -166,13 +179,12 @@ OmniSocket 进程；在此之前桥会保持最后一帧目标。未 START 或 项目自有 `tg3-data-recorder.service` 在 Nvidia 上运行独立 `ros2 bag record`，不停止、 重配或接管 Ubuntu 厂家 `/record_bag_node`。仅录 `config.toml` 中的明确白名单，包含 `/robot_state`、双臂/BrainCo 双手命令与反馈、HBWALK、IMU、电源状态、完整 xTELE -应用帧，以及头/腰 Orbbec 的 RGB、深度、内参和元数据；仍禁止改成 `-a` 或录点云。 +应用帧，以及头部 Orbbec 的 RGB、深度、内参和元数据；仍禁止改成 `-a` 或录点云。 为避免 1280x720 原始 RGB+深度每台相机约 `8.3 GB/min` 的持续写盘负载，默认录制 `color/image_raw/compressed`（JPEG）和 `depth/image_raw/compressedDepth`（16UC1 -无损 PNG）。现场单台腰部相机实测合计约 `22–25 MB/s`（约 `1.3–1.5 GB/min`，随 -场景变化）。头、腰两组相机都不是 required topics：服务在运行就由 rosbag 自动发现并 -采集，未运行不阻塞 L3 或核心 READY。manifest 对每组记录 `absent`、`healthy`、 +无损 PNG）。头部相机不是 required topic：服务在运行就由 rosbag 自动发现并 +采集，未运行不阻塞 L3 或核心 READY。manifest 记录 `absent`、`healthy`、 `partial` 或 `low_rate`，并保存各话题消息数和图像平均频率；相机异常会产生明确质量 警告，但不会丢弃其余机器人控制数据。静态 TF 也尽力采集，不作为启动门槛。 @@ -187,7 +199,7 @@ OmniSocket 进程；在此之前桥会保持最后一帧目标。未 START 或 ``` MCAP 使用 `zstd_fast`、64 MiB cache、每 300 秒分片；每条 episode 最长 30 分钟， -加入双 RGB-D 后启动/运行最低保留 100 GiB。正常停止以 SIGINT 让 rosbag 写完 +启动/运行最低保留 100 GiB。正常停止以 SIGINT 让 rosbag 写完 `metadata.yaml`，随后执行 `ros2 bag info`，并确认所有 required topics 都有非零消息数；然后计算 SHA-256、写 `manifest.json` 和 `READY`，最后才原子进入 `ready/`。Nvidia 需已安装 `python3-yaml` @@ -209,7 +221,8 @@ sudo systemctl enable --now orbbec_head.service ``` 录制服务只加载厂家完整 `/opt/robot_tele_server/install` 消息 overlay，不加载本项目 -`ros2_py/install`；项目 overlay 仅供桥的 Python `ArmStatus` 绑定使用。两者混在同一个 +`ros2_py/install`；项目 overlay 仅供遥操桥的 Python 消息绑定使用 +（包括 `ArmStatus`/`RobotState`/`HeadCtrl`，但不是厂家完整包）。两者混在同一个 rosbag 环境会让部分 `ros2_bridge_msgs` C++ 类型支持出现动态库符号冲突。 部署两个机器人侧服务： diff --git a/tg3_local_teleop/config.toml b/tg3_local_teleop/config.toml index 617232c..40907fc 100644 --- a/tg3_local_teleop/config.toml +++ b/tg3_local_teleop/config.toml @@ -84,8 +84,8 @@ max_duration_seconds = 1800.0 # Explicit data whitelist; never replace this with `-a` on the live robot. # RGB is recorded through image_transport JPEG and 16UC1 depth through its -# lossless compressedDepth PNG transport. Both RGB-D cameras are optional: -# rosbag discovers and records either one whenever its service is running. +# lossless compressedDepth PNG transport. Only the head RGB-D camera is in the +# whitelist. It is optional and is discovered whenever its service is running. # /robot_state is the authoritative measured robot state. topics = [ "/robot_state", @@ -102,6 +102,7 @@ topics = [ "/hric/robot/cmd_vel", "/hric/robot/cmd_vel_status", "/hric/robot/rl_state", + "/head/cmd", "/imu_data", "/power/board/key_status", "/power/board/status", @@ -112,12 +113,6 @@ topics = [ "/ob_camera_head/depth/image_raw/compressedDepth", "/ob_camera_head/depth/camera_info", "/ob_camera_head/depth/metadata", - "/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", "/tf_static", "/tg3/data_collection/control", "/tg3/data_collection/status", @@ -157,20 +152,6 @@ topics = [ "/ob_camera_head/color/image_raw/compressed" = 20.0 "/ob_camera_head/depth/image_raw/compressedDepth" = 20.0 -[data_collection.optional_topic_groups.waist_rgbd] -topics = [ - "/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", -] - -[data_collection.optional_topic_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 - [locomotion] # Once Z+C has armed teleoperation and both sticks have been observed neutral, # left-stick vertical directly controls translation and right-stick horizontal @@ -196,6 +177,39 @@ forward_axis_sign = 1.0 yaw_axis_sign = -1.0 zero_burst_frames = 10 +[head] +# Camera-pitch mode: after Z+C arms teleoperation and the right-stick vertical +# axis has been observed neutral once, hold left Z while pushing the right stick +# up/down. While Z is held, right-stick yaw is suppressed so camera adjustment +# cannot accidentally turn the robot. No R3 is used. +enabled = true +command_topic = "/head/cmd" +state_topic = "/robot_state" +pitch_motor_id = 2 +frame_id = "head" +mode = 0 +label = 151 +max_current_a = 1.0 +command_speed_rad_s = 0.2 + +# Documented mechanical range is -15..+58 deg. Keep the commissioned visual +# working range at -10..+50 deg and always start from fresh measured feedback. +min_pitch_rad = -0.1745329252 +max_pitch_rad = 0.872664626 +max_pitch_speed_rad_s = 0.2 +max_pitch_accel_rad_s2 = 0.5 +pitch_axis_sign = -1.0 +joystick_deadzone = 0.1 +max_integration_dt_s = 0.1 +input_timeout_s = 0.3 +feedback_timeout_s = 0.25 + +# Existing vendor nodes advertise /head/cmd publishers. They are allowed when +# idle, but any actual foreign message leases head control for this interval; +# our bridge resumes only after quiet plus another neutral-stick observation. +foreign_command_quiet_s = 1.0 +self_command_window_s = 0.25 + [hands] # TianGong 3.0 on this robot uses BrainCo Revo2 hands. Commands are published # only while the same physical 3-second teleoperation latch is armed. diff --git a/tg3_local_teleop/head_control.py b/tg3_local_teleop/head_control.py new file mode 100644 index 0000000..0e02322 --- /dev/null +++ b/tg3_local_teleop/head_control.py @@ -0,0 +1,226 @@ +#!/usr/bin/env python3 +"""Pure safety state for direct TS1P right-stick head-pitch control.""" + +from __future__ import annotations + +import math +from typing import Any, Mapping + + +def right_stick_vertical(data: Mapping[str, Any]) -> float | None: + """Return the TS1P right-stick vertical axis or fail closed.""" + + try: + joysticks = data["joystick"] + if not isinstance(joysticks, Mapping): + return None + right = joysticks["right"] + if not isinstance(right, (list, tuple)) or len(right) != 2: + return None + value = float(right[0]) + if not math.isfinite(value) or abs(value) > 1.2: + return None + return max(-1.0, min(1.0, value)) + except (KeyError, TypeError, ValueError): + return None + + +def left_z_pressed(data: Mapping[str, Any]) -> bool | None: + """Return the physical left Z state, accepting only explicit binary data.""" + + try: + buttons = data["button"] + if not isinstance(buttons, Mapping): + return None + left = buttons["left"] + if not isinstance(left, (list, tuple)) or len(left) < 3: + return None + raw = left[2] + if isinstance(raw, bool): + return raw + if isinstance(raw, int) and not isinstance(raw, bool) and raw in (0, 1): + return bool(raw) + return None + except (KeyError, TypeError): + return None + + +def head_pitch_command_axis(data: Mapping[str, Any]) -> float | None: + """Gate right-stick pitch behind left Z; malformed input fails closed.""" + + pressed = left_z_pressed(data) + axis = right_stick_vertical(data) + if pressed is None or axis is None: + return None + return axis if pressed else 0.0 + + +class HeadPitchController: + """Integrate a joystick velocity request into a bounded pitch target.""" + + def __init__( + self, + *, + deadzone: float, + max_speed_rad_s: float, + max_accel_rad_s2: float, + min_pitch_rad: float, + max_pitch_rad: float, + axis_sign: float, + max_dt_s: float = 0.1, + ) -> None: + values = ( + deadzone, + max_speed_rad_s, + max_accel_rad_s2, + min_pitch_rad, + max_pitch_rad, + axis_sign, + max_dt_s, + ) + if not all(math.isfinite(value) for value in values): + raise ValueError("head-pitch settings must be finite") + if not 0.0 <= deadzone < 1.0: + raise ValueError("head-pitch deadzone must be in [0, 1)") + if max_speed_rad_s <= 0.0 or max_accel_rad_s2 <= 0.0: + raise ValueError("head-pitch speed and acceleration must be positive") + if min_pitch_rad >= max_pitch_rad: + raise ValueError("head-pitch range is invalid") + if axis_sign == 0.0 or max_dt_s <= 0.0: + raise ValueError("head-pitch sign and maximum dt must be non-zero") + self.deadzone = float(deadzone) + self.max_speed_rad_s = float(max_speed_rad_s) + self.max_accel_rad_s2 = float(max_accel_rad_s2) + self.min_pitch_rad = float(min_pitch_rad) + self.max_pitch_rad = float(max_pitch_rad) + self.axis_sign = math.copysign(1.0, axis_sign) + self.max_dt_s = float(max_dt_s) + self.armed = False + self.require_neutral = True + self.active = False + self.target_rad: float | None = None + self.velocity_rad_s = 0.0 + self.last_update_at: float | None = None + self.last_transition = "initialized" + + def new_session(self) -> None: + self.armed = True + self.require_neutral = True + self.active = False + self.target_rad = None + self.velocity_rad_s = 0.0 + self.last_update_at = None + self.last_transition = "new_session" + + def disarm(self, reason: str = "disarmed") -> None: + self.armed = False + self.require_neutral = True + self.active = False + self.target_rad = None + self.velocity_rad_s = 0.0 + self.last_update_at = None + self.last_transition = reason + + def _shaped_axis(self, axis: float) -> float: + magnitude = abs(axis) + if magnitude <= self.deadzone: + return 0.0 + normalized = (magnitude - self.deadzone) / (1.0 - self.deadzone) + return math.copysign(normalized, axis) + + def update( + self, + now: float, + *, + armed: bool, + input_healthy: bool, + feedback_healthy: bool, + external_busy: bool, + axis: float | None, + feedback_position_rad: float | None, + ) -> float | None: + """Return one absolute pitch target, or ``None`` when output is gated.""" + + if not math.isfinite(now): + raise ValueError("head-pitch clock must be finite") + if not armed: + if self.armed: + self.disarm() + return None + if not self.armed: + self.new_session() + + feedback_valid = ( + feedback_position_rad is not None + and math.isfinite(float(feedback_position_rad)) + ) + if ( + not input_healthy + or not feedback_healthy + or external_busy + or axis is None + or not feedback_valid + ): + self.require_neutral = True + self.active = False + self.target_rad = None + self.velocity_rad_s = 0.0 + self.last_update_at = now + if external_busy: + self.last_transition = "external_command_busy" + elif not feedback_healthy or not feedback_valid: + self.last_transition = "feedback_unhealthy" + else: + self.last_transition = "input_unhealthy" + return None + + feedback = float(feedback_position_rad) + shaped = self._shaped_axis(float(axis)) + if self.require_neutral: + self.active = False + self.target_rad = feedback + self.velocity_rad_s = 0.0 + self.last_update_at = now + if shaped == 0.0: + self.require_neutral = False + self.last_transition = "neutral_ready" + return None + + if shaped == 0.0: + self.active = False + self.target_rad = feedback + self.velocity_rad_s = 0.0 + self.last_update_at = now + self.last_transition = "neutral" + return None + + if self.target_rad is None: + self.target_rad = feedback + dt = 0.0 + if self.last_update_at is not None: + dt = min(self.max_dt_s, max(0.0, now - self.last_update_at)) + self.last_update_at = now + desired_velocity = shaped * self.axis_sign * self.max_speed_rad_s + max_velocity_step = self.max_accel_rad_s2 * dt + self.velocity_rad_s += max( + -max_velocity_step, + min(max_velocity_step, desired_velocity - self.velocity_rad_s), + ) + self.target_rad = max( + self.min_pitch_rad, + min( + self.max_pitch_rad, + self.target_rad + self.velocity_rad_s * dt, + ), + ) + self.active = True + self.last_transition = "active" + return self.target_rad + + +__all__ = [ + "HeadPitchController", + "head_pitch_command_axis", + "left_z_pressed", + "right_stick_vertical", +] diff --git a/tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/CMakeLists.txt b/tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/CMakeLists.txt index d388987..0aa7ddb 100644 --- a/tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/CMakeLists.txt +++ b/tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/CMakeLists.txt @@ -7,7 +7,14 @@ find_package(std_msgs REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "msg/MotorStatus.msg" + "msg/MotorCtrl.msg" "msg/ArmStatus.msg" + "msg/HeadCtrl.msg" + "msg/HeadStatus.msg" + "msg/WaistStatus.msg" + "msg/LegStatus.msg" + "msg/ImuStatus.msg" + "msg/RobotState.msg" DEPENDENCIES std_msgs ) diff --git a/tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/HeadCtrl.msg b/tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/HeadCtrl.msg new file mode 100644 index 0000000..ab5a351 --- /dev/null +++ b/tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/HeadCtrl.msg @@ -0,0 +1,5 @@ +std_msgs/Header header +uint8 mode +uint8 label +uint8 reserved +ros2_bridge_msgs/MotorCtrl[] ctrl diff --git a/tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/HeadStatus.msg b/tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/HeadStatus.msg new file mode 100644 index 0000000..6e6e77e --- /dev/null +++ b/tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/HeadStatus.msg @@ -0,0 +1,4 @@ +std_msgs/Header header +uint8 label +uint8 reserved +ros2_bridge_msgs/MotorStatus[] status diff --git a/tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/ImuStatus.msg b/tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/ImuStatus.msg new file mode 100644 index 0000000..b7c95a3 --- /dev/null +++ b/tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/ImuStatus.msg @@ -0,0 +1,20 @@ +std_msgs/Header header +uint8 label +uint8 reserved +float64 qx +float64 qy +float64 qz +float64 qw +float64 roll +float64 pitch +float64 yaw +float64 wx +float64 wy +float64 wz +float64 ax +float64 ay +float64 az +float64[9] orientation_covariance +float64[9] angular_velocity_covariance +float64[9] linear_acceleration_covariance +uint32 error diff --git a/tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/LegStatus.msg b/tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/LegStatus.msg new file mode 100644 index 0000000..6e6e77e --- /dev/null +++ b/tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/LegStatus.msg @@ -0,0 +1,4 @@ +std_msgs/Header header +uint8 label +uint8 reserved +ros2_bridge_msgs/MotorStatus[] status diff --git a/tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/MotorCtrl.msg b/tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/MotorCtrl.msg new file mode 100644 index 0000000..19cdb6d --- /dev/null +++ b/tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/MotorCtrl.msg @@ -0,0 +1,8 @@ +uint16 name +float64 kp +float64 kd +float64 pos +float64 spd +float64 tor +float64 cur +string joint_ids diff --git a/tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/RobotState.msg b/tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/RobotState.msg new file mode 100644 index 0000000..dae5e92 --- /dev/null +++ b/tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/RobotState.msg @@ -0,0 +1,6 @@ +std_msgs/Header header +ros2_bridge_msgs/HeadStatus head +ros2_bridge_msgs/WaistStatus waist +ros2_bridge_msgs/LegStatus leg +ros2_bridge_msgs/ArmStatus arm +ros2_bridge_msgs/ImuStatus imu diff --git a/tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/WaistStatus.msg b/tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/WaistStatus.msg new file mode 100644 index 0000000..6e6e77e --- /dev/null +++ b/tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/WaistStatus.msg @@ -0,0 +1,4 @@ +std_msgs/Header header +uint8 label +uint8 reserved +ros2_bridge_msgs/MotorStatus[] status diff --git a/tg3_local_teleop/test_head_control.py b/tg3_local_teleop/test_head_control.py new file mode 100644 index 0000000..4742a00 --- /dev/null +++ b/tg3_local_teleop/test_head_control.py @@ -0,0 +1,190 @@ +#!/usr/bin/env python3 +from __future__ import annotations + +import unittest + +from head_control import ( + HeadPitchController, + head_pitch_command_axis, + left_z_pressed, + right_stick_vertical, +) + + +def controller() -> HeadPitchController: + return HeadPitchController( + deadzone=0.1, + max_speed_rad_s=0.2, + max_accel_rad_s2=0.5, + min_pitch_rad=-0.1745329252, + max_pitch_rad=0.872664626, + axis_sign=-1.0, + max_dt_s=0.1, + ) + + +class RightStickParserTest(unittest.TestCase): + def test_accepts_only_finite_two_axis_right_stick(self) -> None: + self.assertEqual( + right_stick_vertical({"joystick": {"right": [0.5, -0.2]}}), + 0.5, + ) + self.assertIsNone(right_stick_vertical({"joystick": {"right": [0.5]}})) + self.assertIsNone( + right_stick_vertical({"joystick": {"right": [float("nan"), 0.0]}}) + ) + self.assertIsNone(right_stick_vertical({"joystick": {"right": [2.0, 0.0]}})) + + def test_left_z_strictly_gates_head_pitch_axis(self) -> None: + released = { + "button": {"left": [0, 0, 0]}, + "joystick": {"right": [0.8, 0.1]}, + } + pressed = { + "button": {"left": [0, 0, 1]}, + "joystick": {"right": [0.8, 0.1]}, + } + self.assertFalse(left_z_pressed(released)) + self.assertEqual(head_pitch_command_axis(released), 0.0) + self.assertTrue(left_z_pressed(pressed)) + self.assertEqual(head_pitch_command_axis(pressed), 0.8) + self.assertIsNone( + head_pitch_command_axis( + { + "button": {"left": [0, 0, "pressed"]}, + "joystick": {"right": [0.8, 0.1]}, + } + ) + ) + + +class HeadPitchControllerTest(unittest.TestCase): + def test_new_session_requires_neutral_before_direct_motion(self) -> None: + head = controller() + head.new_session() + self.assertIsNone( + head.update( + 0.0, + armed=True, + input_healthy=True, + feedback_healthy=True, + external_busy=False, + axis=1.0, + feedback_position_rad=0.1, + ) + ) + self.assertTrue(head.require_neutral) + head.update( + 0.1, + armed=True, + input_healthy=True, + feedback_healthy=True, + external_busy=False, + axis=0.0, + feedback_position_rad=0.1, + ) + self.assertFalse(head.require_neutral) + target = head.update( + 0.2, + armed=True, + input_healthy=True, + feedback_healthy=True, + external_busy=False, + axis=1.0, + feedback_position_rad=0.1, + ) + self.assertIsNotNone(target) + self.assertLess(target, 0.1) # stick up -> negative pitch -> view up + + def test_center_stops_and_resynchronizes_to_real_feedback(self) -> None: + head = controller() + head.new_session() + head.update( + 0.0, + armed=True, + input_healthy=True, + feedback_healthy=True, + external_busy=False, + axis=0.0, + feedback_position_rad=0.2, + ) + head.update( + 0.1, + armed=True, + input_healthy=True, + feedback_healthy=True, + external_busy=False, + axis=-1.0, + feedback_position_rad=0.2, + ) + self.assertTrue(head.active) + self.assertIsNone( + head.update( + 0.2, + armed=True, + input_healthy=True, + feedback_healthy=True, + external_busy=False, + axis=0.0, + feedback_position_rad=0.25, + ) + ) + self.assertFalse(head.active) + self.assertEqual(head.target_rad, 0.25) + self.assertEqual(head.velocity_rad_s, 0.0) + + def test_feedback_gap_or_external_command_fails_closed(self) -> None: + for feedback_healthy, external_busy in ((False, False), (True, True)): + head = controller() + head.new_session() + head.update( + 0.0, + armed=True, + input_healthy=True, + feedback_healthy=True, + external_busy=False, + axis=0.0, + feedback_position_rad=0.0, + ) + self.assertIsNone( + head.update( + 0.1, + armed=True, + input_healthy=True, + feedback_healthy=feedback_healthy, + external_busy=external_busy, + axis=1.0, + feedback_position_rad=0.0, + ) + ) + self.assertTrue(head.require_neutral) + self.assertFalse(head.active) + + def test_target_is_clamped_to_software_working_range(self) -> None: + head = controller() + head.new_session() + head.update( + 0.0, + armed=True, + input_healthy=True, + feedback_healthy=True, + external_busy=False, + axis=0.0, + feedback_position_rad=-0.17, + ) + target = None + for index in range(1, 200): + target = head.update( + index * 0.1, + armed=True, + input_healthy=True, + feedback_healthy=True, + external_busy=False, + axis=1.0, + feedback_position_rad=-0.17, + ) + self.assertEqual(target, head.min_pitch_rad) + + +if __name__ == "__main__": + unittest.main() diff --git a/tg3_local_teleop/test_idle_session_refresh.py b/tg3_local_teleop/test_idle_session_refresh.py index e2eff11..5387fff 100644 --- a/tg3_local_teleop/test_idle_session_refresh.py +++ b/tg3_local_teleop/test_idle_session_refresh.py @@ -31,6 +31,9 @@ for package, message_name in ( ("diagnostic_msgs.msg", "DiagnosticStatus"), ("geometry_msgs.msg", "TwistStamped"), ("ros2_bridge_msgs.msg", "ArmStatus"), + ("ros2_bridge_msgs.msg", "HeadCtrl"), + ("ros2_bridge_msgs.msg", "MotorCtrl"), + ("ros2_bridge_msgs.msg", "RobotState"), ("sensor_msgs.msg", "JointState"), ("std_srvs.srv", "Trigger"), ): diff --git a/tg3_local_teleop/test_session_gate.py b/tg3_local_teleop/test_session_gate.py index 698fbc2..970350b 100644 --- a/tg3_local_teleop/test_session_gate.py +++ b/tg3_local_teleop/test_session_gate.py @@ -28,7 +28,7 @@ for package, names in { "brainco_hand_msgs.msg": ("MotorStatus", "SetMotorMulti"), "diagnostic_msgs.msg": ("DiagnosticStatus",), "geometry_msgs.msg": ("TwistStamped",), - "ros2_bridge_msgs.msg": ("ArmStatus",), + "ros2_bridge_msgs.msg": ("ArmStatus", "HeadCtrl", "MotorCtrl", "RobotState"), "sensor_msgs.msg": ("JointState",), "std_msgs.msg": ("String",), "std_srvs.srv": ("Trigger",), @@ -83,6 +83,7 @@ class RobotSessionGateTest(unittest.TestCase): bridge.hands_enabled = False bridge.right_point_gesture_enabled = False bridge.right_a_pose_enabled = False + bridge.head_enabled = False bridge.robot_arm_positions = [0.0] * 14 bridge.last_command = None bridge.last_publish_at = 0.0 @@ -307,6 +308,22 @@ class RobotSessionGateTest(unittest.TestCase): bridge._tick_locomotion(1.3, turning) self.assertEqual(published[-1], (0.0, -0.8)) + head_mode_with_sideways_error = ArmSnapshot( + { + "button": { + "left": [False, False, True], + "right": [False, False, False], + }, + "joystick": { + "left": [0.0, 0.0], + "right": [0.8, 1.0], + }, + }, + received_at=1.35, + ) + bridge._tick_locomotion(1.35, head_mode_with_sideways_error) + self.assertEqual(published[-1], (0.0, 0.0)) + old_wrong_binding = ArmSnapshot( { "button": { @@ -351,11 +368,17 @@ class RobotSessionGateTest(unittest.TestCase): bridge._publish_walk = MethodType(publish, bridge) deflected = ArmSnapshot( - {"joystick": {"left": [1.0, 0.0], "right": [0.0, 0.0]}}, + { + "button": {"left": [0, 0, 0]}, + "joystick": {"left": [1.0, 0.0], "right": [0.0, 0.0]}, + }, received_at=2.0, ) neutral = ArmSnapshot( - {"joystick": {"left": [0.0, 0.0], "right": [0.0, 0.0]}}, + { + "button": {"left": [0, 0, 0]}, + "joystick": {"left": [0.0, 0.0], "right": [0.0, 0.0]}, + }, received_at=2.1, ) bridge._tick_locomotion(2.0, deflected) diff --git a/tg3_local_teleop/tg3_local_teleop.py b/tg3_local_teleop/tg3_local_teleop.py index d139677..8410afd 100755 --- a/tg3_local_teleop/tg3_local_teleop.py +++ b/tg3_local_teleop/tg3_local_teleop.py @@ -27,7 +27,7 @@ from brainco_hand_msgs.msg import MotorStatus, SetMotorMulti from diagnostic_msgs.msg import DiagnosticStatus from geometry_msgs.msg import TwistStamped from rclpy.node import Node -from ros2_bridge_msgs.msg import ArmStatus +from ros2_bridge_msgs.msg import ArmStatus, HeadCtrl, MotorCtrl, RobotState from sensor_msgs.msg import JointState from std_msgs.msg import String from std_srvs.srv import Trigger @@ -41,6 +41,7 @@ from gesture_toggle import ( right_b_pressed, select_hand_target, ) +from head_control import HeadPitchController, head_pitch_command_axis, left_z_pressed JOINT_NAMES = [ @@ -453,6 +454,25 @@ class LocalTeleopBridge(Node): self.locomotion_enabled = bool( self.locomotion_cfg.get("enabled", False) ) + self.head_cfg = config.get("head", {}) + self.head_enabled = bool(self.head_cfg.get("enabled", False)) + self.head_pitch_controller = HeadPitchController( + deadzone=float(self.head_cfg.get("joystick_deadzone", 0.1)), + max_speed_rad_s=float( + self.head_cfg.get("max_pitch_speed_rad_s", 0.2) + ), + max_accel_rad_s2=float( + self.head_cfg.get("max_pitch_accel_rad_s2", 0.5) + ), + min_pitch_rad=float( + self.head_cfg.get("min_pitch_rad", -0.1745329252) + ), + max_pitch_rad=float( + self.head_cfg.get("max_pitch_rad", 0.872664626) + ), + axis_sign=float(self.head_cfg.get("pitch_axis_sign", -1.0)), + max_dt_s=float(self.head_cfg.get("max_integration_dt_s", 0.1)), + ) self.data_collection_cfg = config.get("data_collection", {}) self.data_collection_enabled = bool( self.data_collection_cfg.get("enabled", False) @@ -504,6 +524,17 @@ class LocalTeleopBridge(Node): self.walk_zero_frames_remaining = 0 self.walk_require_neutral = True self.last_walk_publish_at = 0.0 + self.head_publisher = None + self.robot_head_positions: dict[int, float] = {} + self.robot_head_errors: dict[int, int] = {} + self.robot_head_at = 0.0 + self.expected_head_messages: list[tuple[tuple[Any, ...], float]] = [] + self.foreign_head_command_at = 0.0 + self.foreign_head_command_count = 0 + self.head_publish_count = 0 + self.last_head_publish_at = 0.0 + self.head_output_reasons: list[str] = [] + self.head_z_pressed: bool | None = None self.publisher = self.create_publisher(JointState, ros_cfg["command_topic"], 10) self.walk_publisher = None @@ -511,6 +542,22 @@ class LocalTeleopBridge(Node): self.walk_publisher = self.create_publisher( TwistStamped, self.locomotion_cfg["command_topic"], 10 ) + if self.head_enabled: + self.head_publisher = self.create_publisher( + HeadCtrl, self.head_cfg["command_topic"], 10 + ) + self.create_subscription( + RobotState, + self.head_cfg["state_topic"], + self._on_robot_state, + 10, + ) + self.create_subscription( + HeadCtrl, + self.head_cfg["command_topic"], + self._on_head_command, + 10, + ) self.create_subscription( DiagnosticStatus, ros_cfg["rl_state_topic"], self._on_rl_state, 10 ) @@ -597,6 +644,7 @@ class LocalTeleopBridge(Node): f"local bridge started in {mode}; source={self.source.description}; " f"target={ros_cfg['command_topic']}; brainco_hands={self.hands_enabled}; " f"locomotion={self.locomotion_enabled}; " + f"head_pitch={self.head_enabled}; " f"data_collection={self.data_collection_enabled}" ) @@ -620,6 +668,20 @@ class LocalTeleopBridge(Node): self.robot_arm_errors = [int(by_id[motor_id].error) for motor_id in MOTOR_IDS] self.robot_arm_at = time.monotonic() + def _on_robot_state(self, msg: RobotState) -> None: + by_id = {int(motor.name): motor for motor in msg.head.status} + pitch_id = int(self.head_cfg.get("pitch_motor_id", 2)) + if pitch_id not in by_id: + return + positions = {motor_id: float(motor.pos) for motor_id, motor in by_id.items()} + if not all(math.isfinite(value) for value in positions.values()): + return + self.robot_head_positions = positions + self.robot_head_errors = { + motor_id: int(motor.error) for motor_id, motor in by_id.items() + } + self.robot_head_at = time.monotonic() + def _on_hand_status(self, side: str, msg: MotorStatus) -> None: positions = [int(value) for value in msg.positions] states = [int(value) for value in msg.states] @@ -899,6 +961,45 @@ class LocalTeleopBridge(Node): "foreign BrainCo hand command source detected", success=False ) + @staticmethod + def _head_message_signature(msg: HeadCtrl) -> tuple[Any, ...]: + return ( + int(msg.mode), + int(msg.label), + int(msg.reserved), + tuple( + ( + int(motor.name), + float(motor.kp), + float(motor.kd), + float(motor.pos), + float(motor.spd), + float(motor.tor), + float(motor.cur), + str(motor.joint_ids), + ) + for motor in msg.ctrl + ), + ) + + def _on_head_command(self, msg: HeadCtrl) -> None: + now = time.monotonic() + window = float(self.head_cfg.get("self_command_window_s", 0.25)) + self.expected_head_messages = [ + expected + for expected in self.expected_head_messages + if now - expected[1] <= window + ] + signature = self._head_message_signature(msg) + if any(signature == expected[0] for expected in self.expected_head_messages): + return + self.foreign_head_command_at = now + self.foreign_head_command_count += 1 + if self.head_pitch_controller.active: + self.get_logger().warning( + "external /head/cmd detected; local head-pitch output paused" + ) + def _on_command_topic(self, msg: JointState) -> None: if msg.header.frame_id != self.FRAME_ID and len(msg.position) in (14, 16): if not self.foreign_source_seen: @@ -1001,6 +1102,8 @@ class LocalTeleopBridge(Node): # decision, so a matching STOP or safety teardown always wins over an # L3 press observed in the same control tick. self._tick_data_collection(now, sample) + if self.head_enabled: + self._tick_head(now, sample) if self.right_point_gesture_enabled: gesture_toggled = self.right_point_gesture.update( @@ -1288,6 +1391,8 @@ class LocalTeleopBridge(Node): self.right_point_gesture.new_session() if self.right_a_pose_enabled: self.right_a_pose.new_session() + if self.head_enabled: + self.head_pitch_controller.new_session() if getattr(self, "data_collection_enabled", False): self.data_capture_session_id = session_id self.data_collection_gate.new_session() @@ -1360,6 +1465,8 @@ class LocalTeleopBridge(Node): self.right_point_gesture.new_session() if self.right_a_pose_enabled: self.right_a_pose.new_session() + if self.head_enabled: + self.head_pitch_controller.new_session() if getattr(self, "data_collection_enabled", False): self.data_capture_session_id = "direct_" + uuid.uuid4().hex self.data_collection_gate.new_session() @@ -1398,6 +1505,8 @@ class LocalTeleopBridge(Node): self.right_point_gesture.disarm(reason) if self.right_a_pose_enabled: self.right_a_pose.disarm(reason) + if self.head_enabled: + self.head_pitch_controller.disarm(reason) self.armed = False self.hand_output_ready = False self.runtime_hand_output_reasons = [] @@ -1438,6 +1547,9 @@ class LocalTeleopBridge(Node): # convention requested for this installation. raw_forward = float(left_joystick[0]) raw_yaw = float(right_joystick[1]) + head_mode = left_z_pressed(sample.data) + if head_mode is None: + raise ValueError("left Z state is malformed") deadzone = float(cfg["joystick_deadzone"]) forward_expo = float(cfg["joystick_expo"]) yaw_expo = float(cfg.get("yaw_joystick_expo", forward_expo)) @@ -1447,6 +1559,11 @@ class LocalTeleopBridge(Node): shaped_yaw = self._shape_joystick_axis( raw_yaw, deadzone, yaw_expo ) + # Left Z reserves the entire right stick for head-pitch mode. + # Suppress yaw so a small sideways component while looking up or + # down cannot turn the robot. + if head_mode: + shaped_yaw = 0.0 signed_forward = shaped_forward * float( cfg.get("forward_axis_sign", 1.0) ) @@ -1533,6 +1650,96 @@ class LocalTeleopBridge(Node): self.last_walk_publish_at = now self.walk_publish_count += 1 + def _tick_head(self, now: float, sample: ArmSnapshot | None) -> None: + pitch_id = int(self.head_cfg.get("pitch_motor_id", 2)) + pitch_position = self.robot_head_positions.get(pitch_id) + feedback_timeout = float(self.head_cfg.get("feedback_timeout_s", 0.25)) + feedback_age = ( + math.inf if self.robot_head_at == 0.0 else now - self.robot_head_at + ) + feedback_healthy = ( + pitch_position is not None + and math.isfinite(float(pitch_position)) + and feedback_age <= feedback_timeout + and int(self.robot_head_errors.get(pitch_id, -1)) == 0 + ) + input_timeout = float(self.head_cfg.get("input_timeout_s", 0.3)) + input_healthy = ( + self.armed + and sample is not None + and now - sample.received_at <= input_timeout + ) + self.head_z_pressed = ( + None if sample is None else left_z_pressed(sample.data) + ) + axis = None if sample is None else head_pitch_command_axis(sample.data) + quiet_s = float(self.head_cfg.get("foreign_command_quiet_s", 1.0)) + foreign_age = ( + math.inf + if self.foreign_head_command_at == 0.0 + else now - self.foreign_head_command_at + ) + external_busy = foreign_age <= quiet_s + + reasons: list[str] = [] + if not self.armed: + reasons.append("teleoperation is not armed") + if not input_healthy: + reasons.append("right-stick input is unavailable/stale") + if axis is None: + reasons.append("left Z or right-stick vertical input is malformed") + elif not self.head_z_pressed: + reasons.append("left Z is not held; head-pitch control is idle") + if not feedback_healthy: + reasons.append("head pitch feedback is unavailable/stale or in error") + if external_busy: + reasons.append("external /head/cmd publisher has an active lease") + self.head_output_reasons = reasons + + target = self.head_pitch_controller.update( + now, + armed=self.armed, + input_healthy=input_healthy, + feedback_healthy=feedback_healthy, + external_busy=external_busy, + axis=axis, + feedback_position_rad=pitch_position, + ) + if target is not None: + self._publish_head_pitch(target, now) + + def _publish_head_pitch(self, target_rad: float, now: float) -> None: + if not self.allow_publish or self.head_publisher is None: + return + motor = MotorCtrl() + motor.name = int(self.head_cfg.get("pitch_motor_id", 2)) + motor.kp = 0.0 + motor.kd = 0.0 + motor.pos = float(target_rad) + motor.spd = float(self.head_cfg.get("command_speed_rad_s", 0.2)) + motor.tor = 0.0 + motor.cur = float(self.head_cfg.get("max_current_a", 1.0)) + motor.joint_ids = "" + + msg = HeadCtrl() + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = str(self.head_cfg.get("frame_id", "head")) + msg.mode = int(self.head_cfg.get("mode", 0)) + msg.label = int(self.head_cfg.get("label", 151)) + msg.reserved = 0 + msg.ctrl = [motor] + signature = self._head_message_signature(msg) + window = float(self.head_cfg.get("self_command_window_s", 0.25)) + self.expected_head_messages = [ + expected + for expected in self.expected_head_messages + if now - expected[1] <= window + ][-64:] + self.expected_head_messages.append((signature, now)) + self.head_publisher.publish(msg) + self.head_publish_count += 1 + self.last_head_publish_at = now + def _home_start_reasons( self, now: float, @@ -2111,6 +2318,42 @@ class LocalTeleopBridge(Node): "last_locomotion_publish_age_s": None if self.last_walk_publish_at == 0.0 else round(now - self.last_walk_publish_at, 4), + "head_pitch_enabled": self.head_enabled, + "head_pitch_binding": "left_Z + right_stick_vertical after neutral", + "head_pitch_left_z_pressed": self.head_z_pressed, + "head_pitch_active": self.head_pitch_controller.active, + "head_pitch_state": self.head_pitch_controller.last_transition, + "head_pitch_requires_neutral": ( + self.head_pitch_controller.require_neutral + ), + "head_pitch_target_rad": self.head_pitch_controller.target_rad, + "head_pitch_velocity_rad_s": ( + self.head_pitch_controller.velocity_rad_s + ), + "head_pitch_feedback_rad": self.robot_head_positions.get( + int(self.head_cfg.get("pitch_motor_id", 2)) + ), + "head_pitch_feedback_error": self.robot_head_errors.get( + int(self.head_cfg.get("pitch_motor_id", 2)) + ), + "head_pitch_feedback_age_s": None + if self.robot_head_at == 0.0 + else round(now - self.robot_head_at, 4), + "head_pitch_output_reasons": self.head_output_reasons, + "head_pitch_limits_rad": { + "up": self.head_pitch_controller.min_pitch_rad, + "down": self.head_pitch_controller.max_pitch_rad, + "max_speed": self.head_pitch_controller.max_speed_rad_s, + "max_accel": self.head_pitch_controller.max_accel_rad_s2, + }, + "head_pitch_foreign_command_age_s": None + if self.foreign_head_command_at == 0.0 + else round(now - self.foreign_head_command_at, 4), + "head_pitch_foreign_command_count": self.foreign_head_command_count, + "head_pitch_publish_count": self.head_publish_count, + "last_head_pitch_publish_age_s": None + if self.last_head_publish_at == 0.0 + else round(now - self.last_head_publish_at, 4), "unsupported_binding": ( "right_C + right_A (C+A) is not registered by xTELE 0.1.2" ), diff --git a/verify.sh b/verify.sh index cf96057..7bed374 100755 --- a/verify.sh +++ b/verify.sh @@ -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