69 lines
4.3 KiB
Python
69 lines
4.3 KiB
Python
"""Synthetic geometry checks only; no hardware or ROS nodes."""
|
|
import json, os, subprocess, sys, tempfile, unittest
|
|
from pathlib import Path
|
|
import numpy as np
|
|
from scipy.spatial.transform import Rotation as Rot
|
|
|
|
ROOT = Path(__file__).resolve().parents[1]
|
|
sys.path.insert(0, str(ROOT/'scripts'))
|
|
from arm_contract import check_sample_arm, check_calibration_arm
|
|
from storage import save_json
|
|
|
|
class OfflineTests(unittest.TestCase):
|
|
def test_left_and_right_joint_solve(self):
|
|
for arm in ('left', 'right'):
|
|
with self.subTest(arm=arm), tempfile.TemporaryDirectory(prefix='handeye solve ') as td:
|
|
cap = Path(td)
|
|
points = np.zeros((40, 3)); points[:, :2] = np.mgrid[0:8, 0:5].T.reshape(-1, 2)*.027
|
|
Tbb = np.eye(4); Tbb[:3,:3] = Rot.from_euler('xyz', [.2,-.1,.3]).as_matrix(); Tbb[:3,3] = [.4,.2,.05]
|
|
Tcb = np.eye(4); Tcb[:3,:3] = Rot.from_euler('xyz', [-.1,.2,.3]).as_matrix(); Tcb[:3,3] = [.1,-.2,.5]
|
|
d = np.array([.02,.01,.11])
|
|
camera = dict(board_objp=points.tolist(), board_grid=[8,5], T_cam_board=Tcb.tolist())
|
|
(cap/'camera.json').write_text(json.dumps(camera))
|
|
samples=[]
|
|
for i,k in enumerate([0,3,6,8,12,14,16,20,22,24,27,30]):
|
|
R = Rot.from_euler('xyz', [.13*i, -.07*(i%4), .09*(i%5)])
|
|
b = (points[k+1]+points[k+8])/2
|
|
t = Tbb[:3,:3]@b + Tbb[:3,3] - R.as_matrix()@d
|
|
samples.append(dict(arm=arm,corner=k,pos=t.tolist(),quat_xyzw=R.as_quat().tolist()))
|
|
(cap/'touch_samples.json').write_text(json.dumps(dict(arm=arm,samples=samples)))
|
|
subprocess.run([sys.executable,str(ROOT/'scripts/solve_base_board.py'),'--capture',str(cap),'--arm',arm],check=True,capture_output=True,text=True)
|
|
actual=json.loads((cap/'camera.json').read_text())
|
|
np.testing.assert_allclose(actual['T_base_cam_opencv'],Tbb@np.linalg.inv(Tcb),atol=1e-6)
|
|
np.testing.assert_allclose(actual['fingertip_offset_m'],d,atol=1e-6)
|
|
self.assertEqual(actual['coordinate_frame'],'base_link')
|
|
self.assertEqual(actual['calibration_arm'],arm)
|
|
self.assertTrue(list((cap/'history').glob('camera_*.json')))
|
|
|
|
def test_cross_arm_data_rejected(self):
|
|
with self.assertRaises(SystemExit): check_sample_arm({'arm':'right','samples':[]},'left')
|
|
with self.assertRaises(SystemExit): check_sample_arm({'arm':'left','samples':[{'arm':'right'}]},'left')
|
|
with self.assertRaises(SystemExit): check_calibration_arm({'calibration_arm':'right'},'left')
|
|
|
|
def test_atomic_backup(self):
|
|
with tempfile.TemporaryDirectory() as td:
|
|
target=Path(td)/'sample.json'; save_json(target,{'v':1}); save_json(target,{'v':2})
|
|
self.assertEqual(json.loads(target.read_text()),{'v':2})
|
|
archive=list((target.parent/'history').glob('sample_*.json'))
|
|
self.assertEqual(len(archive),1)
|
|
self.assertEqual(json.loads(archive[0].read_text()),{'v':1})
|
|
|
|
def test_board_detection_and_guides(self):
|
|
with tempfile.TemporaryDirectory(prefix='handeye board ') as td:
|
|
cap=Path(td)
|
|
image=np.full((420,600,3),255,np.uint8)
|
|
for row in range(6):
|
|
for col in range(9):
|
|
if (row+col)%2==0:image[30+row*60:90+row*60,30+col*60:90+col*60]=0
|
|
# PPM is written without importing a second OpenCV into the solve environment.
|
|
(cap/'rgb.png').write_bytes(b'P6'+bytes([10])+b'600 420'+bytes([10])+b'255'+bytes([10])+image.tobytes())
|
|
(cap/'intrinsics.txt').write_text('500 500 300 210')
|
|
(cap/'camera.json').write_text(json.dumps({'D':[0,0,0,0,0]}))
|
|
command=[os.environ.get('HANDEYE_ROS_PYTHON','/usr/bin/python3'),str(ROOT/'scripts/handeye_solve.py'),'--capture',str(cap),'--cols','8','--rows','5','--square','.027','--arm','right']
|
|
subprocess.run(command,check=True,capture_output=True,text=True)
|
|
camera=json.loads((cap/'camera.json').read_text())
|
|
self.assertEqual(camera['board_grid'],[8,5]); self.assertLess(camera['board_reproj_px'],.5)
|
|
self.assertTrue((cap/'align_lines_left.png').is_file());self.assertTrue((cap/'align_lines_right.png').is_file())
|
|
|
|
if __name__=='__main__':unittest.main()
|