feat: update hand gestures and direct locomotion

tg3_local_teleop/gesture_toggle.py

@@ -1,5 +1,5 @@
 #!/usr/bin/env python3
-"""Pure state machine for the right-B pointing-hand gesture.
+"""Pure state machines for robot-side right-hand button gestures.
 
 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.
@@ -182,6 +182,145 @@ class GestureToggle:
         return "active" if self.active else "idle"
 
 
+class GuardedMomentaryGesture:
+    """Hold one gesture only while its button is pressed.
+
+    A newly armed teleoperation session must first observe a stable release,
+    preventing a button held before START from moving the hand.  Missing or
+    malformed input fails closed and requires another stable release.
+    """
+
+    def __init__(self, release_seconds: float) -> None:
+        if not math.isfinite(release_seconds) or release_seconds <= 0.0:
+            raise ValueError("gesture release time must be positive and finite")
+        self.release_seconds = float(release_seconds)
+        self.active = False
+        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.activation_count = 0
+        self.last_transition = "initialized"
+
+    def new_session(self) -> None:
+        self.active = False
+        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:
+        self.active = False
+        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 gate and report only active/inactive edge changes."""
+
+        if not math.isfinite(now):
+            raise ValueError("gesture clock must be finite")
+        was_active = self.active
+        if not armed:
+            if self.armed or self.active:
+                self.disarm()
+            return was_active != self.active
+        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.active = False
+            self.release_started_at = None
+            self.require_release = True
+            self._freeze_right_hand = True
+            self.last_transition = "input_unhealthy"
+            return was_active != self.active
+
+        if self.require_release:
+            self.active = False
+            self._freeze_right_hand = True
+            if pressed:
+                self.release_started_at = None
+            elif self.release_started_at is None:
+                self.release_started_at = now
+            elif now - self.release_started_at >= self.release_seconds:
+                self.require_release = False
+                self.release_started_at = None
+                self._freeze_right_hand = False
+                self.last_transition = "release_ready"
+            return was_active != self.active
+
+        self.release_started_at = None
+        self._freeze_right_hand = False
+        self.active = bool(pressed)
+        if self.active != was_active:
+            if self.active:
+                self.activation_count += 1
+                self.last_transition = "activated"
+            else:
+                self.last_transition = "deactivated"
+        return was_active != self.active
+
+    @property
+    def freeze_right_hand(self) -> bool:
+        return self.armed and not self.active and self._freeze_right_hand
+
+    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"
+        return "active" if self.active else "idle"
+
+
+def right_a_pressed(data: Mapping[str, Any]) -> bool | None:
+    """Return TS1P right-A 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) < 1:
+            return None
+        value = right[0]
+        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 right_b_pressed(data: Mapping[str, Any]) -> bool | None:
     """Return TS1P right-B state, or ``None`` for a malformed sample."""
 
@@ -219,7 +358,7 @@ def normalized_pose_to_positions(
     ]
 
 
-def select_right_hand_target(
+def select_hand_target(
     source_target: Sequence[int],
     previous_command: Sequence[int] | None,
     gesture_target: Sequence[int],
@@ -227,7 +366,7 @@ def select_right_hand_target(
     active: bool,
     freeze: bool,
 ) -> list[int]:
-    """Apply the right-only gesture override or fail-safe input freeze."""
+    """Apply one hand's gesture override or fail-safe input freeze."""
 
     source = list(source_target)
     gesture = list(gesture_target)
@@ -241,3 +380,22 @@ def select_right_hand_target(
             raise ValueError("cannot freeze without a complete previous command")
         return previous
     return source
+
+
+def select_right_hand_target(
+    source_target: Sequence[int],
+    previous_command: Sequence[int] | None,
+    gesture_target: Sequence[int],
+    *,
+    active: bool,
+    freeze: bool,
+) -> list[int]:
+    """Backward-compatible name for the original right-only implementation."""
+
+    return select_hand_target(
+        source_target,
+        previous_command,
+        gesture_target,
+        active=active,
+        freeze=freeze,
+    )