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

feat: harden TG3 teleop and add right-B point gesture

Browse Source
This commit is contained in:
LengedZhao
2026-08-08 15:51:21 +08:00
parent eab60273bc
commit 1a79fca36f
17 changed files with 1725 additions and 196 deletions
Download Patch File Download Diff File Expand all files Collapse all files

282
tg3_local_teleop/tg3_local_teleop.py
View File

@@ -30,6 +30,13 @@ from ros2_bridge_msgs.msg import ArmStatus
from sensor_msgs.msg import JointState
from std_srvs.srv import Trigger
from gesture_toggle import (
GestureToggle,
normalized_pose_to_positions,
right_b_pressed,
select_right_hand_target,
)
JOINT_NAMES = [
*(f"left_joints_{i}" for i in range(7)),
@@ -70,6 +77,10 @@ class LatestArmData:
self._metrics: dict[str, Any] = {
"transport": self.transport,
"connected": False,
"registered": False,
"session_connects": 0,
"idle_session_refreshes": 0,
"idle_session_refresh_failures": 0,
"frames_received": 0,
"frames_accepted": 0,
"dropped_sender": 0,
@@ -150,6 +161,11 @@ class LatestArmData:
expected_sender = str(self.cfg["omnisocket_expected_sender"])
max_age_ms = float(self.cfg["omnisocket_max_packet_age_ms"])
idle_refresh_s = float(
self.cfg.get("omnisocket_idle_session_refresh_s", 2.0)
)
if idle_refresh_s < 0.0:
raise ValueError("OmniSocket idle session refresh must be non-negative")
last_sequence = 0
while not self._stop.is_set():
session = Session()
@@ -160,13 +176,83 @@ class LatestArmData:
peer_id=str(self.cfg["omnisocket_peer_id"]),
**CONTROL_DEFAULTS,
)
last_accepted_at = time.monotonic()
session_stats = session.stats()
with self._lock:
self._metrics["connected"] = True
self._metrics["registered"] = bool(
int(session_stats.get("registered", 0)) == 1
)
self._metrics["session_connects"] += 1
self._last_error = ""
while not self._stop.is_set():
message = session.recv(timeout_ms=100)
if message is None:
now = time.monotonic()
if self._idle_session_refresh_due(
now, last_accepted_at, idle_refresh_s
):
# The deployed OmniSocket receiver has no idle Hub
# heartbeat. After a Hub restart it may therefore
# keep reporting connected although its server-side
# registration is gone. Refresh with make-before-
# break: register a replacement with the same peer
# ID first, then close the old instance. The Hub
# tracks registration instances, so closing the old
# session does not unregister the replacement and
# there is no healthy-idle routing gap.
replacement = Session()
try:
replacement.connect(
server_addr=str(
self.cfg["omnisocket_server"]
),
peer_id=str(
self.cfg["omnisocket_peer_id"]
),
**CONTROL_DEFAULTS,
)
replacement_stats = replacement.stats()
except Exception as exc:
try:
replacement.close()
except OSError:
pass
last_accepted_at = now
with self._lock:
self._metrics[
"idle_session_refresh_failures"
] += 1
self._last_error = (
"OmniSocket idle registration refresh "
f"failed: {exc}"
)
continue
old_session = session
session = replacement
baseline_ms = None
last_accepted_at = time.monotonic()
with self._lock:
self._metrics["connected"] = True
self._metrics["registered"] = bool(
int(
replacement_stats.get(
"registered", 0
)
)
== 1
)
self._metrics["session_connects"] += 1
self._metrics[
"idle_session_refreshes"
] += 1
self._last_error = ""
try:
old_session.close()
except OSError:
pass
continue
messages = [message]
while True:
@@ -229,9 +315,11 @@ class LatestArmData:
)
self._last_error = ""
self._metrics["frames_accepted"] += 1
last_accepted_at = time.monotonic()
except Exception as exc:
with self._lock:
self._metrics["connected"] = False
self._metrics["registered"] = False
self._last_error = f"OmniSocket connection failed: {exc}"
finally:
try:
@@ -240,8 +328,18 @@ class LatestArmData:
pass
with self._lock:
self._metrics["connected"] = False
self._metrics["registered"] = False
self._stop.wait(1.0)
@staticmethod
def _idle_session_refresh_due(
now: float, last_accepted_at: float, refresh_after_s: float
) -> bool:
return (
refresh_after_s > 0.0
and now - last_accepted_at >= refresh_after_s
)
def _decode_omni_packet(
self,
msg_type: int,
@@ -303,6 +401,27 @@ class LocalTeleopBridge(Node):
net_cfg = config["network"]
self.hands_cfg = config.get("hands", {})
self.hands_enabled = bool(self.hands_cfg.get("enabled", False))
self.right_point_gesture_enabled = self.hands_enabled and bool(
self.hands_cfg.get("right_b_point_gesture_enabled", False)
)
self.right_point_gesture = GestureToggle(
hold_seconds=float(
self.hands_cfg.get("right_b_point_gesture_hold_seconds", 3.0)
),
release_seconds=float(
self.hands_cfg.get("right_b_point_gesture_release_seconds", 0.5)
),
)
point_pose = self.hands_cfg.get(
"right_b_point_pose_normalized",
[0.2, 0.688, 0.0, 0.98, 0.98, 0.98],
)
self.right_point_gesture_pose = [float(value) for value in point_pose]
self.right_point_gesture_target = normalized_pose_to_positions(
self.right_point_gesture_pose,
int(self.hands_cfg.get("position_min", 1)),
int(self.hands_cfg.get("position_max", 1000)),
)
self.locomotion_cfg = config.get("locomotion", {})
self.locomotion_enabled = bool(
self.locomotion_cfg.get("enabled", False)
@@ -571,6 +690,26 @@ class LocalTeleopBridge(Node):
success=False,
)
if self.right_point_gesture_enabled:
gesture_toggled = self.right_point_gesture.update(
now,
armed=self.armed,
input_healthy=(
self.armed
and sample is not None
and not runtime_hand_reasons
),
pressed=(
None if sample is None else right_b_pressed(sample.data)
),
)
if gesture_toggled:
state = "ACTIVE" if self.right_point_gesture.active else "INACTIVE"
self.get_logger().warning(
f"RIGHT-HAND POINT GESTURE {state}: right B held for "
f"{self.right_point_gesture.hold_seconds:.1f}s"
)
if self.returning_home:
self._tick_home(now)
elif self.armed and sample is not None:
@@ -600,6 +739,17 @@ class LocalTeleopBridge(Node):
)
self.hand_output_ready = True
desired_hands = self._hand_targets(sample.data)
if self.right_point_gesture_enabled:
freeze_reference = self.last_hand_commands["right"]
if freeze_reference is None or len(freeze_reference) != 6:
freeze_reference = self.robot_hand_positions["right"]
desired_hands["right"] = select_right_hand_target(
desired_hands["right"],
freeze_reference,
self.right_point_gesture_target,
active=self.right_point_gesture.active,
freeze=self.right_point_gesture.freeze_right_hand,
)
commands = {
side: self._slew_hand(side, desired_hands[side], now)
for side in HAND_SIDES
@@ -786,6 +936,8 @@ class LocalTeleopBridge(Node):
"new teleoperation START accepted", success=False, cancelled=True
)
self.active_session_id = session_id
if self.right_point_gesture_enabled:
self.right_point_gesture.new_session()
self.armed = True
# Start both slew limiters at measured robot feedback, never at a
# potentially distant first network target.
@@ -849,6 +1001,8 @@ class LocalTeleopBridge(Node):
if reasons:
self.get_logger().error("cannot arm: " + "; ".join(reasons))
return
if self.right_point_gesture_enabled:
self.right_point_gesture.new_session()
self.armed = True
# Start the slew limiter at measured robot feedback. Using None here
# would make the first armed frame jump directly to the TS1P target.
@@ -868,6 +1022,11 @@ class LocalTeleopBridge(Node):
def _disarm(self, reason: str) -> None:
was_armed = self.armed
self._stop_locomotion(reason)
if self.right_point_gesture_enabled:
# Clearing the logical override must not publish a hand target.
# Existing STOP behavior leaves the physical hand at its last
# limited command until a later, newly armed session.
self.right_point_gesture.disarm(reason)
self.armed = False
self.hand_output_ready = False
self.runtime_hand_output_reasons = []
@@ -898,20 +1057,29 @@ class LocalTeleopBridge(Node):
def _tick_locomotion(self, now: float, sample: ArmSnapshot) -> None:
cfg = self.locomotion_cfg
try:
buttons = sample.data["button"]["right"]
joystick = sample.data["joystick"]["left"]
right_c = len(buttons) >= 3 and bool(buttons[2])
if len(joystick) != 2:
raise ValueError("left joystick must contain x/y")
# TS1P reports the physical forward/back axis first and the
# left/right axis second. This was verified on the installed
# xTELE 0.1.2 stream; treating the pair as Cartesian x/y made a
# forward stick command become pure yaw.
raw_forward, raw_yaw = (float(joystick[0]), float(joystick[1]))
left_buttons = sample.data["button"]["left"]
right_buttons = sample.data["button"]["right"]
left_joystick = sample.data["joystick"]["left"]
right_joystick = sample.data["joystick"]["right"]
left_z = len(left_buttons) >= 3 and bool(left_buttons[2])
right_c = len(right_buttons) >= 3 and bool(right_buttons[2])
if len(left_joystick) != 2 or len(right_joystick) != 2:
raise ValueError("left and right joysticks must each contain two axes")
# xTELE 0.1.2 stores each TS1P stick as [vertical, horizontal].
# Use the left vertical axis for translation and the right
# horizontal axis for in-place yaw, matching the physical control
# convention requested for this installation.
raw_forward = float(left_joystick[0])
raw_yaw = float(right_joystick[1])
deadzone = float(cfg["joystick_deadzone"])
expo = float(cfg["joystick_expo"])
shaped_forward = self._shape_joystick_axis(raw_forward, deadzone, expo)
shaped_yaw = self._shape_joystick_axis(raw_yaw, deadzone, expo)
forward_expo = float(cfg["joystick_expo"])
yaw_expo = float(cfg.get("yaw_joystick_expo", forward_expo))
shaped_forward = self._shape_joystick_axis(
raw_forward, deadzone, forward_expo
)
shaped_yaw = self._shape_joystick_axis(
raw_yaw, deadzone, yaw_expo
)
signed_forward = shaped_forward * float(
cfg.get("forward_axis_sign", 1.0)
)
@@ -927,9 +1095,12 @@ class LocalTeleopBridge(Node):
self._stop_locomotion(f"invalid locomotion input: {exc}")
return
joystick_active = shaped_forward != 0.0 or shaped_yaw != 0.0
if not right_c or not joystick_active:
self._stop_locomotion("right C released or left joystick returned to center")
forward_active = right_c and shaped_forward != 0.0
yaw_active = left_z and shaped_yaw != 0.0
if not forward_active and not yaw_active:
self._stop_locomotion(
"right C + left forward and left Z + right yaw are both inactive"
)
self._tick_walk_zero_burst(now)
return
@@ -944,17 +1115,19 @@ class LocalTeleopBridge(Node):
self.walk_active = True
self.walk_zero_frames_remaining = 0
self.get_logger().warning(
"LOCAL HBWALK VELOCITY STARTED: immediate right C + left "
"joystick input"
"LOCAL HBWALK VELOCITY STARTED: right C + left-stick forward "
"or left Z + right-stick yaw"
)
linear_limit = max_forward if signed_forward >= 0.0 else max_reverse
linear_x = signed_forward * linear_limit
angular_z = (
shaped_yaw
* float(cfg.get("yaw_axis_sign", -1.0))
* max_angular
)
linear_x = signed_forward * linear_limit if forward_active else 0.0
angular_z = 0.0
if yaw_active:
angular_z = (
shaped_yaw
* float(cfg.get("yaw_axis_sign", -1.0))
* max_angular
)
self._publish_walk(linear_x, angular_z, now)
def _stop_locomotion(self, reason: str) -> None:
@@ -1432,8 +1605,62 @@ class LocalTeleopBridge(Node):
else round(now - self.last_hand_publish_at, 4),
"hand_publish_count": self.hand_publish_count,
"foreign_hand_source_seen": self.foreign_hand_source_seen,
"right_point_gesture_enabled": self.right_point_gesture_enabled,
"right_point_gesture_binding": (
"right_B hold "
f"{self.right_point_gesture.hold_seconds:.1f}s toggle"
),
"right_point_gesture_active": (
self.right_point_gesture.active
if self.right_point_gesture_enabled
else False
),
"right_point_gesture_state": (
self.right_point_gesture.state
if self.right_point_gesture_enabled
else "disabled"
),
"right_point_gesture_hold_s": round(
self.right_point_gesture.hold_elapsed(now), 2
),
"right_point_gesture_release_s": round(
self.right_point_gesture.release_elapsed(now), 2
),
"right_point_gesture_requires_release": (
self.right_point_gesture.require_release
if self.right_point_gesture_enabled
else False
),
"right_point_gesture_freezing_input": (
self.right_point_gesture.freeze_right_hand
if self.right_point_gesture_enabled
else False
),
"right_point_gesture_toggle_count": (
self.right_point_gesture.toggle_count
if self.right_point_gesture_enabled
else 0
),
"right_point_gesture_last_transition": (
self.right_point_gesture.last_transition
if self.right_point_gesture_enabled
else "disabled"
),
"right_point_gesture_target_normalized": (
self.right_point_gesture_pose
if self.right_point_gesture_enabled
else None
),
"right_point_gesture_target_positions": (
self.right_point_gesture_target
if self.right_point_gesture_enabled
else None
),
"locomotion_enabled": self.locomotion_enabled,
"locomotion_binding": "right_C + left_joystick, immediate",
"locomotion_binding": (
"right_C + left_stick_vertical; "
"left_Z + right_stick_horizontal, immediate"
),
"locomotion_active": self.walk_active,
"locomotion_hold_s": 0.0,
"locomotion_command": {
@@ -1444,6 +1671,11 @@ class LocalTeleopBridge(Node):
"forward_m_s": self.locomotion_cfg.get("max_forward_m_s"),
"reverse_m_s": self.locomotion_cfg.get("max_reverse_m_s"),
"angular_rad_s": self.locomotion_cfg.get("max_angular_rad_s"),
"forward_expo": self.locomotion_cfg.get("joystick_expo"),
"yaw_expo": self.locomotion_cfg.get(
"yaw_joystick_expo",
self.locomotion_cfg.get("joystick_expo"),
),
},
"locomotion_publish_count": self.walk_publish_count,
"locomotion_fsm_publish_enabled": False,