feat: update hand gestures and direct locomotion

tg3_local_teleop/test_gesture_toggle.py

@@ -4,9 +4,12 @@ from __future__ import annotations
 import unittest
 
 from gesture_toggle import (
+    GuardedMomentaryGesture,
     GestureToggle,
     normalized_pose_to_positions,
+    right_a_pressed,
     right_b_pressed,
+    select_hand_target,
     select_right_hand_target,
 )
 
@@ -227,6 +230,13 @@ class GestureToggleTest(unittest.TestCase):
 
 
 class GestureHelpersTest(unittest.TestCase):
+    def test_right_a_is_index_zero_and_malformed_values_fail_closed(self) -> None:
+        self.assertTrue(right_a_pressed({"button": {"right": [1, 0, 0]}}))
+        self.assertFalse(right_a_pressed({"button": {"right": [0, 1, 1]}}))
+        self.assertIsNone(right_a_pressed({"button": {"right": []}}))
+        self.assertIsNone(right_a_pressed({"button": {"right": [-1]}}))
+        self.assertIsNone(right_a_pressed({"button": {"right": [0.5]}}))
+
     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]}}))
@@ -263,6 +273,81 @@ class GestureHelpersTest(unittest.TestCase):
             source,
         )
 
+    def test_same_point_target_can_be_applied_to_both_hands(self) -> None:
+        point = [201, 688, 1, 980, 980, 980]
+        sources = {
+            "left": [401, 401, 51, 51, 51, 51],
+            "right": [402, 402, 52, 52, 52, 52],
+        }
+        previous = {
+            "left": [410, 420, 60, 70, 80, 90],
+            "right": [430, 440, 100, 110, 120, 130],
+        }
+        selected = {
+            side: select_hand_target(
+                sources[side],
+                previous[side],
+                point,
+                active=True,
+                freeze=False,
+            )
+            for side in ("left", "right")
+        }
+        self.assertEqual(selected, {"left": point, "right": point})
+
+
+class GuardedMomentaryGestureTest(unittest.TestCase):
+    def setUp(self) -> None:
+        self.gesture = GuardedMomentaryGesture(release_seconds=0.5)
+        self.gesture.new_session()
+
+    def test_held_at_session_start_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_press_is_immediate_after_stable_release_and_release_restores(self) -> None:
+        self.gesture.update(0.0, armed=True, input_healthy=True, pressed=False)
+        self.gesture.update(0.5, armed=True, input_healthy=True, pressed=False)
+        self.assertFalse(self.gesture.require_release)
+        self.assertTrue(
+            self.gesture.update(
+                0.51, armed=True, input_healthy=True, pressed=True
+            )
+        )
+        self.assertTrue(self.gesture.active)
+        self.assertEqual(self.gesture.activation_count, 1)
+        self.assertTrue(
+            self.gesture.update(
+                0.52, armed=True, input_healthy=True, pressed=False
+            )
+        )
+        self.assertFalse(self.gesture.active)
+        self.assertFalse(self.gesture.freeze_right_hand)
+
+    def test_malformed_input_deactivates_and_requires_release(self) -> None:
+        self.gesture.update(0.0, armed=True, input_healthy=True, pressed=False)
+        self.gesture.update(0.5, armed=True, input_healthy=True, pressed=False)
+        self.gesture.update(0.6, armed=True, input_healthy=True, pressed=True)
+        self.assertTrue(self.gesture.active)
+        self.assertTrue(
+            self.gesture.update(
+                0.7, armed=True, input_healthy=True, pressed=None
+            )
+        )
+        self.assertFalse(self.gesture.active)
+        self.assertTrue(self.gesture.require_release)
+        self.assertTrue(self.gesture.freeze_right_hand)
+
 
 if __name__ == "__main__":
     unittest.main()