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

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,