feat: package TG3 TS1P OmniSocket teleoperation

tg3_local_teleop/README.md

@@ -0,0 +1,185 @@
+# 天工 3.0 本地同构臂双臂、BrainCo 灵巧手与 HBWALK 行走控制
+
+这套桥接不修改工控机 xTELE、`robot_tele_server` 或机器人控制源码。数据链路为：
+
+```text
+TS1P 同构臂
+  -> 工控机 xTELE tcp://127.0.0.1:5003
+  -> OmniSocket KCP Hub 175.178.116.187:14049
+  -> tg3_local_teleop（直接 OmniSocket Session；HBWALK / 错误 / 限位 / 失联门控）
+  -> /encoder_identical_joint（14 维 JointState）
+  -> 厂家 freq_change_tg3_node（同步、滤波、200 Hz）
+  -> /arm/cmd
+  -> 天工 3.0 双臂
+  -> /left_hand/set_motor_multi + /right_hand/set_motor_multi
+  -> 天工 3.0 BrainCo Revo2 双灵巧手
+  -> /hric/robot/cmd_vel（右 C + 左摇杆；50 Hz TwistStamped）
+  -> 天工 3.0 HBWALK 行走
+```
+
+## 自动运行与操作
+
+- 桥接服务随机器人算力主机的用户服务自动启动；未进入 `HBWALK` 时只监测、不发布。
+- 开始遥操：进入 `HBWALK` 后，同时长按左手 `Z` + 右手 `C` 3 秒。该计时在 EAI
+  本机完成；计时未通过前不向 Hub 发送 xTELE 业务帧。
+- 结束遥操：再次同时长按 3 秒；EAI 发送最后一个匹配会话的 `STOP` 后停止业务数据，
+  机器人立即停控并以 `0.25 rad/s` 自动回 Home。
+- 回 Home 过程中用新的 Z+C 3 秒会话可取消回位并重新启动遥操。
+- 本地桥控制左右各 7 个手臂关节、BrainCo Revo2 双灵巧手和 HBWALK
+  前后/转向；不控制腰和头。
+- 行走：遥操已启动后，右 `C` 是即时 deadman；按住 C 并把左摇杆推出死区后，在下一个
+  `50 Hz` 周期立即响应，不再等待 3 秒。松开 C 或摇杆回中立即发零速，再按也立即恢复。
+  左摇杆上下控制前后，左右控制原地转向。
+  按官方半身行走 Topic 范围限幅：前进 `1.0 m/s`、后退 `0.8 m/s`、转向
+  `0.8 rad/s`；死区 `0.2`，输出频率 `50 Hz`。
+- 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 后续直接输出六维归一化位置，会自动使用六维位置。
+
+## 安全门控
+
+只有以下条件全部满足才允许开始：机器人 `current_state` 和 `child_state` 均为
+`HBWALK`、状态为 `running`；同构臂数据新鲜；左右 CAN/伺服无错误；采样频率正常；
+启动瞬间的 14 个目标均在天工 3.0 文档限位内；没有检测到云端第二路同构臂命令。
+自定义关节目标限位仅对每个新 `session_id` 的 START 检查一次，采集/跟随过程中不再检查；失联、电机错误、
+HBWALK 状态等运行时保护仍持续检查。厂家 `freq_change_tg3_node` 原有的姿态同步、
+关节限位和碰撞保护保持不变。为启用二次开放文档规定的 HBWALK topic 行走，本机
+`xmigcs/config/dex_config.yaml` 的 HBWALK 速度许可已由全零改为当前 NAVIGATE 的上限；
+实际发出的速度由本桥按官方 Topic 范围限制为 `linear.x=[-0.8, 1.0] m/s`、
+`angular.z=[-0.8, 0.8] rad/s`；本桥不发布侧移速度。
+
+行走速度只在同一次遥操武装、`HBWALK/HBWALK/running`、网络数据新鲜且右 C + 左摇杆
+即时成立时发布。解除组合后立即发布零速，并补发 10 帧零速。公网断线、退出遥操、
+回 Home 或机器人退出 HBWALK 也走同一停止逻辑。
+
+灵巧手仅在同一次长按武装后发布；以机器人真实手指位置为起点，并按每秒最多 400 个
+位置单位平滑跟随。BrainCo 状态 `0`（空闲）、`1`（运动）、`2`（接触/堵转或到限位）
+和 `3`（持续力）均为厂家定义的正常运行状态；仅状态失联、未知/非法状态、同构臂失联
+或检测到另一命令源时，双臂和双手会一起解除武装并停止发布。停止遥操后灵巧手保持
+最后目标，双臂按原逻辑限速回 Home。BrainCo 驱动原有的电流、堵转和碰撞保护不作修改。
+
+## 限速双臂回 Home
+
+回 Home 命令读取机器人真实 `/freq_change/arm_status` 作为轨迹起点，以 `0.25 rad/s`
+把关节 11..17、21..27 平滑移动到现场保存的双臂 Home 姿态；Home 不是电机全零位。
+只有未武装、HBWALK、机器人双臂反馈新鲜且无错误时才接受。Home 轨迹不依赖已经主动
+停止的 EAI 数据，只依赖机器人反馈和机器人侧安全门；执行取消命令会立即停止发布。
+若机器人实际位置落后指令超过 `0.08 rad`，也会自动停止。
+
+```bash
+./home.sh start
+./home.sh status
+./home.sh cancel
+```
+
+`home.sh start` 会引起机器人双臂运动，执行前必须确认防摔、净空和急停。当前 Home
+角度保存在 `config.toml` 的 `home.joint_goal_rad` 中。
+
+## 部署与检查
+
+程序目录放在机器人算力主机 `/home/nvidia/tg3_local_teleop`。只读监测测试：
+
+```bash
+./run.sh --duration 10
+./status.sh
+```
+
+安装并启用用户服务自动启动：
+
+```bash
+mkdir -p ~/.config/systemd/user
+cp tg3-local-teleop.service ~/.config/systemd/user/
+systemctl --user daemon-reload
+systemctl --user enable tg3-local-teleop.service
+```
+
+现场确认防摔、双臂活动范围无人、急停可用后，才可启动：
+
+```bash
+systemctl --user start tg3-local-teleop.service
+systemctl --user status tg3-local-teleop.service
+```
+
+停止与查看状态：
+
+```bash
+systemctl --user stop tg3-local-teleop.service
+./status.sh
+```
+
+当前 `config.toml` 由桥接程序直接建立 OmniSocket Session，不再经过机器人本机 ZMQ
+接收代理。若需回退到机器人与工控机的局域网直连，可把 `network.transport` 改成
+`zmq`；保留的 `iarm_endpoint` 为 `tcp://192.168.5.14:5003`。
+
+遥操活动期间公网输入超过 `0.25 s` 未更新会立即解除武装并停止发布，同一会话不能自动
+重新武装；活动会话超过 `2 s` 仍无帧时才重建机器人 OmniSocket 进程。未 START 或收到
+STOP 后没有业务帧是正常待机，不触发反复重启。只有匹配的操作员 STOP 自动回 Home；
+意外断网只停控，不自动产生回位运动。
+
+## 完整重启顺序
+
+只重启本项目的网络与机器人桥时，先启动 Nvidia 接收端，再启动 EAI 本地门控服务；
+EAI 在收到物理 START 前不会建立发送 Session：
+
+```bash
+ssh nvidia@192.168.41.2 'systemctl --user restart tg3-local-teleop.service'
+ssh eai 'systemctl --user restart tg3-omnisocket-sender.service'
+```
+
+检查：
+
+```bash
+ssh eai 'systemctl --user --no-pager status tg3-omnisocket-sender.service'
+ssh nvidia@192.168.41.2 'systemctl --user --no-pager status tg3-local-teleop.service'
+```
+
+修改 Ubuntu 的 `xmigcs/config/dex_config.yaml` 后，必须重启 xmigcs 才会加载新值。现场
+固件没有公开的单独 `rl/xmigcs` 启停 service、action 或 CLI；
+`/proc_manager/config/notify` 只通知进程管理器重新读取它自己的固定
+`proc_manager.json`，消息内容只写入日志，不能执行 `CMD_STOP_PROC` 或
+`CMD_START_PROC`。不要向该话题发送进程启停 JSON。
+
+首选做法是先退出遥操并停止本项目两个服务，再按厂商原有物理流程切到 `48V Off`。
+现场 `proc_manager` 会在真实的 `48vOff` 状态下停止 `rl` 和 `robot_control`；随后按原流程
+恢复 48V 并启动机器人，`robotcontrol_state` 恢复后会重新创建 xmigcs，新进程才会读取
+修改后的配置：
+
+```bash
+ssh eai 'systemctl --user stop tg3-omnisocket-sender.service'
+ssh nvidia@192.168.41.2 'systemctl --user stop tg3-local-teleop.service'
+
+# 此处按厂商原有物理流程切到 48V Off。
+# 本机断电后 Ubuntu/网络可能同时离线，SSH 失败是正常现象，不在断电期间执行检查。
+
+# 按厂商原有流程恢复 48V 并启动机器人，等待状态初始化完成；下列命令应显示新 PID。
+ssh ubuntu@192.168.41.1 \
+  "pgrep -af '^/usr/bin/python3 /home/ubuntu/.local/bin/xmigcs '"
+ssh ubuntu@192.168.41.1 \
+  "ps -eo pid,lstart,comm,args | grep '[x]migcs'"
+
+ssh eai 'systemctl --user restart tg3-omnisocket-sender.service'
+ssh nvidia@192.168.41.2 'systemctl --user restart tg3-local-teleop.service'
+```
+
+不要直接 `kill` xmigcs，也不要仅为加载此配置执行
+`sudo systemctl restart proc_manager.service`。后者会同时停止 joystick、robot_control、
+xmigcs 等全部子进程；而 `robot_control` 和 `rl` 均为 `boot_start=false`，可能不能自动
+恢复，不能把它当作 xmigcs 单进程重启命令。
+
+EAI 当前用户服务虽已启用，但该用户的 systemd linger 为关闭状态；重启命令可通过 SSH
+正常执行，整机冷启动后则需先登录 EAI。若需要无人登录也随系统启动，只执行一次：
+
+```bash
+sudo loginctl enable-linger eai
+```
+
+当前厂商配置备份位于：
+
+```text
+/home/ubuntu/.local/lib/python3.12/site-packages/xmigcs/config/
+  dex_config.yaml.before-local-teleop-20260807
+```

tg3_local_teleop/config.toml

@@ -0,0 +1,119 @@
+[network]
+# Direct OmniSocket KCP input. No robot-side ZMQ receiving proxy is used.
+transport = "omnisocket"
+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 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.
+omnisocket_restart_after_stale_s = 2.0
+# Retained only as a direct-LAN fallback when transport is changed to "zmq".
+iarm_endpoint = "tcp://192.168.5.14:5003"
+expected_iarm_id = "IArm009027FA8190"
+expected_iarm_type = "TS1P"
+source_timeout_s = 0.25
+minimum_arm_frequency_hz = 30.0
+
+[ros]
+command_topic = "/encoder_identical_joint"
+rl_state_topic = "/hric/robot/rl_state"
+arm_state_topic = "/freq_change/arm_status"
+home_service = "/tg3_local_teleop/return_home"
+cancel_home_service = "/tg3_local_teleop/cancel_home"
+publish_rate_hz = 50.0
+
+[robot]
+required_state = "HBWALK"
+required_status = "running"
+state_timeout_s = 1.0
+arm_state_timeout_s = 0.25
+
+[control]
+# OmniSocket start/stop is timed on EAI before any business frame is sent.
+# This value remains for the direct-ZMQ fallback latch.
+auto_home_on_stop = true
+start_stop_hold_seconds = 3.0
+max_slew_rad_s = 1.0
+# This custom margin is checked only when teleoperation starts. It is not
+# checked while collecting/following. Vendor limit protection is unchanged.
+joint_limit_margin_rad = 0.03
+
+# Joint order: left 11..17, then right 21..27. Values are the TG3.0 limits
+# from the supplied 26.04.x SDK document, expressed in radians.
+joint_lower_rad = [
+  -2.8449, -0.1920, -2.8449, -2.5482, -2.8449, -1.3265, -1.3265,
+  -2.8449, -3.3336, -2.8449, -2.5482, -2.8449, -1.3265, -1.3265,
+]
+joint_upper_rad = [
+   2.8449,  3.3336,  2.8449,  0.1920,  2.8449,  1.3265,  1.3265,
+   2.8449,  0.1920,  2.8449,  0.1920,  2.8449,  1.3265,  1.3265,
+]
+
+[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.
+enabled = true
+command_topic = "/hric/robot/cmd_vel"
+# This bridge never publishes FSM commands. The robot must already report
+# HBWALK/running through the existing safety gate before velocity is allowed.
+# Match the TG3 secondary-development TwistStamped example.
+frame_id = "pelvis"
+hold_seconds = 0.0
+joystick_deadzone = 0.2
+joystick_expo = 2.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
+max_reverse_m_s = 0.8
+max_angular_rad_s = 0.8
+forward_axis_sign = 1.0
+yaw_axis_sign = -1.0
+zero_burst_frames = 10
+
+[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.
+enabled = true
+left_command_topic = "/left_hand/set_motor_multi"
+right_command_topic = "/right_hand/set_motor_multi"
+left_status_topic = "/left_hand/motor_status"
+right_status_topic = "/right_hand/motor_status"
+status_timeout_s = 0.25
+
+# Vendor robot_tele_server uses mode 5 and an 800 ms expected duration.
+mode = 5
+duration_ms = 800
+position_min = 1
+position_max = 1000
+# Commissioning limit: no commanded motor may change faster than 400/1000 of
+# its full stroke per second. The driver keeps its own current/stall protection.
+slew_units_per_s = 400.0
+invert_scalar = false
+
+# Scalar hand.position=0 is the robot's measured default-open pose. The closed
+# endpoint is the conservative BrainCoRevo2 "normal" grasp from the installed
+# xTELE GestureController.
+# Order: thumb bend, thumb rotation, index, middle, ring, little.
+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]
+
+[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,
+# followed by 21..27; values are radians. Home is not the motor-zero pose.
+slew_rad_s = 0.25
+joint_goal_rad = [
+   0.1842578799,  0.1234146282,  0.0396494754, -0.4828015864,
+  -0.0024294013,  0.0065013529, -0.0040423824,
+   0.1721130311, -0.1383204311, -0.0142812012, -0.3408825397,
+   0.0026911981,  0.0039934102, -0.0030838961,
+]
+# Abort if the commanded trajectory gets more than ~4.6 degrees ahead of
+# measured robot motion.
+max_tracking_error_rad = 0.08
+command_tolerance_rad = 0.005
+actual_tolerance_rad = 0.03
+settle_s = 1.0
+timeout_s = 30.0

tg3_local_teleop/home.sh

@@ -0,0 +1,31 @@
+#!/usr/bin/env bash
+set -eo pipefail
+
+APP_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+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
+if [[ -f "$APP_DIR/ros2_py/install/setup.bash" ]]; then
+  source "$APP_DIR/ros2_py/install/setup.bash"
+fi
+set -u
+
+case "${1:-status}" in
+  start)
+    ros2 service call /tg3_local_teleop/return_home std_srvs/srv/Trigger "{}"
+    ;;
+  cancel|stop)
+    ros2 service call /tg3_local_teleop/cancel_home std_srvs/srv/Trigger "{}"
+    ;;
+  status)
+    "$APP_DIR/status.sh"
+    ;;
+  *)
+    echo "usage: $0 {start|cancel|status}" >&2
+    exit 2
+    ;;
+esac

tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/CMakeLists.txt

@@ -0,0 +1,15 @@
+cmake_minimum_required(VERSION 3.8)
+project(ros2_bridge_msgs)
+
+find_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(std_msgs REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/MotorStatus.msg"
+  "msg/ArmStatus.msg"
+  DEPENDENCIES std_msgs
+)
+
+ament_export_dependencies(rosidl_default_runtime)
+ament_package()

tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/ArmStatus.msg

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

tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/msg/MotorStatus.msg

@@ -0,0 +1,7 @@
+uint16 name
+float64 pos
+float64 speed
+float64 current
+float64 temperature
+float64 mos_temperature
+uint32 error

tg3_local_teleop/ros2_py/src/ros2_bridge_msgs/package.xml

@@ -0,0 +1,13 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>ros2_bridge_msgs</name>
+  <version>0.0.0</version>
+  <description>Python bindings for the installed TG3 ArmStatus wire type.</description>
+  <maintainer email="nvidia@example.com">nvidia</maintainer>
+  <license>Proprietary</license>
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
+  <depend>std_msgs</depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+</package>

tg3_local_teleop/run.sh

@@ -0,0 +1,26 @@
+#!/usr/bin/env bash
+# ROS setup scripts probe optional variables, so enable nounset only after
+# sourcing the overlays.
+set -eo pipefail
+
+APP_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+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
+if [[ -f "$APP_DIR/ros2_py/install/setup.bash" ]]; then
+  source "$APP_DIR/ros2_py/install/setup.bash"
+fi
+if [[ -d /home/nvidia/OmniSocketGo/python ]]; then
+  export PYTHONPATH="/home/nvidia/OmniSocketGo/python${PYTHONPATH:+:$PYTHONPATH}"
+fi
+set -u
+
+exec python3 "$APP_DIR/tg3_local_teleop.py" \
+  --config "$APP_DIR/config.toml" \
+  --status-file "$APP_DIR/status.json" \
+  "$@"

tg3_local_teleop/status.sh

@@ -0,0 +1,9 @@
+#!/usr/bin/env bash
+set -euo pipefail
+
+APP_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+if [[ ! -f "$APP_DIR/status.json" ]]; then
+  echo "status.json does not exist; the bridge has not produced status yet"
+  exit 1
+fi
+python3 -m json.tool "$APP_DIR/status.json"

tg3_local_teleop/test_session_gate.py

@@ -0,0 +1,217 @@
+#!/usr/bin/env python3
+"""Offline protocol tests for the robot-side session gate."""
+
+from __future__ import annotations
+
+import importlib.util
+from pathlib import Path
+from types import MethodType, ModuleType
+import sys
+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=Dummy)
+for package, names in {
+    "brainco_hand_msgs.msg": ("MotorStatus", "SetMotorMulti"),
+    "diagnostic_msgs.msg": ("DiagnosticStatus",),
+    "geometry_msgs.msg": ("TwistStamped",),
+    "ros2_bridge_msgs.msg": ("ArmStatus",),
+    "sensor_msgs.msg": ("JointState",),
+    "std_srvs.srv": ("Trigger",),
+}.items():
+    install_module(package, **{name: Dummy for name in names})
+
+module_path = Path(__file__).with_name("tg3_local_teleop.py")
+spec = importlib.util.spec_from_file_location("tg3_local_teleop_module", module_path)
+assert spec is not None and spec.loader is not None
+bridge_module = importlib.util.module_from_spec(spec)
+sys.modules[spec.name] = bridge_module
+spec.loader.exec_module(bridge_module)
+ArmSnapshot = bridge_module.ArmSnapshot
+LocalTeleopBridge = bridge_module.LocalTeleopBridge
+
+
+class NullLogger:
+    def info(self, _message: str) -> None:
+        pass
+
+    def warning(self, _message: str) -> None:
+        pass
+
+    def error(self, _message: str) -> None:
+        pass
+
+
+def sample(session_id: str, seq: int, state: str, reason: str = "") -> object:
+    metadata: dict[str, object] = {
+        "protocol_version": 2,
+        "session_id": session_id,
+        "session_seq": seq,
+        "session_state": state,
+    }
+    if reason:
+        metadata["stop_reason"] = reason
+    return ArmSnapshot({"tg3_transport": metadata}, received_at=10.0)
+
+
+class RobotSessionGateTest(unittest.TestCase):
+    def make_bridge(self) -> object:
+        bridge = LocalTeleopBridge.__new__(LocalTeleopBridge)
+        bridge.last_session_state = "inactive"
+        bridge.last_session_stop_reason = ""
+        bridge.active_session_id = None
+        bridge.session_start_attempted_id = None
+        bridge.armed = False
+        bridge.returning_home = False
+        bridge.allow_publish = True
+        bridge.cfg = {"control": {"auto_home_on_stop": True}}
+        bridge.hands_enabled = False
+        bridge.robot_arm_positions = [0.0] * 14
+        bridge.last_command = None
+        bridge.last_publish_at = 0.0
+        bridge.get_logger = MethodType(lambda _self: NullLogger(), bridge)
+        bridge._safety_reasons = MethodType(
+            lambda _self, _now, _sample, _error: [], bridge
+        )
+
+        def disarm(self: object, _reason: str) -> None:
+            self.armed = False
+
+        bridge._disarm = MethodType(disarm, bridge)
+        bridge.home_started = False
+
+        def start_home(
+            self: object,
+            _now: float,
+            _sample: object,
+            _error: str,
+            _reason: str,
+        ) -> bool:
+            self.home_started = True
+            return True
+
+        bridge._start_home_if_safe = MethodType(start_home, bridge)
+        return bridge
+
+    def test_start_active_stop_is_session_scoped(self) -> None:
+        bridge = self.make_bridge()
+        session_id = "a" * 32
+        bridge._update_session_gate(10.0, sample(session_id, 1, "start"), "")
+        self.assertTrue(bridge.armed)
+        self.assertEqual(bridge.active_session_id, session_id)
+
+        bridge._update_session_gate(10.1, sample(session_id, 2, "active"), "")
+        self.assertTrue(bridge.armed)
+        bridge._update_session_gate(
+            10.2, sample("b" * 32, 3, "stop", "operator"), ""
+        )
+        self.assertTrue(bridge.armed)
+
+        bridge._update_session_gate(
+            10.3, sample(session_id, 4, "stop", "operator"), ""
+        )
+        self.assertFalse(bridge.armed)
+        self.assertIsNone(bridge.active_session_id)
+        self.assertTrue(bridge.home_started)
+
+    def test_active_without_start_never_arms(self) -> None:
+        bridge = self.make_bridge()
+        bridge._update_session_gate(10.0, sample("c" * 32, 1, "active"), "")
+        self.assertFalse(bridge.armed)
+        self.assertIsNone(bridge.active_session_id)
+
+    def test_rejected_start_is_not_retried_in_same_session(self) -> None:
+        bridge = self.make_bridge()
+        calls = 0
+
+        def reject(_self: object, _now: float, _sample: object, _error: str) -> list[str]:
+            nonlocal calls
+            calls += 1
+            return ["blocked"]
+
+        bridge._safety_reasons = MethodType(reject, bridge)
+        session_id = "d" * 32
+        bridge._update_session_gate(10.0, sample(session_id, 1, "start"), "")
+        bridge._update_session_gate(10.1, sample(session_id, 2, "start"), "")
+        bridge._update_session_gate(10.2, sample(session_id, 3, "active"), "")
+        self.assertEqual(calls, 1)
+        self.assertFalse(bridge.armed)
+
+    def test_metadata_validation(self) -> None:
+        valid = {
+            "tg3_transport": {
+                "protocol_version": 2,
+                "session_id": "e" * 32,
+                "session_seq": 1,
+                "session_state": "start",
+            }
+        }
+        self.assertIsNotNone(LocalTeleopBridge._teleop_session_info(valid))
+        valid["tg3_transport"]["protocol_version"] = 1
+        self.assertIsNone(LocalTeleopBridge._teleop_session_info(valid))
+
+    def test_locomotion_reacts_immediately_after_repress(self) -> None:
+        bridge = LocalTeleopBridge.__new__(LocalTeleopBridge)
+        bridge.locomotion_cfg = {
+            "hold_seconds": 0.0,
+            "joystick_deadzone": 0.2,
+            "joystick_expo": 2.0,
+            "max_forward_m_s": 1.0,
+            "max_reverse_m_s": 0.8,
+            "max_angular_rad_s": 0.8,
+            "forward_axis_sign": 1.0,
+            "yaw_axis_sign": -1.0,
+            "zero_burst_frames": 10,
+        }
+        bridge.walk_combo_started_at = None
+        bridge.walk_active = False
+        bridge.walk_command = [0.0, 0.0]
+        bridge.walk_zero_frames_remaining = 0
+        bridge.get_logger = MethodType(lambda _self: NullLogger(), bridge)
+        published: list[tuple[float, float]] = []
+
+        def publish(
+            self: object, linear_x: float, angular_z: float, _now: float
+        ) -> None:
+            published.append((linear_x, angular_z))
+            self.walk_command = [linear_x, angular_z]
+
+        bridge._publish_walk = MethodType(publish, bridge)
+
+        moving = ArmSnapshot(
+            {
+                "button": {"right": [False, False, True]},
+                "joystick": {"left": [1.0, 0.0]},
+            },
+            received_at=1.0,
+        )
+        released = ArmSnapshot(
+            {
+                "button": {"right": [False, False, False]},
+                "joystick": {"left": [1.0, 0.0]},
+            },
+            received_at=1.1,
+        )
+        bridge._tick_locomotion(1.0, moving)
+        self.assertEqual(published[-1], (1.0, -0.0))
+        bridge._tick_locomotion(1.1, released)
+        self.assertEqual(published[-1], (0.0, 0.0))
+        bridge._tick_locomotion(1.2, moving)
+        self.assertEqual(published[-1], (1.0, -0.0))
+
+
+if __name__ == "__main__":
+    unittest.main()

tg3_local_teleop/tg3-local-teleop.service

@@ -0,0 +1,15 @@
+[Unit]
+Description=TG3 session-gated local TS1P teleoperation bridge
+After=network-online.target
+Wants=network-online.target
+
+[Service]
+Type=simple
+ExecStart=/home/nvidia/tg3_local_teleop/run.sh --allow-publish
+Restart=on-failure
+RestartSec=2
+KillSignal=SIGINT
+TimeoutStopSec=5
+
+[Install]
+WantedBy=default.target