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

tg3_omnisocket_transport/README.md

@@ -21,6 +21,17 @@ Z+C 按键及诊断字段；若 5001 的双侧 `hand.position` 在 250 ms 内有
 xTELE 已处理的标量或六维 BrainCoRevo2 目标。5002 的界面事件和腰、头、腿、行走、
 底盘命令不会进入机器人双臂桥。
 
+右侧原始按键顺序是 `A/B/C`。按住右 `B` 时，sender 仍合并 5001 的左手
+目标，但保留 5003 的原始右手开合量，不用 xTELE 处理后的右手目标覆盖。
+这样右 B 的自定义长按手势还未满 3 秒时，原厂 B 处理不会让右手提前动作。
+松开 B 并连续稳定释放 `0.5 s` 后，还要等新鲜 5001 右手目标连续至少 `0.25 s`
+恢复到按键前基线或当前 5003 原始开合量，才恢复双侧合并；无法确认时持续使用原始
+右手值，防止释放沿后的锁存/延迟目标漏入。
+5001 目标的时间戳也必须不晚于当前 5003 原始帧；若两个本机 ZMQ socket 的轮询顺序
+暂时颠倒，该 processed 目标会等到对应或更新的原始帧到达后才可合并。
+`tg3_transport.processed_hand_sides` 记录本帧实际合并
+的侧；右 B 抑制生效时还会设置 `processed_right_hand_suppressed_by_b=true`。
+
 公网业务数据由 EAI 本地会话门控：服务启动后仍持续读取本机 5003/5001，但不发送
 xTELE 帧；连续长按左 Z + 右 C 3 秒后才生成新的 128-bit `session_id` 并开始发送。
 再次长按 3 秒时发送最后一帧 `stop`，等待有界 KCP 刷新后关闭 OmniSocket Session；
@@ -42,7 +53,7 @@ xTELE 帧；连续长按左 Z + 右 C 3 秒后才生成新的 128-bit `session_i
 }
 ```
 
-`start` 连续发送 50 个源帧，避免接收端只取最新帧时错过唯一启动事件；机器人对同一
+当前用户服务将 `start` 连续发送 500 个源帧，避免接收端只取最新帧时错过唯一启动事件；机器人对同一
 `session_id` 只做一次启动安全检查。`stop` 是该会话最后一个业务帧。服务重启、源数据
 失联或网络错误都会使会话失效，恢复后必须先松开组合键，再重新长按 3 秒。
 `tg3_local_teleop` 内部直接拒绝非预期发送端、

tg3_omnisocket_transport/omnisocket_xtele_sender.py

@@ -38,6 +38,8 @@ class XteleSender:
     def __init__(self, args: argparse.Namespace) -> None:
         if args.start_stop_hold_s <= 0.0:
             raise ValueError("start/stop hold time must be positive")
+        if args.combo_release_s <= 0.0:
+            raise ValueError("combo release confirmation time must be positive")
         if args.start_marker_frames <= 0:
             raise ValueError("start marker frame count must be positive")
         if args.source_timeout_s <= 0.0:
@@ -62,8 +64,18 @@ class XteleSender:
         self.packet_sequence = time.time_ns()
         self.start_markers_remaining = 0
         self.combo_started_at: float | None = None
+        self.combo_release_started_at: float | None = None
         # A service restart must never turn an already-held combo into a start.
         self.require_combo_release = True
+        # Keep xTELE's processed right-hand stream isolated for the complete
+        # right-B press and stable-release transaction.  The robot runs the
+        # authoritative three-second gesture toggle from the raw B state.
+        self.right_b_merge_suppressed = False
+        self.right_b_release_started_at: float | None = None
+        self.right_b_recovery_started_at: float | None = None
+        self.right_b_processed_baseline: object | None = None
+        self.right_b_baseline_candidate: object | None = None
+        self.right_b_baseline_candidate_started_at: float | None = None
         self.counters = {
             "connected": 0,
             "reconnects": 0,
@@ -156,6 +168,16 @@ class XteleSender:
             if self.combo_started_at is None
             else round(time.monotonic() - self.combo_started_at, 2),
             "teleop_require_combo_release": self.require_combo_release,
+            "teleop_combo_release_hold_s": 0.0
+            if self.combo_release_started_at is None
+            else round(time.monotonic() - self.combo_release_started_at, 2),
+            "right_b_processed_merge_suppressed": self.right_b_merge_suppressed,
+            "right_b_merge_release_hold_s": 0.0
+            if self.right_b_release_started_at is None
+            else round(time.monotonic() - self.right_b_release_started_at, 2),
+            "right_b_processed_recovery_hold_s": 0.0
+            if self.right_b_recovery_started_at is None
+            else round(time.monotonic() - self.right_b_recovery_started_at, 2),
             "application_data_sending": (
                 self.teleop_active and self.session is not None
             ),
@@ -226,6 +248,171 @@ class XteleSender:
             return None
         return {"left": copy.deepcopy(left), "right": copy.deepcopy(right)}
 
+    @staticmethod
+    def _command_aligned_with_raw(
+        command: dict[str, object], data: dict[str, object]
+    ) -> bool:
+        """Reject a processed 5001 target newer than the current raw frame."""
+
+        command_timestamp = command.get("timestamp")
+        raw_timestamp = data.get("timestamp")
+        if (
+            isinstance(command_timestamp, bool)
+            or isinstance(raw_timestamp, bool)
+            or not isinstance(command_timestamp, (int, float))
+            or not isinstance(raw_timestamp, (int, float))
+        ):
+            return False
+        command_value = float(command_timestamp)
+        raw_value = float(raw_timestamp)
+        return (
+            math.isfinite(command_value)
+            and math.isfinite(raw_value)
+            and command_value <= raw_value
+        )
+
+    @staticmethod
+    def _right_b_pressed(data: dict[str, object]) -> bool | None:
+        """Return raw TS1P right-B, or ``None`` for malformed input."""
+        try:
+            buttons = data["button"]
+            right = buttons["right"]  # type: ignore[index]
+            value = right[1]  # type: ignore[index]
+        except (IndexError, KeyError, TypeError):
+            return None
+        if isinstance(value, bool):
+            return value
+        if isinstance(value, int) and value in (0, 1):
+            return bool(value)
+        return None
+
+    @classmethod
+    def _hand_targets_equivalent(cls, first: object, second: object) -> bool:
+        """Compare normalized processed/raw hand targets with small jitter."""
+
+        if not cls._valid_hand_side(first) or not cls._valid_hand_side(second):
+            return False
+        if isinstance(first, (int, float)) and not isinstance(first, bool):
+            if not isinstance(second, (int, float)) or isinstance(second, bool):
+                return False
+            return abs(float(first) - float(second)) <= 0.02
+        if not isinstance(first, list) or not isinstance(second, list):
+            return False
+        return all(
+            abs(float(left) - float(right)) <= 0.02
+            for left, right in zip(first, second)
+        )
+
+    @staticmethod
+    def _raw_right_hand_target(data: dict[str, object]) -> object | None:
+        try:
+            hand = data["hand"]
+            position = hand["position"]  # type: ignore[index]
+            return position["right"]  # type: ignore[index]
+        except (KeyError, TypeError):
+            return None
+
+    def _update_right_b_merge_gate(
+        self,
+        now: float,
+        data: dict[str, object],
+        processed_right: object | None,
+    ) -> bool:
+        """Suppress processed right hand until B release and target recovery."""
+
+        pressed = self._right_b_pressed(data)
+        if pressed is True:
+            self.right_b_merge_suppressed = True
+            self.right_b_release_started_at = None
+            self.right_b_recovery_started_at = None
+            self.right_b_baseline_candidate = None
+            self.right_b_baseline_candidate_started_at = None
+            return True
+        if pressed is None:
+            # A malformed button sample may never clear an in-progress gate.
+            self.right_b_merge_suppressed = True
+            self.right_b_release_started_at = None
+            self.right_b_recovery_started_at = None
+            self.right_b_baseline_candidate = None
+            self.right_b_baseline_candidate_started_at = None
+            return True
+        if not self.right_b_merge_suppressed:
+            self.right_b_release_started_at = None
+            self.right_b_recovery_started_at = None
+            if not self._valid_hand_side(processed_right):
+                self.right_b_baseline_candidate = None
+                self.right_b_baseline_candidate_started_at = None
+                return False
+            if not self._hand_targets_equivalent(
+                processed_right, self.right_b_baseline_candidate
+            ):
+                self.right_b_baseline_candidate = copy.deepcopy(processed_right)
+                self.right_b_baseline_candidate_started_at = now
+                return False
+            if self.right_b_baseline_candidate_started_at is None:
+                self.right_b_baseline_candidate_started_at = now
+                return False
+            baseline_seconds = max(0.1, float(self.args.cmd_max_age_s))
+            if now - self.right_b_baseline_candidate_started_at >= baseline_seconds:
+                self.right_b_processed_baseline = copy.deepcopy(processed_right)
+            return False
+        if self.right_b_release_started_at is None:
+            self.right_b_release_started_at = now
+            self.right_b_recovery_started_at = None
+            return True
+        if now - self.right_b_release_started_at < self.args.combo_release_s:
+            self.right_b_recovery_started_at = None
+            return True
+
+        # A stable raw release alone is insufficient: xTELE may retain a
+        # processed B gesture after the release edge.  Re-enable the processed
+        # side only after fresh 5001 data continuously matches either its
+        # pre-B baseline or the current raw scalar target.
+        raw_right = self._raw_right_hand_target(data)
+        recovered = self._hand_targets_equivalent(
+            processed_right, self.right_b_processed_baseline
+        ) or self._hand_targets_equivalent(processed_right, raw_right)
+        if not recovered:
+            self.right_b_recovery_started_at = None
+            return True
+        if self.right_b_recovery_started_at is None:
+            self.right_b_recovery_started_at = now
+            return True
+        recovery_seconds = max(0.1, float(self.args.cmd_max_age_s))
+        if now - self.right_b_recovery_started_at < recovery_seconds:
+            return True
+
+        self.right_b_merge_suppressed = False
+        self.right_b_release_started_at = None
+        self.right_b_recovery_started_at = None
+        self.right_b_processed_baseline = copy.deepcopy(processed_right)
+        self.right_b_baseline_candidate = copy.deepcopy(processed_right)
+        self.right_b_baseline_candidate_started_at = now
+        return False
+
+    @staticmethod
+    def _select_processed_hand_position(
+        raw_position: object,
+        processed_position: dict[str, object] | None,
+        *,
+        suppress_right: bool,
+    ) -> tuple[dict[str, object] | None, tuple[str, ...]]:
+        """Select processed sides without mutating either source structure."""
+        if processed_position is None:
+            return None, ()
+        if not suppress_right:
+            return copy.deepcopy(processed_position), ("left", "right")
+
+        # While right B is held, xTELE may emit its own right-hand gesture
+        # before our separate three-second B latch fires on the robot.  Keep
+        # the authoritative raw 5003 right-hand value, but allow an unrelated
+        # processed left-hand target to pass through.
+        if not isinstance(raw_position, dict) or "right" not in raw_position:
+            return None, ()
+        selected = copy.deepcopy(raw_position)
+        selected["left"] = copy.deepcopy(processed_position["left"])
+        return selected, ("left",)
+
     @classmethod
     def _build_payload(
         cls,
@@ -235,26 +422,44 @@ class XteleSender:
         session_seq: int,
         session_state: str,
         stop_reason: str = "",
+        suppress_processed_right: bool | None = None,
     ) -> tuple[bytes, bool]:
-        merged = False
+        merged_sides: tuple[str, ...] = ()
         try:
+            # Build from a snapshot so callers retain the unmodified raw 5003
+            # frame even when a processed 5001 hand target is selected.
+            payload_data = copy.deepcopy(data)
             position = cls._processed_hand_position(command) if command else None
-            hand = data.get("hand")
-            if position is not None and isinstance(hand, dict):
-                hand["position"] = position
-                merged = True
+            hand = payload_data.get("hand")
+            right_b_pressed = cls._right_b_pressed(payload_data)
+            if suppress_processed_right is None:
+                suppress_processed_right = right_b_pressed is not False
+            if isinstance(hand, dict):
+                selected, merged_sides = cls._select_processed_hand_position(
+                    hand.get("position"),
+                    position,
+                    suppress_right=suppress_processed_right,
+                )
+                if selected is not None:
+                    hand["position"] = selected
 
-            metadata = data.get("tg3_transport")
+            metadata = payload_data.get("tg3_transport")
             if not isinstance(metadata, dict):
                 metadata = {}
-                data["tg3_transport"] = metadata
-            if merged:
+                payload_data["tg3_transport"] = metadata
+            if merged_sides:
                 metadata["processed_hand_from_xtele_cmd"] = True
+                metadata["processed_hand_sides"] = list(merged_sides)
                 assert command is not None
                 metadata["xtele_cmd_timestamp"] = command.get("timestamp")
             else:
                 metadata.pop("processed_hand_from_xtele_cmd", None)
+                metadata.pop("processed_hand_sides", None)
                 metadata.pop("xtele_cmd_timestamp", None)
+            if suppress_processed_right and position is not None:
+                metadata["processed_right_hand_suppressed_by_b"] = True
+            else:
+                metadata.pop("processed_right_hand_suppressed_by_b", None)
             # Always overwrite untrusted source metadata.  The robot accepts
             # start/active/stop only from this sender and expected Omni peer.
             metadata["protocol_version"] = TELEOP_PROTOCOL_VERSION
@@ -266,11 +471,11 @@ class XteleSender:
             else:
                 metadata.pop("stop_reason", None)
             encoded = json.dumps(
-                data, ensure_ascii=False, separators=(",", ":")
+                payload_data, ensure_ascii=False, separators=(",", ":")
             ).encode("utf-8")
         except (TypeError, ValueError):
             raise ValueError("cannot encode xTELE session payload")
-        return encoded, merged
+        return encoded, bool(merged_sides)
 
     @staticmethod
     def _start_stop_pressed(data: dict[str, object]) -> bool:
@@ -293,9 +498,20 @@ class XteleSender:
         pressed = self._start_stop_pressed(data)
         if self.require_combo_release:
             self.combo_started_at = None
-            if not pressed:
+            if pressed:
+                self.combo_release_started_at = None
+                return None
+            if self.combo_release_started_at is None:
+                self.combo_release_started_at = now
+                return None
+            if (
+                now - self.combo_release_started_at
+                >= self.args.combo_release_s
+            ):
                 self.require_combo_release = False
+                self.combo_release_started_at = None
             return None
+        self.combo_release_started_at = None
         if not pressed:
             self.combo_started_at = None
             return None
@@ -305,6 +521,7 @@ class XteleSender:
             return None
 
         self.combo_started_at = None
+        self.combo_release_started_at = None
         self.require_combo_release = True
         if self.teleop_active:
             self.teleop_active = False
@@ -451,6 +668,7 @@ class XteleSender:
                         > self.args.source_timeout_s
                     ):
                         self.combo_started_at = None
+                        self.combo_release_started_at = None
                         if self.teleop_active:
                             self._abort_teleop(
                                 "local xTELE source became stale; a new Z+C hold "
@@ -476,6 +694,7 @@ class XteleSender:
                     # A malformed/frozen frame may never contribute time to a
                     # physical three-second start/stop hold.
                     self.combo_started_at = None
+                    self.combo_release_started_at = None
                     self.counters["dropped_malformed"] += 1
                     continue
 
@@ -487,8 +706,22 @@ class XteleSender:
                 if (
                     latest_command is not None
                     and now - self.last_command_at <= self.args.cmd_max_age_s
+                    and self._command_aligned_with_raw(latest_command, data)
                 ):
                     command = latest_command
+                processed_position = (
+                    self._processed_hand_position(command)
+                    if command is not None
+                    else None
+                )
+                processed_right = (
+                    None
+                    if processed_position is None
+                    else processed_position["right"]
+                )
+                suppress_processed_right = self._update_right_b_merge_gate(
+                    now, data, processed_right
+                )
 
                 if not self.teleop_active and transition != "stop":
                     self.counters["frames_suppressed_inactive"] += 1
@@ -528,6 +761,7 @@ class XteleSender:
                         self.teleop_session_seq,
                         session_state,
                         stop_reason,
+                        suppress_processed_right,
                     )
                 except ValueError:
                     self.counters["dropped_malformed"] += 1
@@ -543,8 +777,8 @@ class XteleSender:
                     self.start_markers_remaining -= 1
                 if transition == "stop":
                     # STOP is the final xTELE business frame.  The underlying
-                    # registered OmniSocket session remains warm for low-latency
-                    # next start and KCP delivery, but no arm data follows.
+                    # Session is flushed for bounded delivery and then closed;
+                    # the next physical START creates a fresh registration.
                     self.teleop_session_id = None
                     self.teleop_session_seq = 0
                     self.start_markers_remaining = 0
@@ -581,6 +815,15 @@ def parse_args() -> argparse.Namespace:
     parser.add_argument("--max-feedback-age-ms", type=float, default=500.0)
     parser.add_argument("--max-pending-frames", type=int, default=100)
     parser.add_argument("--start-stop-hold-s", type=float, default=3.0)
+    parser.add_argument(
+        "--combo-release-s",
+        type=float,
+        default=0.5,
+        help=(
+            "require both combo buttons to remain released for this long "
+            "before another start/stop hold can begin"
+        ),
+    )
     parser.add_argument(
         "--start-marker-frames",
         type=int,

tg3_omnisocket_transport/test_session_gate.py

@@ -1,13 +1,13 @@
 #!/usr/bin/env python3
-"""Offline tests for the EAI teleoperation session gate."""
-
 from __future__ import annotations
 
+import argparse
+import copy
 import importlib.util
 import json
 from pathlib import Path
-from types import ModuleType, SimpleNamespace
 import sys
+from types import ModuleType
 import unittest
 
 
@@ -22,75 +22,109 @@ spec = importlib.util.spec_from_file_location("omnisocket_xtele_sender", module_
 assert spec is not None and spec.loader is not None
 sender_module = importlib.util.module_from_spec(spec)
 spec.loader.exec_module(sender_module)
-XteleSender = sender_module.XteleSender
 
 
-def buttons(pressed: bool) -> dict[str, object]:
+def args() -> argparse.Namespace:
+    return argparse.Namespace(
+        server="127.0.0.1:14049",
+        peer_id="sender",
+        target_peer="robot",
+        zmq_endpoint="tcp://127.0.0.1:5003",
+        cmd_zmq_endpoint="",
+        cmd_max_age_s=0.25,
+        source_timeout_s=0.25,
+        max_feedback_age_ms=500.0,
+        max_pending_frames=100,
+        start_stop_hold_s=3.0,
+        combo_release_s=0.5,
+        start_marker_frames=50,
+        status_file="/tmp/tg3_sender_test_status.json",
+    )
+
+
+def frame(pressed: bool) -> dict[str, object]:
+    value = 1 if pressed else 0
     return {
         "button": {
-            "left": [False, False, pressed],
-            "right": [False, False, pressed],
-        },
-        "hand": {"position": {"left": 0.0, "right": 0.0}},
+            "left": [0, 0, value],
+            "right": [0, 0, value],
+        }
     }
 
 
 class SessionGateTest(unittest.TestCase):
     def setUp(self) -> None:
-        args = SimpleNamespace(
-            start_stop_hold_s=3.0,
-            start_marker_frames=50,
-            source_timeout_s=0.25,
-            max_feedback_age_ms=500.0,
-            max_pending_frames=100,
+        self.sender = sender_module.XteleSender(args())
+
+    def stable_release(self, started_at: float) -> float:
+        self.assertIsNone(
+            self.sender._update_teleop_gate(started_at, frame(False))
         )
-        self.sender = XteleSender(args)
-
-    def test_start_and_stop_each_require_a_new_continuous_hold(self) -> None:
-        # Boot requires a release, so a button held across service restart
-        # cannot start a session.
-        self.assertIsNone(self.sender._update_teleop_gate(0.0, buttons(True)))
-        self.assertIsNone(self.sender._update_teleop_gate(0.1, buttons(False)))
-
-        self.assertIsNone(self.sender._update_teleop_gate(1.0, buttons(True)))
-        self.assertIsNone(self.sender._update_teleop_gate(3.99, buttons(True)))
-        self.assertEqual(
-            self.sender._update_teleop_gate(4.01, buttons(True)), "start"
+        finished_at = started_at + 0.51
+        self.assertIsNone(
+            self.sender._update_teleop_gate(finished_at, frame(False))
         )
-        first_id = self.sender.teleop_session_id
-        self.assertTrue(self.sender.teleop_active)
-        self.assertIsNotNone(first_id)
+        self.assertFalse(self.sender.require_combo_release)
+        return finished_at
 
-        # Keeping the same hold cannot immediately toggle the new session off.
-        self.assertIsNone(self.sender._update_teleop_gate(8.0, buttons(True)))
-        self.assertTrue(self.sender.teleop_active)
-        self.assertIsNone(self.sender._update_teleop_gate(8.1, buttons(False)))
-
-        self.assertIsNone(self.sender._update_teleop_gate(9.0, buttons(True)))
-        self.assertIsNone(self.sender._update_teleop_gate(11.99, buttons(True)))
+    def test_boot_requires_stable_release_before_start(self) -> None:
+        self.assertIsNone(self.sender._update_teleop_gate(0.0, frame(True)))
+        self.assertIsNone(self.sender._update_teleop_gate(4.0, frame(True)))
+        released_at = self.stable_release(5.0)
+        self.assertIsNone(
+            self.sender._update_teleop_gate(released_at + 0.01, frame(True))
+        )
         self.assertEqual(
-            self.sender._update_teleop_gate(12.01, buttons(True)), "stop"
+            self.sender._update_teleop_gate(released_at + 3.02, frame(True)),
+            "start",
+        )
+        self.assertTrue(self.sender.teleop_active)
+
+    def test_single_false_frame_cannot_rearm_stop(self) -> None:
+        released_at = self.stable_release(0.0)
+        self.sender._update_teleop_gate(released_at + 0.01, frame(True))
+        self.assertEqual(
+            self.sender._update_teleop_gate(released_at + 3.02, frame(True)),
+            "start",
+        )
+
+        self.assertIsNone(
+            self.sender._update_teleop_gate(released_at + 3.03, frame(False))
+        )
+        self.assertIsNone(
+            self.sender._update_teleop_gate(released_at + 3.04, frame(True))
+        )
+        self.assertIsNone(
+            self.sender._update_teleop_gate(released_at + 7.00, frame(True))
+        )
+        self.assertTrue(self.sender.teleop_active)
+        self.assertTrue(self.sender.require_combo_release)
+
+    def test_stable_release_allows_separate_stop_hold(self) -> None:
+        released_at = self.stable_release(0.0)
+        self.sender._update_teleop_gate(released_at + 0.01, frame(True))
+        self.assertEqual(
+            self.sender._update_teleop_gate(released_at + 3.02, frame(True)),
+            "start",
+        )
+        second_release = self.stable_release(released_at + 3.03)
+        self.sender._update_teleop_gate(second_release + 0.01, frame(True))
+        self.assertEqual(
+            self.sender._update_teleop_gate(second_release + 3.02, frame(True)),
+            "stop",
         )
         self.assertFalse(self.sender.teleop_active)
-        self.assertEqual(self.sender.teleop_session_id, first_id)
 
-    def test_releasing_during_hold_resets_the_timer(self) -> None:
-        self.sender._update_teleop_gate(0.0, buttons(False))
-        self.sender._update_teleop_gate(1.0, buttons(True))
-        self.sender._update_teleop_gate(2.0, buttons(False))
-        self.sender._update_teleop_gate(3.0, buttons(True))
-        self.assertIsNone(self.sender._update_teleop_gate(5.9, buttons(True)))
-        self.assertEqual(
-            self.sender._update_teleop_gate(6.01, buttons(True)), "start"
-        )
-
-    def test_transport_metadata_is_overwritten(self) -> None:
-        data = buttons(False)
+    def test_untrusted_transport_metadata_is_overwritten(self) -> None:
+        data = frame(False)
         data["tg3_transport"] = {
             "protocol_version": 999,
             "session_id": "forged",
+            "session_seq": 999,
             "session_state": "stop",
+            "stop_reason": "forged",
         }
+
         payload, merged = self.sender._build_payload(
             data,
             None,
@@ -98,34 +132,230 @@ class SessionGateTest(unittest.TestCase):
             7,
             "start",
         )
+
         self.assertFalse(merged)
         metadata = json.loads(payload)["tg3_transport"]
         self.assertEqual(metadata["protocol_version"], 2)
         self.assertEqual(metadata["session_id"], "a" * 32)
         self.assertEqual(metadata["session_seq"], 7)
         self.assertEqual(metadata["session_state"], "start")
+        self.assertNotIn("stop_reason", metadata)
 
     def test_stale_or_backlogged_transport_is_rejected(self) -> None:
         class FakeSession:
-            def __init__(self, feedback_age: int, pending: int) -> None:
-                self.feedback_age = feedback_age
+            def __init__(self, feedback_age_ms: int, pending: int) -> None:
+                self.feedback_age_ms = feedback_age_ms
                 self.pending = pending
 
-            def stats(self) -> dict[str, int]:
+            @staticmethod
+            def stats() -> dict[str, int]:
                 return {"connected": 1, "registered": 1}
 
             def kcp_stats(self) -> dict[str, int]:
                 return {
                     "snd_queue": self.pending,
                     "snd_buffer": 0,
-                    "last_feedback_age_ms": self.feedback_age,
+                    "last_feedback_age_ms": self.feedback_age_ms,
                 }
 
         self.sender.session_connected_at = 0.0
         self.sender.session = FakeSession(600, 0)
-        self.assertIn("feedback stale", self.sender._session_unhealthy_reason())
+        self.assertIn(
+            "feedback stale", self.sender._session_unhealthy_reason() or ""
+        )
         self.sender.session = FakeSession(1, 101)
-        self.assertIn("pending queue", self.sender._session_unhealthy_reason())
+        self.assertIn(
+            "pending queue", self.sender._session_unhealthy_reason() or ""
+        )
+
+
+class ProcessedHandMergeTest(unittest.TestCase):
+    @staticmethod
+    def raw_frame(right_b: int) -> dict[str, object]:
+        return {
+            "button": {
+                "left": [0, 0, 0],
+                "right": [0, right_b, 0],
+            },
+            "trigger": {"left": 0.12, "right": 0.73},
+            "hand": {"position": {"left": 0.12, "right": 0.73}},
+        }
+
+    @staticmethod
+    def processed_command() -> dict[str, object]:
+        return {
+            "timestamp": 123.5,
+            "hand": {
+                "position": {
+                    "left": [0.1, 0.2, 0.3, 0.4, 0.5, 0.6],
+                    "right": [0.6, 0.5, 0.4, 0.3, 0.2, 0.1],
+                }
+            },
+        }
+
+    def test_processed_command_cannot_run_ahead_of_raw_timestamp(self) -> None:
+        command = self.processed_command()
+        self.assertFalse(
+            sender_module.XteleSender._command_aligned_with_raw(
+                command, {"timestamp": 123.4}
+            )
+        )
+        self.assertTrue(
+            sender_module.XteleSender._command_aligned_with_raw(
+                command, {"timestamp": 123.5}
+            )
+        )
+        self.assertTrue(
+            sender_module.XteleSender._command_aligned_with_raw(
+                command, {"timestamp": 124.0}
+            )
+        )
+        self.assertFalse(
+            sender_module.XteleSender._command_aligned_with_raw(
+                command, {"timestamp": "124.0"}
+            )
+        )
+
+    def build(
+        self,
+        data: dict[str, object],
+        suppress_processed_right: bool | None = None,
+    ) -> tuple[dict[str, object], bool]:
+        encoded, merged = sender_module.XteleSender._build_payload(
+            data,
+            self.processed_command(),
+            "session-id",
+            7,
+            "active",
+            suppress_processed_right=suppress_processed_right,
+        )
+        return json.loads(encoded), merged
+
+    def test_right_b_preserves_raw_right_hand_and_trigger(self) -> None:
+        raw = self.raw_frame(right_b=1)
+        original = copy.deepcopy(raw)
+
+        payload, merged = self.build(raw)
+
+        self.assertTrue(merged)
+        self.assertEqual(
+            payload["hand"]["position"]["left"],
+            self.processed_command()["hand"]["position"]["left"],
+        )
+        self.assertEqual(payload["hand"]["position"]["right"], 0.73)
+        self.assertEqual(payload["trigger"], original["trigger"])
+        self.assertEqual(
+            payload["tg3_transport"]["processed_hand_sides"], ["left"]
+        )
+        self.assertTrue(
+            payload["tg3_transport"][
+                "processed_right_hand_suppressed_by_b"
+            ]
+        )
+        self.assertEqual(raw, original, "payload building must not mutate raw 5003")
+
+    def test_right_b_release_restores_bilateral_processed_merge(self) -> None:
+        raw = self.raw_frame(right_b=0)
+
+        payload, merged = self.build(raw)
+
+        self.assertTrue(merged)
+        self.assertEqual(
+            payload["hand"]["position"],
+            self.processed_command()["hand"]["position"],
+        )
+        self.assertEqual(payload["trigger"]["right"], 0.73)
+        self.assertEqual(
+            payload["tg3_transport"]["processed_hand_sides"],
+            ["left", "right"],
+        )
+        self.assertNotIn(
+            "processed_right_hand_suppressed_by_b",
+            payload["tg3_transport"],
+        )
+
+    def test_runtime_gate_suppresses_through_stable_b_release(self) -> None:
+        sender = sender_module.XteleSender(args())
+        pressed = self.raw_frame(right_b=1)
+        released = self.raw_frame(right_b=0)
+        baseline = self.processed_command()["hand"]["position"]["right"]
+        b_gesture = [0.2, 0.688, 0.0, 0.98, 0.98, 0.98]
+
+        self.assertFalse(sender._update_right_b_merge_gate(0.0, released, baseline))
+        self.assertFalse(
+            sender._update_right_b_merge_gate(0.26, released, baseline)
+        )
+        self.assertTrue(
+            sender._update_right_b_merge_gate(1.0, pressed, b_gesture)
+        )
+        self.assertTrue(
+            sender._update_right_b_merge_gate(1.1, released, b_gesture)
+        )
+        self.assertTrue(
+            sender._update_right_b_merge_gate(1.61, released, b_gesture)
+        )
+        held_payload, _ = self.build(
+            released, suppress_processed_right=True
+        )
+        self.assertEqual(held_payload["hand"]["position"]["right"], 0.73)
+
+        # Stable B release is not enough: processed 5001 must also remain at
+        # a safe baseline for at least cmd_max_age_s before it is trusted.
+        self.assertTrue(
+            sender._update_right_b_merge_gate(1.62, released, baseline)
+        )
+        self.assertTrue(
+            sender._update_right_b_merge_gate(1.86, released, baseline)
+        )
+        self.assertFalse(
+            sender._update_right_b_merge_gate(1.88, released, baseline)
+        )
+        restored_payload, _ = self.build(
+            released, suppress_processed_right=False
+        )
+        self.assertEqual(
+            restored_payload["hand"]["position"]["right"],
+            self.processed_command()["hand"]["position"]["right"],
+        )
+
+    def test_malformed_b_cannot_clear_runtime_suppression(self) -> None:
+        sender = sender_module.XteleSender(args())
+        self.assertTrue(
+            sender._update_right_b_merge_gate(
+                1.0, self.raw_frame(right_b=1), None
+            )
+        )
+        malformed = self.raw_frame(right_b=0)
+        malformed["button"] = {"right": [0]}
+        self.assertTrue(
+            sender._update_right_b_merge_gate(2.0, malformed, 0.73)
+        )
+        self.assertIsNone(sender.right_b_release_started_at)
+
+    def test_one_early_processed_change_cannot_replace_pre_b_baseline(self) -> None:
+        sender = sender_module.XteleSender(args())
+        released = self.raw_frame(right_b=0)
+        pressed = self.raw_frame(right_b=1)
+        baseline = self.processed_command()["hand"]["position"]["right"]
+        b_gesture = [0.2, 0.688, 0.0, 0.98, 0.98, 0.98]
+
+        sender._update_right_b_merge_gate(0.0, released, baseline)
+        sender._update_right_b_merge_gate(0.26, released, baseline)
+        self.assertEqual(sender.right_b_processed_baseline, baseline)
+
+        # Model 5001 being polled just before the corresponding raw B frame.
+        # It may become a candidate, but cannot immediately replace the last
+        # stable, B-false baseline.
+        sender._update_right_b_merge_gate(1.0, released, b_gesture)
+        self.assertEqual(sender.right_b_processed_baseline, baseline)
+        sender._update_right_b_merge_gate(1.01, pressed, b_gesture)
+        sender._update_right_b_merge_gate(1.02, released, b_gesture)
+        self.assertTrue(
+            sender._update_right_b_merge_gate(1.53, released, b_gesture)
+        )
+        self.assertTrue(
+            sender._update_right_b_merge_gate(2.0, released, b_gesture)
+        )
 
 
 if __name__ == "__main__":

tg3_omnisocket_transport/tg3-omnisocket-sender.service

@@ -7,7 +7,7 @@ Wants=network-online.target
 Type=simple
 WorkingDirectory=/home/eai/tg3_omnisocket_transport
 Environment=PYTHONPATH=/home/eai/OmniSocketGo/python
-ExecStart=/usr/bin/python3 /home/eai/tg3_omnisocket_transport/omnisocket_xtele_sender.py --server 175.178.116.187:14049 --peer-id tg3-009027fa8190-iarm --target-peer tg3-009027fa8190-robot --zmq-endpoint tcp://127.0.0.1:5003 --cmd-zmq-endpoint tcp://127.0.0.1:5001 --cmd-max-age-s 0.25 --source-timeout-s 0.25 --start-stop-hold-s 3.0 --start-marker-frames 50 --max-feedback-age-ms 500 --max-pending-frames 100 --status-file /home/eai/tg3_omnisocket_transport/status.json
+ExecStart=/usr/bin/python3 /home/eai/tg3_omnisocket_transport/omnisocket_xtele_sender.py --server 175.178.116.187:14049 --peer-id tg3-009027fa8190-iarm --target-peer tg3-009027fa8190-robot --zmq-endpoint tcp://127.0.0.1:5003 --cmd-zmq-endpoint tcp://127.0.0.1:5001 --cmd-max-age-s 0.25 --source-timeout-s 0.25 --start-stop-hold-s 3.0 --combo-release-s 0.5 --start-marker-frames 500 --max-feedback-age-ms 500 --max-pending-frames 100 --status-file /home/eai/tg3_omnisocket_transport/status.json
 Restart=always
 RestartSec=1
 KillSignal=SIGINT