96 lines
3.3 KiB
Python
96 lines
3.3 KiB
Python
import sys
|
|
import unittest
|
|
from pathlib import Path
|
|
sys.path.insert(0, str(Path(__file__).resolve().parents[1] / 'src/linker_hand_ros2_sdk'))
|
|
from linker_hand_ros2_sdk.trial_guard import TrialGuard
|
|
|
|
class GuardTests(unittest.TestCase):
|
|
def test_no_input_no_command(self):
|
|
self.assertIsNone(TrialGuard([100]*20).step(100))
|
|
|
|
def test_anchor_no_jump(self):
|
|
g = TrialGuard([100]*20)
|
|
g.accept([255]*20, 0)
|
|
self.assertEqual(g.step(0.05), [100]*20)
|
|
|
|
def test_slew_and_envelope(self):
|
|
g = TrialGuard([100]*20)
|
|
g.accept([100]*20, 0)
|
|
for i in range(1, 101):
|
|
now = i * 0.05
|
|
g.accept([255]*20, now)
|
|
out = g.step(now)
|
|
self.assertLessEqual(max(out), min(110, 100 + i*0.5 + 0.5))
|
|
self.assertEqual(out, [110]*20)
|
|
|
|
def test_timeout_latches_even_if_input_resumes(self):
|
|
g = TrialGuard([100]*20)
|
|
g.accept([100]*20, 0)
|
|
self.assertIsNone(g.step(0.31))
|
|
g.accept([120]*20, 0.32)
|
|
self.assertIsNone(g.step(0.33))
|
|
self.assertTrue(g.locked)
|
|
|
|
def test_callback_gap_latches(self):
|
|
g = TrialGuard([100]*20)
|
|
g.accept([100]*20, 0)
|
|
g.accept([120]*20, 0.31)
|
|
self.assertTrue(g.locked)
|
|
|
|
def test_half_mapping(self):
|
|
g = TrialGuard([100]*20, 0.5)
|
|
g.accept([200]*20, 0)
|
|
self.assertEqual(g.target, [150]*20)
|
|
self.assertEqual(g.step(0), [100]*20)
|
|
self.assertEqual(g.step(0.1), [100]*20) # 0.5-unit capped step
|
|
|
|
def test_full_mapping_reaches_target_slowly(self):
|
|
g = TrialGuard([100]*20, 1.0)
|
|
g.accept([200]*20, 0)
|
|
self.assertEqual(g.target, [200]*20)
|
|
self.assertEqual(g.step(0), [100]*20)
|
|
for i in range(1, 241):
|
|
now = i * 0.05
|
|
g.accept([200]*20, now)
|
|
out = g.step(now)
|
|
self.assertLessEqual(max(out), min(200, 100 + i*0.5 + 0.5))
|
|
self.assertEqual(out, [200]*20)
|
|
|
|
def test_scaled_modes_bounds_and_timeout(self):
|
|
for scale in (0.5, 1.0):
|
|
for initial, target in ((0,255),(255,0)):
|
|
g = TrialGuard([initial]*20, scale)
|
|
g.accept([target]*20, 0)
|
|
self.assertTrue(all(0 <= x <= 255 for x in g.target))
|
|
self.assertIsNone(g.step(0.31))
|
|
self.assertTrue(g.locked)
|
|
|
|
def test_follow_slew_and_stall_cap(self):
|
|
g = TrialGuard([100]*20, 1.0, 60.0)
|
|
g.accept([200]*20, 0)
|
|
self.assertEqual(g.step(0.05), [103]*20)
|
|
g.accept([200]*20, 0.2)
|
|
self.assertEqual(g.step(0.2), [106]*20)
|
|
self.assertIsNone(g.step(0.51))
|
|
self.assertTrue(g.locked)
|
|
|
|
def test_bad_slew(self):
|
|
for rate in (0, -1, 61, float('inf'), float('nan')):
|
|
with self.assertRaises(ValueError):
|
|
TrialGuard([100]*20, 1.0, rate)
|
|
|
|
def test_bad_scale(self):
|
|
for scale in (-1, 0.75, 2, float('nan')):
|
|
with self.assertRaises(ValueError):
|
|
TrialGuard([100]*20, scale)
|
|
|
|
def test_invalid_input_latches(self):
|
|
for values in ([1]*19, [float('nan')]*20, [256]*20, [-1]*20):
|
|
g = TrialGuard([100]*20)
|
|
g.accept(values, 0)
|
|
self.assertTrue(g.locked)
|
|
self.assertIsNone(g.step(0))
|
|
|
|
if __name__ == '__main__':
|
|
unittest.main()
|