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

25
tg3_local_teleop/README.md
View File

@@ -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++ 类型支持出现动态库符号冲突。
部署两个机器人侧服务：

58
tg3_local_teleop/config.toml
View File

@@ -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.

226
tg3_local_teleop/head_control.py Normal file
View File

@@ -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",
]

7
tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/CMakeLists.txt
View File

@@ -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
)

5
tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/HeadCtrl.msg Normal file
View File

@@ -0,0 +1,5 @@
std_msgs/Header header
uint8 mode
uint8 label
uint8 reserved
ros2_bridge_msgs/MotorCtrl[] ctrl

4
tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/HeadStatus.msg Normal file
View File

@@ -0,0 +1,4 @@
std_msgs/Header header
uint8 label
uint8 reserved
ros2_bridge_msgs/MotorStatus[] status

20
tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/ImuStatus.msg Normal file
View File

@@ -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

4
tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/LegStatus.msg Normal file
View File

@@ -0,0 +1,4 @@
std_msgs/Header header
uint8 label
uint8 reserved
ros2_bridge_msgs/MotorStatus[] status

8
tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/MotorCtrl.msg Normal file
View File

@@ -0,0 +1,8 @@
uint16 name
float64 kp
float64 kd
float64 pos
float64 spd
float64 tor
float64 cur
string joint_ids

6
tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/RobotState.msg Normal file
View File

@@ -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

4
tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/WaistStatus.msg Normal file
View File

@@ -0,0 +1,4 @@
std_msgs/Header header
uint8 label
uint8 reserved
ros2_bridge_msgs/MotorStatus[] status

190
tg3_local_teleop/test_head_control.py Normal file
View File

@@ -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()

3
tg3_local_teleop/test_idle_session_refresh.py
View File

@@ -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"),
):

29
tg3_local_teleop/test_session_gate.py
View File

@@ -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)

245
tg3_local_teleop/tg3_local_teleop.py
View File

@@ -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"
),