feat: harden TG3 teleop and add right-B point gesture
This commit is contained in:
LengedZhao
268
tg3_local_teleop/test_gesture_toggle.py
Normal file
268
tg3_local_teleop/test_gesture_toggle.py
Normal file
@@ -0,0 +1,268 @@
|
||||
#!/usr/bin/env python3
|
||||
from __future__ import annotations
|
||||
|
||||
import unittest
|
||||
|
||||
from gesture_toggle import (
|
||||
GestureToggle,
|
||||
normalized_pose_to_positions,
|
||||
right_b_pressed,
|
||||
select_right_hand_target,
|
||||
)
|
||||
|
||||
|
||||
POINT_POSE = [0.2, 0.688, 0.0, 0.98, 0.98, 0.98]
|
||||
|
||||
|
||||
class GestureToggleTest(unittest.TestCase):
|
||||
def setUp(self) -> None:
|
||||
self.gesture = GestureToggle(hold_seconds=3.0, release_seconds=0.5)
|
||||
self.gesture.new_session()
|
||||
|
||||
def stable_release(self, started_at: float) -> float:
|
||||
self.assertFalse(
|
||||
self.gesture.update(
|
||||
started_at, armed=True, input_healthy=True, pressed=False
|
||||
)
|
||||
)
|
||||
finished_at = started_at + 0.5
|
||||
self.assertFalse(
|
||||
self.gesture.update(
|
||||
finished_at, armed=True, input_healthy=True, pressed=False
|
||||
)
|
||||
)
|
||||
self.assertFalse(self.gesture.require_release)
|
||||
return finished_at
|
||||
|
||||
def test_new_session_held_button_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_continuous_three_second_holds_toggle_once_each(self) -> None:
|
||||
released_at = self.stable_release(0.0)
|
||||
self.assertFalse(
|
||||
self.gesture.update(
|
||||
released_at + 0.01,
|
||||
armed=True,
|
||||
input_healthy=True,
|
||||
pressed=True,
|
||||
)
|
||||
)
|
||||
self.assertFalse(
|
||||
self.gesture.update(
|
||||
released_at + 3.0,
|
||||
armed=True,
|
||||
input_healthy=True,
|
||||
pressed=True,
|
||||
)
|
||||
)
|
||||
self.assertTrue(
|
||||
self.gesture.update(
|
||||
released_at + 3.01,
|
||||
armed=True,
|
||||
input_healthy=True,
|
||||
pressed=True,
|
||||
)
|
||||
)
|
||||
self.assertTrue(self.gesture.active)
|
||||
self.assertEqual(self.gesture.toggle_count, 1)
|
||||
|
||||
# Keeping the same physical press held cannot turn the gesture off.
|
||||
self.assertFalse(
|
||||
self.gesture.update(
|
||||
released_at + 10.0,
|
||||
armed=True,
|
||||
input_healthy=True,
|
||||
pressed=True,
|
||||
)
|
||||
)
|
||||
self.assertTrue(self.gesture.active)
|
||||
|
||||
second_release = self.stable_release(released_at + 10.01)
|
||||
self.gesture.update(
|
||||
second_release + 0.01,
|
||||
armed=True,
|
||||
input_healthy=True,
|
||||
pressed=True,
|
||||
)
|
||||
self.assertTrue(
|
||||
self.gesture.update(
|
||||
second_release + 3.02,
|
||||
armed=True,
|
||||
input_healthy=True,
|
||||
pressed=True,
|
||||
)
|
||||
)
|
||||
self.assertFalse(self.gesture.active)
|
||||
self.assertEqual(self.gesture.toggle_count, 2)
|
||||
self.assertFalse(self.gesture.freeze_right_hand)
|
||||
|
||||
def test_short_press_freezes_and_requires_stable_release(self) -> None:
|
||||
released_at = self.stable_release(0.0)
|
||||
self.gesture.update(
|
||||
released_at + 0.01,
|
||||
armed=True,
|
||||
input_healthy=True,
|
||||
pressed=True,
|
||||
)
|
||||
self.assertTrue(self.gesture.freeze_right_hand)
|
||||
self.assertEqual(self.gesture.state, "holding_start")
|
||||
|
||||
self.gesture.update(
|
||||
released_at + 2.0,
|
||||
armed=True,
|
||||
input_healthy=True,
|
||||
pressed=False,
|
||||
)
|
||||
self.assertFalse(self.gesture.active)
|
||||
self.assertTrue(self.gesture.require_release)
|
||||
self.assertTrue(self.gesture.freeze_right_hand)
|
||||
self.gesture.update(
|
||||
released_at + 2.49,
|
||||
armed=True,
|
||||
input_healthy=True,
|
||||
pressed=False,
|
||||
)
|
||||
self.assertTrue(self.gesture.require_release)
|
||||
self.gesture.update(
|
||||
released_at + 2.5,
|
||||
armed=True,
|
||||
input_healthy=True,
|
||||
pressed=False,
|
||||
)
|
||||
self.assertFalse(self.gesture.require_release)
|
||||
self.assertFalse(self.gesture.freeze_right_hand)
|
||||
|
||||
def test_invalid_button_or_feedback_gap_cancels_pending_hold(self) -> None:
|
||||
released_at = self.stable_release(0.0)
|
||||
self.gesture.update(
|
||||
released_at + 0.01,
|
||||
armed=True,
|
||||
input_healthy=True,
|
||||
pressed=True,
|
||||
)
|
||||
self.gesture.update(
|
||||
released_at + 2.99,
|
||||
armed=True,
|
||||
input_healthy=True,
|
||||
pressed=None,
|
||||
)
|
||||
self.assertTrue(self.gesture.require_release)
|
||||
self.assertIsNone(self.gesture.hold_started_at)
|
||||
|
||||
released_at = self.stable_release(released_at + 3.0)
|
||||
self.gesture.update(
|
||||
released_at + 0.01,
|
||||
armed=True,
|
||||
input_healthy=True,
|
||||
pressed=True,
|
||||
)
|
||||
self.gesture.update(
|
||||
released_at + 2.99,
|
||||
armed=True,
|
||||
input_healthy=False,
|
||||
pressed=True,
|
||||
)
|
||||
self.assertFalse(self.gesture.active)
|
||||
self.assertTrue(self.gesture.require_release)
|
||||
self.assertIsNone(self.gesture.hold_started_at)
|
||||
|
||||
def test_feedback_gap_preserves_already_active_gesture(self) -> None:
|
||||
released_at = self.stable_release(0.0)
|
||||
self.gesture.update(
|
||||
released_at + 0.01,
|
||||
armed=True,
|
||||
input_healthy=True,
|
||||
pressed=True,
|
||||
)
|
||||
self.assertTrue(
|
||||
self.gesture.update(
|
||||
released_at + 3.02,
|
||||
armed=True,
|
||||
input_healthy=True,
|
||||
pressed=True,
|
||||
)
|
||||
)
|
||||
self.gesture.update(
|
||||
released_at + 3.03,
|
||||
armed=True,
|
||||
input_healthy=False,
|
||||
pressed=True,
|
||||
)
|
||||
self.assertTrue(self.gesture.active)
|
||||
self.assertTrue(self.gesture.require_release)
|
||||
|
||||
def test_disarm_clears_active_without_an_output_action(self) -> None:
|
||||
released_at = self.stable_release(0.0)
|
||||
self.gesture.update(
|
||||
released_at + 0.01,
|
||||
armed=True,
|
||||
input_healthy=True,
|
||||
pressed=True,
|
||||
)
|
||||
self.gesture.update(
|
||||
released_at + 3.02,
|
||||
armed=True,
|
||||
input_healthy=True,
|
||||
pressed=True,
|
||||
)
|
||||
self.assertTrue(self.gesture.active)
|
||||
self.assertFalse(
|
||||
self.gesture.update(
|
||||
released_at + 3.03,
|
||||
armed=False,
|
||||
input_healthy=True,
|
||||
pressed=True,
|
||||
)
|
||||
)
|
||||
self.assertFalse(self.gesture.active)
|
||||
self.assertEqual(self.gesture.state, "disarmed")
|
||||
self.assertTrue(self.gesture.require_release)
|
||||
|
||||
|
||||
class GestureHelpersTest(unittest.TestCase):
|
||||
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]}}))
|
||||
self.assertIsNone(right_b_pressed({"button": {"right": [0]}}))
|
||||
self.assertIsNone(right_b_pressed({"button": {"right": [0, -1]}}))
|
||||
self.assertIsNone(right_b_pressed({"button": {"right": [0, 0.5]}}))
|
||||
|
||||
def test_point_pose_converts_to_expected_brainco_positions(self) -> None:
|
||||
self.assertEqual(
|
||||
normalized_pose_to_positions(POINT_POSE, 1, 1000),
|
||||
[201, 688, 1, 980, 980, 980],
|
||||
)
|
||||
|
||||
def test_right_only_override_and_short_press_freeze(self) -> None:
|
||||
source = [401, 401, 51, 51, 51, 51]
|
||||
previous = [450, 430, 80, 90, 100, 110]
|
||||
point = [201, 688, 1, 980, 980, 980]
|
||||
self.assertEqual(
|
||||
select_right_hand_target(
|
||||
source, previous, point, active=True, freeze=False
|
||||
),
|
||||
point,
|
||||
)
|
||||
self.assertEqual(
|
||||
select_right_hand_target(
|
||||
source, previous, point, active=False, freeze=True
|
||||
),
|
||||
previous,
|
||||
)
|
||||
self.assertEqual(
|
||||
select_right_hand_target(
|
||||
source, previous, point, active=False, freeze=False
|
||||
),
|
||||
source,
|
||||
)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
Reference in New Issue
Block a user