73 lines
3.8 KiB
Python
73 lines
3.8 KiB
Python
import sys
|
|
import tempfile
|
|
import unittest
|
|
from pathlib import Path
|
|
from unittest.mock import Mock
|
|
sys.path.insert(0,str(Path(__file__).resolve().parents[1]/'scripts'))
|
|
from trajectory_supervisor import Trajectory,Supervisor
|
|
from joint_follow_adapter import ReferenceGate,SdkFollowBackend,ControlLease,native_state_observation,bind_sdk
|
|
|
|
|
|
class AdapterTest(unittest.TestCase):
|
|
def setUp(self):self.t=Trajectory([[0.]*7,[.005]*7])
|
|
def test_reference_stream(self):
|
|
gate=ReferenceGate(self.t)
|
|
for t in (0,.02,.04):self.assertEqual(len(gate.accept(t,self.t.sample(t)['position'])),7)
|
|
def test_bad_packet_latches(self):
|
|
gate=ReferenceGate(self.t)
|
|
with self.assertRaises(ValueError):gate.accept(0,[float('nan')]*7)
|
|
with self.assertRaises(ValueError):gate.accept(0,[0.]*7)
|
|
def test_jump_and_unplanned_reference(self):
|
|
for time,q in [(.1,self.t.sample(.1)['position']),(.02,[.1]*7)]:
|
|
gate=ReferenceGate(self.t);gate.accept(0,[0.]*7)
|
|
with self.assertRaises(ValueError):gate.accept(time,q)
|
|
def test_no_live_connection(self):
|
|
backend=SdkFollowBackend()
|
|
with self.assertRaises(RuntimeError):backend.connect_live()
|
|
with self.assertRaises(RuntimeError):backend.send_checked(ReferenceGate(self.t),0,[0.]*7)
|
|
self.assertFalse(backend.request_controlled_stop()['stopped'])
|
|
def test_mock_sdk_abi_and_left_side(self):
|
|
fake=Mock();fake.lbot_joint_follow.return_value=True;bind_sdk(fake)
|
|
backend=SdkFollowBackend();backend.library=fake;backend.handle=123
|
|
r=backend.send_checked(ReferenceGate(self.t),0,[0.]*7)
|
|
self.assertFalse(r['arrival_confirmed'])
|
|
self.assertEqual(fake.lbot_joint_follow.call_args.args[1],0)
|
|
self.assertEqual(list(fake.lbot_joint_follow.call_args.args[2]),[0.]*7)
|
|
fake.lbot_init.assert_not_called()
|
|
def test_send_failure_no_retry(self):
|
|
backend=SdkFollowBackend();backend.handle=123;backend.library=Mock()
|
|
backend.library.lbot_joint_follow.return_value=False
|
|
for _ in range(2):
|
|
with self.assertRaises(RuntimeError):backend.send_checked(ReferenceGate(self.t),0,[0.]*7)
|
|
self.assertEqual(backend.library.lbot_joint_follow.call_count,1)
|
|
def test_lease(self):
|
|
with tempfile.TemporaryDirectory() as d:
|
|
path=Path(d)/'lock'
|
|
with ControlLease(path):
|
|
with self.assertRaises(RuntimeError):
|
|
with ControlLease(path):pass
|
|
with ControlLease(path):pass
|
|
def test_state_mapping(self):
|
|
message={'joint_state':dict(name=[f'L{i}_joint' for i in range(1,8)],position=[0.]*7,velocity=[0.]*7,header={'stamp':{'sec':1}})}
|
|
self.assertEqual(native_state_observation(message,1.,1)['seq'],1)
|
|
message['joint_state']['name'][0]='R1_joint'
|
|
with self.assertRaises(ValueError):native_state_observation(message,1.,1)
|
|
|
|
def test_supervisor_to_mock_follow_end_to_end(self):
|
|
backend=SdkFollowBackend();backend.handle=123;backend.library=Mock()
|
|
backend.library.lbot_joint_follow.return_value=True
|
|
gate=ReferenceGate(self.t);supervisor=Supervisor(self.t)
|
|
for step in range(100):
|
|
now=step*.02;ref=self.t.sample(max(0,now-.02))
|
|
observation=dict(seq=step,received=now,position=ref['position'],velocity=ref['velocity'])
|
|
actions=supervisor.tick(now,observation,acknowledgement=True if step==1 else None)
|
|
for action in actions:
|
|
if action['action']=='reference_only' and action['elapsed_s']<=self.t.duration:
|
|
backend.send_checked(gate,action['elapsed_s'],action['position'])
|
|
self.assertEqual(supervisor.state,'DONE')
|
|
self.assertGreater(backend.library.lbot_joint_follow.call_count,10)
|
|
backend.library.lbot_init.assert_not_called()
|
|
|
|
|
|
if __name__=='__main__':unittest.main()
|