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

tg3_local_teleop/README.md

@@ -13,13 +13,15 @@ TS1P 同构臂
   -> 天工 3.0 双臂
   -> /left_hand/set_motor_multi + /right_hand/set_motor_multi
   -> 天工 3.0 BrainCo Revo2 双灵巧手
-  -> /hric/robot/cmd_vel（右 C + 左摇杆；50 Hz TwistStamped）
+  -> /hric/robot/cmd_vel（右 C + 左摇杆前后 / 左 Z + 右摇杆转向；50 Hz TwistStamped）
   -> 天工 3.0 HBWALK 行走
 ```
 
 ## 自动运行与操作
 
 - 桥接服务随机器人算力主机的用户服务自动启动；未进入 `HBWALK` 时只监测、不发布。
+- 用户服务启动前会用短生命周期 ROS 探针等待 `/hric/robot/rl_state` 可发现，再创建
+  长生命周期桥接节点，避免开机早期网络接口尚未就绪时 Fast DDS 固化为空接口。
 - 开始遥操：进入 `HBWALK` 后，同时长按左手 `Z` + 右手 `C` 3 秒。该计时在 EAI
   本机完成；计时未通过前不向 Hub 发送 xTELE 业务帧。
 - 结束遥操：再次同时长按 3 秒；EAI 发送最后一个匹配会话的 `STOP` 后停止业务数据，
@@ -27,18 +29,29 @@ TS1P 同构臂
 - 回 Home 过程中用新的 Z+C 3 秒会话可取消回位并重新启动遥操。
 - 本地桥控制左右各 7 个手臂关节、BrainCo Revo2 双灵巧手和 HBWALK
   前后/转向；不控制腰和头。
-- 行走：遥操已启动后，右 `C` 是即时 deadman；按住 C 并把左摇杆推出死区后，在下一个
-  `50 Hz` 周期立即响应，不再等待 3 秒。松开 C 或摇杆回中立即发零速，再按也立即恢复。
-  左摇杆上下控制前后，左右控制原地转向。
+- 行走：遥操已启动后，右 `C` + 左摇杆上下控制前后，左 `Z` + 右摇杆左右控制原地
+  转向；两个组合都在下一个 `50 Hz` 周期立即响应，不再等待 3 秒。松开对应按键或
+  摇杆回中立即把该轴清零，再按也立即恢复。
+  前后仍使用二次细控曲线；转向在死区后使用线性曲线，使右摇杆中段有足够角速度，
+  但最大值仍受官方 `0.8 rad/s` 上限约束。
   按官方半身行走 Topic 范围限幅：前进 `1.0 m/s`、后退 `0.8 m/s`、转向
   `0.8 rad/s`；死区 `0.2`，输出频率 `50 Hz`。
-- xTELE 0.1.2 的左摇杆原始数组顺序是“前后、左右”；前后映射到 `linear.x`，左右
+- xTELE 0.1.2 的摇杆数组顺序是“纵轴、横轴”；左纵轴映射到 `linear.x`，右横轴
   映射到 `angular.z`。`TwistStamped.header.frame_id` 按二次开放文档设置为 `pelvis`。
 - 当前安装的 xTELE 0.1.2 没有注册同侧右 `C+A` 组合，因此 `C+A` 不执行动作；不能
   在不清楚厂商语义的情况下把它擅自绑定为状态切换。
 - 同构臂当前的一维手部开合量以机器人实测默认打开姿态为 0 端点，并按厂家 xTELE 的
   BrainCoRevo2 保守“常规”抓握姿态作为 1 端点映射到六电机；
   若 xTELE 后续直接输出六维归一化位置，会自动使用六维位置。
+- 右手指向手势：遥操已启动且灵巧手反馈健康时，连续长按右 `B` 3 秒开启；松开
+  至少 `0.5 s` 后，再连续长按右 `B` 3 秒关闭。首次启动和每个新遥操会话都必须先
+  稳定松开 B，保持同一次按压不会反复切换。计时未满、中途松开、输入畸形或反馈
+  中断均不触发；计时期间冻结右手最后一条命令，左手和双臂仍照常跟随。
+- 指向只覆盖右手。目标采用工控机 xTELE `GestureController` 的 BrainCoRevo2 第 2 号
+  “单食指”手势 `state 0`：归一化目标
+  `[0.2, 0.688, 0.0, 0.98, 0.98, 0.98]`，对应当前 `1~1000` 位置范围约为
+  `[201, 688, 1, 980, 980, 980]`；顺序为大拇指弯曲、大拇指旋转、食指、中指、
+  无名指、小拇指。进入和退出手势都继续使用每秒最多 400 个位置单位的现有限速。
 
 ## 安全门控
 
@@ -53,7 +66,7 @@ HBWALK 状态等运行时保护仍持续检查。厂家 `freq_change_tg3_node`
 `angular.z=[-0.8, 0.8] rad/s`；本桥不发布侧移速度。
 
 行走速度只在同一次遥操武装、`HBWALK/HBWALK/running`、网络数据新鲜且右 C + 左摇杆
-即时成立时发布。解除组合后立即发布零速，并补发 10 帧零速。公网断线、退出遥操、
+前后或左 Z + 右摇杆横向即时成立时发布。解除组合后立即发布零速，并补发 10 帧零速。公网断线、退出遥操、
 回 Home 或机器人退出 HBWALK 也走同一停止逻辑。
 
 灵巧手仅在同一次长按武装后发布；以机器人真实手指位置为起点，并按每秒最多 400 个
@@ -61,6 +74,8 @@ HBWALK 状态等运行时保护仍持续检查。厂家 `freq_change_tg3_node`
 和 `3`（持续力）均为厂家定义的正常运行状态；仅状态失联、未知/非法状态、同构臂失联
 或检测到另一命令源时，双臂和双手会一起解除武装并停止发布。停止遥操后灵巧手保持
 最后目标，双臂按原逻辑限速回 Home。BrainCo 驱动原有的电流、堵转和碰撞保护不作修改。
+匹配的 STOP、安全解除武装或服务退出都会清除右 B 指向手势的逻辑状态，但不会在
+STOP 后额外发送张手/恢复命令；下一次会话从机器人实测手指位置重新限速跟随。
 
 ## 限速双臂回 Home
 

tg3_local_teleop/config.toml

@@ -5,6 +5,11 @@ omnisocket_server = "175.178.116.187:14049"
 omnisocket_peer_id = "tg3-009027fa8190-robot"
 omnisocket_expected_sender = "tg3-009027fa8190-iarm"
 omnisocket_max_packet_age_ms = 300.0
+# The deployed OmniSocket receiver does not exchange a Hub heartbeat while idle.
+# Re-register periodically with make-before-break so a Hub restart cannot leave
+# a false connected state and healthy refreshes never create a routing gap.
+# Active traffic resets this timer and is never interrupted by this refresh.
+omnisocket_idle_session_refresh_s = 30.0
 # The native session can remain blocked on an old connection after the public
 # Hub disappears. Publication stops at source_timeout_s; after this longer
 # interval the process exits and systemd creates a completely fresh session.
@@ -52,8 +57,9 @@ joint_upper_rad = [
 ]
 
 [locomotion]
-# Right C remains an immediate deadman: while teleoperation is active, C plus
-# a left-stick command acts on the next 50 Hz tick. There is no second hold.
+# The two original immediate bindings are independent: right C + left-stick
+# vertical controls translation; left Z + right-stick horizontal controls
+# in-place yaw. Both act on the next 50 Hz tick, with no second hold.
 enabled = true
 command_topic = "/hric/robot/cmd_vel"
 # This bridge never publishes FSM commands. The robot must already report
@@ -63,6 +69,10 @@ frame_id = "pelvis"
 hold_seconds = 0.0
 joystick_deadzone = 0.2
 joystick_expo = 2.0
+# Turning is linear after the deadzone so medium right-stick travel is not
+# suppressed by the quadratic forward-motion curve. The official yaw cap is
+# unchanged.
+yaw_joystick_expo = 1.0
 # TianGong secondary-development /cmd_vel limits for full/half-body walking:
 # linear.x [-0.8, +1.0] m/s and angular.z [-0.8, +0.8] rad/s.
 max_forward_m_s = 1.0
@@ -99,6 +109,17 @@ invert_scalar = false
 open_normalized = [0.4, 0.4, 0.05, 0.05, 0.05, 0.05]
 closed_normalized = [0.9, 0.5, 0.45, 0.52, 0.52, 0.45]
 
+# Right B owns a robot-side, release-guarded pointing gesture while a teleop
+# session is armed. Both activation and deactivation require one continuous
+# 3-second hold, separated by at least 0.5 seconds of stable release. A new
+# session is always release-locked. The pose is xTELE GestureController's
+# BrainCoRevo2 gesture 2 ("single index finger"), state 0, in motor order:
+# thumb bend, thumb rotation, index, middle, ring, little.
+right_b_point_gesture_enabled = true
+right_b_point_gesture_hold_seconds = 3.0
+right_b_point_gesture_release_seconds = 0.5
+right_b_point_pose_normalized = [0.2, 0.688, 0.0, 0.98, 0.98, 0.98]
+
 [home]
 # Deliberately slower than manual teleoperation. This Home pose was captured
 # from the robot's real arm feedback on 2026-08-06. Joint order is 11..17,

tg3_local_teleop/gesture_toggle.py

@@ -0,0 +1,243 @@
+#!/usr/bin/env python3
+"""Pure state machine for the right-B pointing-hand gesture.
+
+This module deliberately has no ROS dependencies so the hold/release safety
+rules can be exercised offline before the bridge is deployed on a robot.
+"""
+
+from __future__ import annotations
+
+import math
+from typing import Any, Mapping, Sequence
+
+
+class GestureToggle:
+    """Toggle one persistent gesture with a guarded long press.
+
+    A new session always starts release-locked.  A continuous release clears
+    that lock, then a continuous press toggles the gesture once.  The same
+    physical press cannot toggle twice; another stable release is required.
+    """
+
+    def __init__(self, hold_seconds: float, release_seconds: float) -> None:
+        if not math.isfinite(hold_seconds) or hold_seconds <= 0.0:
+            raise ValueError("gesture hold time must be positive and finite")
+        if not math.isfinite(release_seconds) or release_seconds <= 0.0:
+            raise ValueError("gesture release time must be positive and finite")
+        self.hold_seconds = float(hold_seconds)
+        self.release_seconds = float(release_seconds)
+        self.active = False
+        self.hold_started_at: float | None = None
+        self.release_started_at: float | None = None
+        self.require_release = True
+        self.armed = False
+        self.input_healthy = False
+        self.button_pressed: bool | None = None
+        self._freeze_right_hand = False
+        self.toggle_count = 0
+        self.last_transition = "initialized"
+
+    def new_session(self) -> None:
+        """Start release-locked and never carry a gesture across sessions."""
+
+        self.active = False
+        self.hold_started_at = None
+        self.release_started_at = None
+        self.require_release = True
+        self.armed = True
+        self.input_healthy = False
+        self.button_pressed = None
+        self._freeze_right_hand = True
+        self.last_transition = "new_session"
+
+    def disarm(self, reason: str = "disarmed") -> None:
+        """Clear the logical gesture without commanding any hand movement."""
+
+        self.active = False
+        self.hold_started_at = None
+        self.release_started_at = None
+        self.require_release = True
+        self.armed = False
+        self.input_healthy = False
+        self.button_pressed = None
+        self._freeze_right_hand = False
+        self.last_transition = reason
+
+    def update(
+        self,
+        now: float,
+        *,
+        armed: bool,
+        input_healthy: bool,
+        pressed: bool | None,
+    ) -> bool:
+        """Advance the state and return ``True`` only when a toggle occurs.
+
+        ``pressed=None`` represents a missing or malformed B-button sample.  It
+        cancels a pending hold and requires a fresh stable release.  A hand
+        feedback gap behaves the same way, but an already active gesture is
+        retained so it can resume through the bridge's measured-feedback slew.
+        """
+
+        if not math.isfinite(now):
+            raise ValueError("gesture clock must be finite")
+        if not armed:
+            if self.armed or self.active:
+                self.disarm()
+            return False
+        if not self.armed:
+            self.new_session()
+
+        self.input_healthy = bool(input_healthy)
+        self.button_pressed = pressed
+        if not input_healthy or pressed is None:
+            self.hold_started_at = None
+            self.release_started_at = None
+            self.require_release = True
+            if not self.active:
+                self._freeze_right_hand = True
+            return False
+
+        if self.require_release:
+            self.hold_started_at = None
+            if pressed:
+                self.release_started_at = None
+                return False
+            if self.release_started_at is None:
+                self.release_started_at = now
+                return False
+            if now - self.release_started_at >= self.release_seconds:
+                self.require_release = False
+                self.release_started_at = None
+                if not self.active:
+                    self._freeze_right_hand = False
+            return False
+
+        self.release_started_at = None
+        if not pressed:
+            if self.hold_started_at is not None:
+                # A short/interrupted press is not a toggle.  Debounce the
+                # release before another hold can start.
+                self.hold_started_at = None
+                self.release_started_at = now
+                self.require_release = True
+                if not self.active:
+                    self._freeze_right_hand = True
+            return False
+
+        if self.hold_started_at is None:
+            self.hold_started_at = now
+            if not self.active:
+                self._freeze_right_hand = True
+            return False
+        if now - self.hold_started_at < self.hold_seconds:
+            return False
+
+        self.active = not self.active
+        self.toggle_count += 1
+        self.last_transition = "activated" if self.active else "deactivated"
+        # Activation uses the point target.  Deactivation starts slewing back
+        # to live hand input immediately; the release lock only prevents a
+        # second toggle from the same physical press.
+        self._freeze_right_hand = False
+        self.hold_started_at = None
+        self.release_started_at = None
+        self.require_release = True
+        return True
+
+    @property
+    def freeze_right_hand(self) -> bool:
+        """Whether the bridge must hold its last right-hand command.
+
+        While inactive, freeze through an incomplete B hold and its required
+        release.  A completed deactivation starts returning to live input at
+        once; its release lock prevents only another toggle.  This keeps a
+        short B press from leaking through xTELE's processed command stream.
+        """
+
+        return self.armed and not self.active and self._freeze_right_hand
+
+    def hold_elapsed(self, now: float) -> float:
+        if self.hold_started_at is None:
+            return 0.0
+        return max(0.0, now - self.hold_started_at)
+
+    def release_elapsed(self, now: float) -> float:
+        if self.release_started_at is None:
+            return 0.0
+        return max(0.0, now - self.release_started_at)
+
+    @property
+    def state(self) -> str:
+        if not self.armed:
+            return "disarmed"
+        if not self.input_healthy:
+            return "input_unhealthy"
+        if self.button_pressed is None:
+            return "invalid_button"
+        if self.require_release:
+            return "awaiting_release"
+        if self.hold_started_at is not None:
+            return "holding_stop" if self.active else "holding_start"
+        return "active" if self.active else "idle"
+
+
+def right_b_pressed(data: Mapping[str, Any]) -> bool | None:
+    """Return TS1P right-B state, or ``None`` for a malformed sample."""
+
+    try:
+        buttons = data["button"]
+        if not isinstance(buttons, Mapping):
+            return None
+        right = buttons["right"]
+        if not isinstance(right, (list, tuple)) or len(right) < 2:
+            return None
+        value = right[1]
+        if isinstance(value, bool):
+            return value
+        if isinstance(value, int) and value in (0, 1):
+            return bool(value)
+        return None
+    except (KeyError, TypeError):
+        return None
+
+
+def normalized_pose_to_positions(
+    pose: Sequence[float], minimum: int, maximum: int
+) -> list[int]:
+    """Convert a six-motor normalized BrainCo pose to driver positions."""
+
+    if len(pose) != 6:
+        raise ValueError("gesture pose must contain 6 normalized values")
+    values = [float(value) for value in pose]
+    if not all(math.isfinite(value) and 0.0 <= value <= 1.0 for value in values):
+        raise ValueError("gesture pose must contain finite values in [0, 1]")
+    if minimum < 0 or maximum <= minimum:
+        raise ValueError("invalid BrainCo position range")
+    return [
+        int(round(minimum + value * (maximum - minimum))) for value in values
+    ]
+
+
+def select_right_hand_target(
+    source_target: Sequence[int],
+    previous_command: Sequence[int] | None,
+    gesture_target: Sequence[int],
+    *,
+    active: bool,
+    freeze: bool,
+) -> list[int]:
+    """Apply the right-only gesture override or fail-safe input freeze."""
+
+    source = list(source_target)
+    gesture = list(gesture_target)
+    if len(source) != 6 or len(gesture) != 6:
+        raise ValueError("right-hand targets must each contain 6 positions")
+    if active:
+        return gesture
+    if freeze:
+        previous = [] if previous_command is None else list(previous_command)
+        if len(previous) != 6:
+            raise ValueError("cannot freeze without a complete previous command")
+        return previous
+    return source

tg3_local_teleop/test_gesture_toggle.py

@@ -0,0 +1,268 @@
+#!/usr/bin/env python3
+from __future__ import annotations
+
+import unittest
+
+from gesture_toggle import (
+    GestureToggle,
+    normalized_pose_to_positions,
+    right_b_pressed,
+    select_right_hand_target,
+)
+
+
+POINT_POSE = [0.2, 0.688, 0.0, 0.98, 0.98, 0.98]
+
+
+class GestureToggleTest(unittest.TestCase):
+    def setUp(self) -> None:
+        self.gesture = GestureToggle(hold_seconds=3.0, release_seconds=0.5)
+        self.gesture.new_session()
+
+    def stable_release(self, started_at: float) -> float:
+        self.assertFalse(
+            self.gesture.update(
+                started_at, armed=True, input_healthy=True, pressed=False
+            )
+        )
+        finished_at = started_at + 0.5
+        self.assertFalse(
+            self.gesture.update(
+                finished_at, armed=True, input_healthy=True, pressed=False
+            )
+        )
+        self.assertFalse(self.gesture.require_release)
+        return finished_at
+
+    def test_new_session_held_button_cannot_activate(self) -> None:
+        self.assertFalse(
+            self.gesture.update(0.0, armed=True, input_healthy=True, pressed=True)
+        )
+        self.assertFalse(
+            self.gesture.update(10.0, armed=True, input_healthy=True, pressed=True)
+        )
+        self.assertFalse(self.gesture.active)
+        self.assertTrue(self.gesture.freeze_right_hand)
+
+    def test_continuous_three_second_holds_toggle_once_each(self) -> None:
+        released_at = self.stable_release(0.0)
+        self.assertFalse(
+            self.gesture.update(
+                released_at + 0.01,
+                armed=True,
+                input_healthy=True,
+                pressed=True,
+            )
+        )
+        self.assertFalse(
+            self.gesture.update(
+                released_at + 3.0,
+                armed=True,
+                input_healthy=True,
+                pressed=True,
+            )
+        )
+        self.assertTrue(
+            self.gesture.update(
+                released_at + 3.01,
+                armed=True,
+                input_healthy=True,
+                pressed=True,
+            )
+        )
+        self.assertTrue(self.gesture.active)
+        self.assertEqual(self.gesture.toggle_count, 1)
+
+        # Keeping the same physical press held cannot turn the gesture off.
+        self.assertFalse(
+            self.gesture.update(
+                released_at + 10.0,
+                armed=True,
+                input_healthy=True,
+                pressed=True,
+            )
+        )
+        self.assertTrue(self.gesture.active)
+
+        second_release = self.stable_release(released_at + 10.01)
+        self.gesture.update(
+            second_release + 0.01,
+            armed=True,
+            input_healthy=True,
+            pressed=True,
+        )
+        self.assertTrue(
+            self.gesture.update(
+                second_release + 3.02,
+                armed=True,
+                input_healthy=True,
+                pressed=True,
+            )
+        )
+        self.assertFalse(self.gesture.active)
+        self.assertEqual(self.gesture.toggle_count, 2)
+        self.assertFalse(self.gesture.freeze_right_hand)
+
+    def test_short_press_freezes_and_requires_stable_release(self) -> None:
+        released_at = self.stable_release(0.0)
+        self.gesture.update(
+            released_at + 0.01,
+            armed=True,
+            input_healthy=True,
+            pressed=True,
+        )
+        self.assertTrue(self.gesture.freeze_right_hand)
+        self.assertEqual(self.gesture.state, "holding_start")
+
+        self.gesture.update(
+            released_at + 2.0,
+            armed=True,
+            input_healthy=True,
+            pressed=False,
+        )
+        self.assertFalse(self.gesture.active)
+        self.assertTrue(self.gesture.require_release)
+        self.assertTrue(self.gesture.freeze_right_hand)
+        self.gesture.update(
+            released_at + 2.49,
+            armed=True,
+            input_healthy=True,
+            pressed=False,
+        )
+        self.assertTrue(self.gesture.require_release)
+        self.gesture.update(
+            released_at + 2.5,
+            armed=True,
+            input_healthy=True,
+            pressed=False,
+        )
+        self.assertFalse(self.gesture.require_release)
+        self.assertFalse(self.gesture.freeze_right_hand)
+
+    def test_invalid_button_or_feedback_gap_cancels_pending_hold(self) -> None:
+        released_at = self.stable_release(0.0)
+        self.gesture.update(
+            released_at + 0.01,
+            armed=True,
+            input_healthy=True,
+            pressed=True,
+        )
+        self.gesture.update(
+            released_at + 2.99,
+            armed=True,
+            input_healthy=True,
+            pressed=None,
+        )
+        self.assertTrue(self.gesture.require_release)
+        self.assertIsNone(self.gesture.hold_started_at)
+
+        released_at = self.stable_release(released_at + 3.0)
+        self.gesture.update(
+            released_at + 0.01,
+            armed=True,
+            input_healthy=True,
+            pressed=True,
+        )
+        self.gesture.update(
+            released_at + 2.99,
+            armed=True,
+            input_healthy=False,
+            pressed=True,
+        )
+        self.assertFalse(self.gesture.active)
+        self.assertTrue(self.gesture.require_release)
+        self.assertIsNone(self.gesture.hold_started_at)
+
+    def test_feedback_gap_preserves_already_active_gesture(self) -> None:
+        released_at = self.stable_release(0.0)
+        self.gesture.update(
+            released_at + 0.01,
+            armed=True,
+            input_healthy=True,
+            pressed=True,
+        )
+        self.assertTrue(
+            self.gesture.update(
+                released_at + 3.02,
+                armed=True,
+                input_healthy=True,
+                pressed=True,
+            )
+        )
+        self.gesture.update(
+            released_at + 3.03,
+            armed=True,
+            input_healthy=False,
+            pressed=True,
+        )
+        self.assertTrue(self.gesture.active)
+        self.assertTrue(self.gesture.require_release)
+
+    def test_disarm_clears_active_without_an_output_action(self) -> None:
+        released_at = self.stable_release(0.0)
+        self.gesture.update(
+            released_at + 0.01,
+            armed=True,
+            input_healthy=True,
+            pressed=True,
+        )
+        self.gesture.update(
+            released_at + 3.02,
+            armed=True,
+            input_healthy=True,
+            pressed=True,
+        )
+        self.assertTrue(self.gesture.active)
+        self.assertFalse(
+            self.gesture.update(
+                released_at + 3.03,
+                armed=False,
+                input_healthy=True,
+                pressed=True,
+            )
+        )
+        self.assertFalse(self.gesture.active)
+        self.assertEqual(self.gesture.state, "disarmed")
+        self.assertTrue(self.gesture.require_release)
+
+
+class GestureHelpersTest(unittest.TestCase):
+    def test_right_b_is_index_one_and_malformed_values_fail_closed(self) -> None:
+        self.assertTrue(right_b_pressed({"button": {"right": [0, 1, 0]}}))
+        self.assertFalse(right_b_pressed({"button": {"right": [1, 0, 1]}}))
+        self.assertIsNone(right_b_pressed({"button": {"right": [0]}}))
+        self.assertIsNone(right_b_pressed({"button": {"right": [0, -1]}}))
+        self.assertIsNone(right_b_pressed({"button": {"right": [0, 0.5]}}))
+
+    def test_point_pose_converts_to_expected_brainco_positions(self) -> None:
+        self.assertEqual(
+            normalized_pose_to_positions(POINT_POSE, 1, 1000),
+            [201, 688, 1, 980, 980, 980],
+        )
+
+    def test_right_only_override_and_short_press_freeze(self) -> None:
+        source = [401, 401, 51, 51, 51, 51]
+        previous = [450, 430, 80, 90, 100, 110]
+        point = [201, 688, 1, 980, 980, 980]
+        self.assertEqual(
+            select_right_hand_target(
+                source, previous, point, active=True, freeze=False
+            ),
+            point,
+        )
+        self.assertEqual(
+            select_right_hand_target(
+                source, previous, point, active=False, freeze=True
+            ),
+            previous,
+        )
+        self.assertEqual(
+            select_right_hand_target(
+                source, previous, point, active=False, freeze=False
+            ),
+            source,
+        )
+
+
+if __name__ == "__main__":
+    unittest.main()

tg3_local_teleop/test_idle_session_refresh.py

@@ -0,0 +1,159 @@
+#!/usr/bin/env python3
+from __future__ import annotations
+
+import importlib.util
+import json
+from pathlib import Path
+import struct
+import sys
+import time
+from types import ModuleType
+import unittest
+
+
+class Dummy:
+    pass
+
+
+def install_module(name: str, **attributes: object) -> ModuleType:
+    module = ModuleType(name)
+    for key, value in attributes.items():
+        setattr(module, key, value)
+    sys.modules[name] = module
+    return module
+
+
+install_module("rclpy")
+install_module("rclpy.node", Node=object)
+for package, message_name in (
+    ("brainco_hand_msgs.msg", "MotorStatus"),
+    ("brainco_hand_msgs.msg", "SetMotorMulti"),
+    ("diagnostic_msgs.msg", "DiagnosticStatus"),
+    ("geometry_msgs.msg", "TwistStamped"),
+    ("ros2_bridge_msgs.msg", "ArmStatus"),
+    ("sensor_msgs.msg", "JointState"),
+    ("std_srvs.srv", "Trigger"),
+):
+    module = sys.modules.get(package) or install_module(package)
+    setattr(module, message_name, Dummy)
+
+
+fake_omnisocket = install_module(
+    "omnisocket", CONTROL_DEFAULTS={}, MSG_TYPE_BINARY=2
+)
+
+module_path = Path(__file__).with_name("tg3_local_teleop.py")
+spec = importlib.util.spec_from_file_location("tg3_local_teleop", module_path)
+assert spec is not None and spec.loader is not None
+teleop = importlib.util.module_from_spec(spec)
+sys.modules[spec.name] = teleop
+spec.loader.exec_module(teleop)
+
+
+class FakeSession:
+    mode = "idle"
+    connect_count = 0
+    sequence = 0
+    sent_one = False
+    next_instance_id = 0
+    events: list[str] = []
+
+    def __init__(self) -> None:
+        type(self).next_instance_id += 1
+        self.instance_id = type(self).next_instance_id
+
+    def connect(self, **_kwargs: object) -> None:
+        type(self).connect_count += 1
+        type(self).events.append(f"connect:{self.instance_id}")
+
+    def stats(self) -> dict[str, int]:
+        return {"connected": 1, "registered": 1}
+
+    def recv(self, timeout_ms: int) -> tuple[str, int, bytes] | None:
+        if timeout_ms == 0:
+            return None
+        time.sleep(0.003)
+        if self.mode == "idle":
+            return None
+        if self.mode == "one_then_idle" and type(self).sent_one:
+            return None
+        type(self).sent_one = True
+        type(self).sequence += 1
+        data = {
+            "arm": {
+                "position": {"left": [0.0] * 7, "right": [0.0] * 7}
+            }
+        }
+        payload = json.dumps(data).encode()
+        packet = struct.pack(
+            "!4sQQI",
+            b"TG3A",
+            type(self).sequence,
+            time.time_ns(),
+            len(payload),
+        ) + payload
+        return "expected-sender", 2, packet
+
+    def close(self) -> None:
+        type(self).events.append(f"close:{self.instance_id}")
+        return None
+
+
+def config(refresh_s: float) -> dict[str, object]:
+    return {
+        "transport": "omnisocket",
+        "omnisocket_server": "127.0.0.1:14049",
+        "omnisocket_peer_id": "robot",
+        "omnisocket_expected_sender": "expected-sender",
+        "omnisocket_max_packet_age_ms": 300.0,
+        "omnisocket_idle_session_refresh_s": refresh_s,
+    }
+
+
+class IdleSessionRefreshTest(unittest.TestCase):
+    def setUp(self) -> None:
+        FakeSession.connect_count = 0
+        FakeSession.sequence = 0
+        FakeSession.sent_one = False
+        FakeSession.next_instance_id = 0
+        FakeSession.events = []
+        fake_omnisocket.Session = FakeSession
+
+    def run_source(self, mode: str, run_s: float = 0.13) -> dict[str, object]:
+        FakeSession.mode = mode
+        source = teleop.LatestArmData(config(0.02))
+        source.start()
+        time.sleep(run_s)
+        source.close()
+        return source.metrics()
+
+    def test_idle_session_is_periodically_reconnected(self) -> None:
+        metrics = self.run_source("idle")
+        self.assertGreaterEqual(FakeSession.connect_count, 2)
+        self.assertGreaterEqual(metrics["idle_session_refreshes"], 1)
+        self.assertEqual(
+            FakeSession.events[:3],
+            ["connect:1", "connect:2", "close:1"],
+        )
+
+    def test_continuous_valid_business_frames_prevent_refresh(self) -> None:
+        metrics = self.run_source("active", 0.08)
+        self.assertEqual(FakeSession.connect_count, 1)
+        self.assertEqual(metrics["idle_session_refreshes"], 0)
+        self.assertGreater(metrics["frames_accepted"], 1)
+
+    def test_session_refreshes_after_last_valid_frame(self) -> None:
+        metrics = self.run_source("one_then_idle")
+        self.assertGreaterEqual(FakeSession.connect_count, 2)
+        self.assertGreaterEqual(metrics["idle_session_refreshes"], 1)
+        self.assertEqual(metrics["frames_accepted"], 1)
+
+    def test_refresh_boundary_and_disable_switch(self) -> None:
+        due = teleop.LatestArmData._idle_session_refresh_due
+        self.assertFalse(due(10.0, 0.0, 0.0))
+        self.assertFalse(due(1.999, 0.0, 2.0))
+        self.assertTrue(due(2.0, 0.0, 2.0))
+
+
+if __name__ == "__main__":
+    unittest.main()

tg3_local_teleop/test_session_gate.py

@@ -79,6 +79,7 @@ class RobotSessionGateTest(unittest.TestCase):
         bridge.allow_publish = True
         bridge.cfg = {"control": {"auto_home_on_stop": True}}
         bridge.hands_enabled = False
+        bridge.right_point_gesture_enabled = False
         bridge.robot_arm_positions = [0.0] * 14
         bridge.last_command = None
         bridge.last_publish_at = 0.0
@@ -169,6 +170,7 @@ class RobotSessionGateTest(unittest.TestCase):
             "hold_seconds": 0.0,
             "joystick_deadzone": 0.2,
             "joystick_expo": 2.0,
+            "yaw_joystick_expo": 1.0,
             "max_forward_m_s": 1.0,
             "max_reverse_m_s": 0.8,
             "max_angular_rad_s": 0.8,
@@ -193,15 +195,27 @@ class RobotSessionGateTest(unittest.TestCase):
 
         moving = ArmSnapshot(
             {
-                "button": {"right": [False, False, True]},
-                "joystick": {"left": [1.0, 0.0]},
+                "button": {
+                    "left": [False, False, False],
+                    "right": [False, False, True],
+                },
+                "joystick": {
+                    "left": [1.0, 0.0],
+                    "right": [0.0, 0.0],
+                },
             },
             received_at=1.0,
         )
         released = ArmSnapshot(
             {
-                "button": {"right": [False, False, False]},
-                "joystick": {"left": [1.0, 0.0]},
+                "button": {
+                    "left": [False, False, False],
+                    "right": [False, False, False],
+                },
+                "joystick": {
+                    "left": [1.0, 0.0],
+                    "right": [0.0, 0.0],
+                },
             },
             received_at=1.1,
         )
@@ -212,6 +226,38 @@ class RobotSessionGateTest(unittest.TestCase):
         bridge._tick_locomotion(1.2, moving)
         self.assertEqual(published[-1], (1.0, -0.0))
 
+        turning = ArmSnapshot(
+            {
+                "button": {
+                    "left": [False, False, True],
+                    "right": [False, False, False],
+                },
+                "joystick": {
+                    "left": [0.0, 0.0],
+                    "right": [0.0, 1.0],
+                },
+            },
+            received_at=1.3,
+        )
+        bridge._tick_locomotion(1.3, turning)
+        self.assertEqual(published[-1], (0.0, -0.8))
+
+        old_wrong_binding = ArmSnapshot(
+            {
+                "button": {
+                    "left": [False, False, False],
+                    "right": [False, False, True],
+                },
+                "joystick": {
+                    "left": [0.0, 1.0],
+                    "right": [0.0, 0.0],
+                },
+            },
+            received_at=1.4,
+        )
+        bridge._tick_locomotion(1.4, old_wrong_binding)
+        self.assertEqual(published[-1], (0.0, 0.0))
+
 
 if __name__ == "__main__":
     unittest.main()

tg3_local_teleop/tg3-local-teleop.service

@@ -1,14 +1,16 @@
 [Unit]
-Description=TG3 session-gated local TS1P teleoperation bridge
+Description=TG3 local TS1P dual-arm bridge (no cloud pairing)
 After=network-online.target
 Wants=network-online.target
 
 [Service]
 Type=simple
+ExecStartPre=/home/nvidia/tg3_local_teleop/wait_ros_ready.sh
 ExecStart=/home/nvidia/tg3_local_teleop/run.sh --allow-publish
 Restart=on-failure
 RestartSec=2
 KillSignal=SIGINT
+TimeoutStartSec=150
 TimeoutStopSec=5
 
 [Install]

tg3_local_teleop/tg3_local_teleop.py

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

tg3_local_teleop/wait_ros_ready.sh

@@ -0,0 +1,34 @@
+#!/usr/bin/env bash
+# Avoid creating the long-lived Fast DDS participant before the robot network
+# interfaces and the vendor ROS graph are available. A participant created
+# while the configured interface whitelist is empty does not recover later.
+set -eo pipefail
+
+source /opt/ros/jazzy/setup.bash
+if [[ -f /home/nvidia/xos/setup.bash ]]; then
+  source /home/nvidia/xos/setup.bash
+fi
+if [[ -f /opt/robot_tele_server/install/setup.bash ]]; then
+  source /opt/robot_tele_server/install/setup.bash
+fi
+
+required_topic="/hric/robot/rl_state"
+wait_seconds="${TG3_ROS_READY_WAIT_SECONDS:-120}"
+retry_seconds="${TG3_ROS_READY_RETRY_SECONDS:-2}"
+deadline=$((SECONDS + wait_seconds))
+
+while ((SECONDS < deadline)); do
+  # Capture first: grep -q in a pipe can close stdout early and make ros2
+  # report SIGPIPE under pipefail even though the topic was found.
+  topic_list="$(
+    timeout 8 ros2 topic list --no-daemon --spin-time 3 2>/dev/null || true
+  )"
+  if grep -Fxq "$required_topic" <<<"$topic_list"; then
+    echo "ROS discovery ready: $required_topic"
+    exit 0
+  fi
+  sleep "$retry_seconds"
+done
+
+echo "ROS discovery did not expose $required_topic within ${wait_seconds}s" >&2
+exit 1