175 lines
8.7 KiB
Python
175 lines
8.7 KiB
Python
"""Offline fake CAN tests; never open real hardware."""
|
|
import contextlib
|
|
import importlib.util
|
|
import io
|
|
import json
|
|
import sys
|
|
from pathlib import Path
|
|
import tempfile
|
|
from types import SimpleNamespace
|
|
import unittest
|
|
from unittest.mock import patch
|
|
|
|
sys.path.insert(0,str(Path(__file__).resolve().parents[1]/'scripts'))
|
|
spec = importlib.util.spec_from_file_location('fist', Path(__file__).resolve().parents[1]/'scripts/left_hand_fist_restore.py')
|
|
fist = importlib.util.module_from_spec(spec)
|
|
spec.loader.exec_module(fist)
|
|
INITIAL = [[247,76,253,0,0,253], [54,0,171,0,0,240],
|
|
[12,0,157,0,0,253], [37,0,163,0,0,253], [93,0,168,0,0,250]]
|
|
|
|
|
|
def message(**kw):
|
|
return SimpleNamespace(is_rx=True, is_error_frame=False, is_remote_frame=False, **kw)
|
|
|
|
|
|
class Bus:
|
|
def __init__(self, blocked=False, lag=False):
|
|
self.rows = [r.copy() for r in INITIAL]
|
|
self.queue, self.commands = [], []
|
|
self.blocked = blocked
|
|
self.lag = lag
|
|
self.targets = [r.copy() for r in INITIAL]
|
|
def __enter__(self): return self
|
|
def __exit__(self, *args): pass
|
|
def send(self, m, timeout=None):
|
|
assert m.arbitration_id == 0x28 and not m.is_extended_id
|
|
prop = m.data[0]
|
|
if 0x49 <= prop <= 0x4d or 0x59 <= prop <= 0x5d:
|
|
assert len(m.data)==1
|
|
raw=[2,0,2,0,0,2] if prop<0x50 else [0]*6
|
|
self.queue.append(message(arbitration_id=0x28,is_extended_id=False,data=[prop]+raw))
|
|
return
|
|
assert 0x41 <= prop <= 0x45
|
|
i = prop-0x41
|
|
if len(m.data) == 1:
|
|
if self.lag:
|
|
self.rows[i] = [a+max(-2,min(2,b-a)) for a,b in zip(self.rows[i],self.targets[i])]
|
|
self.queue.append(message(arbitration_id=0x28, is_extended_id=False, data=[prop]+self.rows[i]))
|
|
else:
|
|
assert len(m.data) == 7
|
|
self.commands.append(m)
|
|
self.targets[i] = list(m.data[1:])
|
|
if not self.blocked and not self.lag: self.rows[i] = list(m.data[1:])
|
|
def recv(self, timeout=0):
|
|
return self.queue.pop(0) if self.queue else None
|
|
|
|
|
|
class TestFist(unittest.TestCase):
|
|
def setUp(self): self.lookup=fist.LeftLookup()
|
|
def test_target_preserves_nonflexion(self):
|
|
target = fist.close_target(INITIAL,self.lookup)
|
|
for i in range(5):
|
|
for j in range(6):
|
|
if j not in (2,5): self.assertEqual(target[i][j], INITIAL[i][j])
|
|
else: self.assertLessEqual(target[i][j], INITIAL[i][j])
|
|
# Middle side swing byte 12 is preserved, NOT clipped through URDF limits.
|
|
self.assertEqual(target[2][0],12)
|
|
def test_invalid_input(self):
|
|
with self.assertRaises(ValueError): fist.close_target([[0]*6]*4,self.lookup)
|
|
with self.assertRaises(ValueError): fist.close_target([[999]*6]*5,self.lookup)
|
|
def test_default_no_can(self):
|
|
with patch('sys.argv',['fist']), contextlib.redirect_stdout(io.StringIO()):
|
|
self.assertEqual(fist.main(),0)
|
|
def run_fake(self, blocked, lag=False, open_only=False):
|
|
bus=Bus(blocked,lag)
|
|
fake=SimpleNamespace(Message=message,Bus=lambda **kwargs:bus)
|
|
with tempfile.TemporaryDirectory() as tmp:
|
|
root=Path(tmp); (root/'data').mkdir()
|
|
with patch.dict('sys.modules', {'can':fake}), patch.object(fist,'ROOT',root), \
|
|
patch.object(fist.time,'sleep',lambda _:None), \
|
|
patch('sys.argv',['fist','--execute']+(['--open'] if open_only else [])), \
|
|
contextlib.redirect_stdout(io.StringIO()), contextlib.redirect_stderr(io.StringIO()):
|
|
result=fist.main()
|
|
report=json.loads(next((root/'data').glob('left_hand_*/result.json')).read_text())
|
|
report['_events']=[json.loads(line) for line in next((root/'data').glob('left_hand_*/events.jsonl')).read_text().splitlines()]
|
|
self.assertEqual(next((root/'data').glob('left_hand_*/calibration_used.json')).read_bytes(),self.lookup.raw)
|
|
return result,report,bus
|
|
def test_close_and_restore(self):
|
|
result,report,bus=self.run_fake(False)
|
|
self.assertEqual(result,0); self.assertTrue(report['restored'])
|
|
self.assertEqual(bus.rows,INITIAL)
|
|
self.assertEqual(report['device_status_before']['fault_raw'],[[0]*6]*5)
|
|
previous=[r.copy() for r in INITIAL]
|
|
for m in bus.commands:
|
|
i=m.data[0]-0x41
|
|
self.assertLessEqual(max(abs(a-b) for a,b in zip(previous[i],m.data[1:])),2)
|
|
previous[i]=list(m.data[1:])
|
|
def test_open_only_never_closes_or_restores(self):
|
|
result,report,bus=self.run_fake(False,open_only=True)
|
|
self.assertEqual(result,0)
|
|
self.assertEqual(report['status'],'opened')
|
|
self.assertTrue(report['opened'])
|
|
previous=[r.copy() for r in INITIAL]
|
|
for m in bus.commands:
|
|
i=m.data[0]-0x41
|
|
for slot in range(6):
|
|
if slot in (2,5):
|
|
self.assertGreaterEqual(m.data[1+slot],previous[i][slot])
|
|
self.assertLessEqual(m.data[1+slot]-previous[i][slot],2)
|
|
else: self.assertEqual(m.data[1+slot],INITIAL[i][slot])
|
|
previous[i]=list(m.data[1:])
|
|
self.assertEqual(bus.rows,fist.open_target(INITIAL,self.lookup))
|
|
phases={e['context']['phase'] for e in report['_events'] if e['event']=='command_sent'}
|
|
self.assertEqual(phases,{'open'})
|
|
def test_blocked_stops_without_false_restore(self):
|
|
result,report,bus=self.run_fake(True)
|
|
self.assertEqual(result,1); self.assertFalse(report['restored'])
|
|
self.assertTrue(report['hold_command_sent'])
|
|
self.assertIn('fault_raw',report['device_status_after_failure'])
|
|
self.assertLess(len(bus.commands),160*5)
|
|
self.assertEqual(report['failure']['reason'],'tracking_error')
|
|
self.assertEqual(report['failure']['details']['context']['phase'],'close')
|
|
self.assertTrue(report['failure']['details']['offending_joints'])
|
|
self.assertTrue(any(e['event']=='fault' for e in report['_events']))
|
|
def test_realistic_lag_is_logged_not_silently_accepted(self):
|
|
result,report,bus=self.run_fake(False,lag=True)
|
|
self.assertEqual(result,1)
|
|
detail=report['failure']['details']
|
|
self.assertIn('actual_raw',detail); self.assertIn('command_raw',detail)
|
|
worst=detail['offending_joints'][0]
|
|
self.assertIn('error_deg',worst); self.assertIn('joint',worst)
|
|
self.assertGreater(abs(worst['error_u8']),18)
|
|
self.assertFalse(report['restored'])
|
|
def test_json_replaces_linear_pip_mapping(self):
|
|
self.assertAlmostEqual(self.lookup.angle('index_pip',31),1.54800055)
|
|
self.assertEqual(self.lookup.command('index_pip',.95),130)
|
|
target=fist.close_target(INITIAL,self.lookup)
|
|
self.assertEqual(target[1][5],130)
|
|
def test_motor_names_follow_json_not_legacy_thumb_swap(self):
|
|
desc=self.lookup.describe(INITIAL)
|
|
self.assertEqual(desc['thumb_cmc_yaw']['raw_u8'],247)
|
|
self.assertEqual(desc['thumb_cmc_roll']['raw_u8'],76)
|
|
self.assertEqual(desc['thumb_ip']['raw_u8'],253)
|
|
self.assertTrue(desc['thumb_ip']['passive_estimate'])
|
|
def test_no_out_of_range_clipping_or_passive_commands(self):
|
|
with self.assertRaises(ValueError): self.lookup.command('index_pip',100)
|
|
with self.assertRaises(ValueError): self.lookup.command('index_dip',.5)
|
|
def test_reject_wrong_side_and_malformed_table(self):
|
|
for kind in ('side','length','motor'):
|
|
d=json.loads(self.lookup.raw)
|
|
if kind=='side': d['side']='right'
|
|
if kind=='length': d['joints']['index_pip']['angle_rad'].pop()
|
|
if kind=='motor': d['joints']['thumb_cmc_roll']['motor_index']=10
|
|
with tempfile.TemporaryDirectory() as tmp:
|
|
p=Path(tmp)/'bad.json';p.write_text(json.dumps(d))
|
|
with self.assertRaises(ValueError): fist.LeftLookup(p)
|
|
def test_timeout_retains_property_and_partial_feedback(self):
|
|
class Missing(Bus):
|
|
def send(self,m,timeout=None):
|
|
if m.data==[0x43]: return
|
|
super().send(m,timeout)
|
|
with self.assertRaises(fist.MotionFault) as cm:
|
|
fist.read_all(Missing(),SimpleNamespace(Message=message))
|
|
self.assertEqual(cm.exception.details['can_property'],0x43)
|
|
self.assertEqual(len(cm.exception.details['partial_feedback']),2)
|
|
def test_every_active_table_point_round_trips_in_angle(self):
|
|
from g20_left_lookup import EXPECTED
|
|
for name in EXPECTED:
|
|
for raw in range(256):
|
|
angle=self.lookup.angle(name,raw)
|
|
recovered=self.lookup.command(name,angle,near=raw)
|
|
self.assertEqual(recovered,raw)
|
|
|
|
|
|
if __name__ == '__main__': unittest.main()
|