cheng
135 lines
6.7 KiB
Python
135 lines
6.7 KiB
Python
import tempfile
|
|
import threading
|
|
import time
|
|
from unittest import TestCase
|
|
from station.control_service import StationService
|
|
|
|
|
|
class Gateway:
|
|
active_arm='89'
|
|
def __init__(self): self.started=None;self.requests=[]
|
|
def graph(self):
|
|
return {'nodes':['dh_pgea'],'topics':['/gripper/states'],
|
|
'services':['/gripper/open','/gripper/close','/gripper/initialize','/dh_pgea/execute']}
|
|
def publisher_count(self, topic): return 1 if topic=='/gripper/states' else 0
|
|
def states(self):
|
|
elapsed=time.monotonic()-self.started if self.started else 0
|
|
moving=bool(self.started and elapsed<getattr(self,'motion_duration',.05))
|
|
last=self.requests[-1][0] if self.requests else ''
|
|
if last=='/gripper/close':
|
|
permille=0. if not moving else 400.
|
|
elif last=='/gripper/open':
|
|
permille=1000. if not moving else 400.
|
|
else:
|
|
permille=500.
|
|
return {},{'89':{'connected':True,'moving':moving,
|
|
'grip_state':0 if moving else 1,'permille':permille,
|
|
'gripped':False,'opening_mm':permille/1000.*20}}
|
|
def call(self,typ,name,request,event,timeout):
|
|
self.requests.append((name,request));self.started=time.monotonic()
|
|
return type('Reply',(),{'success':True,'message':'accepted'})()
|
|
def close(self): pass
|
|
|
|
|
|
class GripperCommandsTest(TestCase):
|
|
def test_waits_for_physical_completion_and_rejects_bounds(self):
|
|
gateway=Gateway();gateway.motion_duration=.25
|
|
with tempfile.TemporaryDirectory() as root:
|
|
service=StationService(root=root,gateway=gateway)
|
|
try:
|
|
arm=service.arms['89'];start=time.monotonic()
|
|
result=service._gripper('gripper_tune',arm,{'kind':'pos','pos':500,'force':33,'speed':25},threading.Event())
|
|
self.assertTrue(result['ok']);self.assertGreater(time.monotonic()-start,.2)
|
|
self.assertEqual(gateway.requests[0][0],'/dh_pgea/execute')
|
|
self.assertEqual(gateway.requests[0][1].command.position,arm['stroke_mm']/2)
|
|
count=len(gateway.requests)
|
|
with self.assertRaises(ValueError):
|
|
service._gripper('gripper_tune',arm,{'kind':'pos','pos':500,'force':999,'speed':25},threading.Event())
|
|
self.assertEqual(len(gateway.requests),count)
|
|
finally: service.close()
|
|
|
|
def test_configuration_changes_never_retarget_commands(self):
|
|
from pathlib import Path
|
|
gateway=Gateway()
|
|
with tempfile.TemporaryDirectory() as root:
|
|
service=StationService(root=root,gateway=gateway)
|
|
try:
|
|
path=Path(root)/'site.yaml';path.write_text('robot: before')
|
|
service.config_path=path;service._config_signature=service._signature()
|
|
path.write_text('robot: different-device')
|
|
with self.assertRaisesRegex(RuntimeError,'配置已变化'):
|
|
service.submit('gripper_probe','89')
|
|
self.assertEqual(gateway.requests,[])
|
|
self.assertTrue(service._frozen_config.is_file())
|
|
finally: service.close()
|
|
|
|
def test_open_close_do_not_require_initialized(self):
|
|
gateway=Gateway()
|
|
original=gateway.states
|
|
def states():
|
|
arms,grips=original()
|
|
row=dict(grips['89']);row['initialized']=False
|
|
return arms,{'89':row}
|
|
gateway.states=states
|
|
with tempfile.TemporaryDirectory() as root:
|
|
service=StationService(root=root,gateway=gateway)
|
|
try:
|
|
closed=service._gripper('gripper_close',service.arms['89'],{},threading.Event())
|
|
self.assertTrue(closed['ok'])
|
|
self.assertEqual(gateway.requests[-1][0],'/gripper/close')
|
|
opened=service._gripper('gripper_open',service.arms['89'],{},threading.Event())
|
|
self.assertTrue(opened['ok'])
|
|
self.assertEqual(gateway.requests[-1][0],'/gripper/open')
|
|
finally: service.close()
|
|
|
|
def test_need_init_summary_does_not_block_open_close(self):
|
|
from station.operator_gripper_state import present
|
|
row=present('89',rotary=False,stroke_mm=20.0,
|
|
reading={'connected':True,'initialized':False,'permille':981,
|
|
'grip_state':1,'source':'ROS'})
|
|
self.assertIn('零点未标定', row['summary'])
|
|
self.assertIn('开口 19.6 mm', row['summary'])
|
|
self.assertNotIn('不会动', row['summary'])
|
|
self.assertNotIn('请先点到位', row['summary'])
|
|
|
|
def test_open_close_init_call_dh_pgea_triggers_and_close_must_reach_zero(self):
|
|
gateway=Gateway()
|
|
with tempfile.TemporaryDirectory() as root:
|
|
service=StationService(root=root,gateway=gateway)
|
|
try:
|
|
arm=service.arms['89']
|
|
closed=service._gripper('gripper_close',arm,{},threading.Event())
|
|
self.assertTrue(closed['ok'])
|
|
self.assertEqual(gateway.requests[-1][0],'/gripper/close')
|
|
opened=service._gripper('gripper_open',arm,{},threading.Event())
|
|
self.assertTrue(opened['ok'])
|
|
self.assertEqual(gateway.requests[-1][0],'/gripper/open')
|
|
inited=service._gripper('gripper_init',arm,{},threading.Event())
|
|
self.assertTrue(inited['ok'])
|
|
self.assertEqual(gateway.requests[-1][0],'/gripper/initialize')
|
|
finally: service.close()
|
|
|
|
def test_close_fails_when_firmware_says_arrived_but_opening_remains(self):
|
|
gateway=Gateway()
|
|
def states():
|
|
return {},{'89':{'connected':True,'moving':False,'grip_state':1,'permille':130.,
|
|
'gripped':False,'opening_mm':2.6}}
|
|
gateway.states=states
|
|
with tempfile.TemporaryDirectory() as root:
|
|
service=StationService(root=root,gateway=gateway)
|
|
try:
|
|
result=service._gripper('gripper_close',service.arms['89'],{},threading.Event())
|
|
self.assertFalse(result['ok'])
|
|
self.assertIn('未闭合到位', result['message'])
|
|
self.assertIn('2.6', result['message'])
|
|
finally: service.close()
|
|
|
|
def test_gripper_plan_starts_dh_pgea_without_auto_init(self):
|
|
from station.launch_plans import gripper_plan
|
|
argv=gripper_plan({'id':'89','rotary':False,'port':'/dev/ttyUSB0','stroke_mm':20.0,'gripper':'PGEA-100-20-W'},
|
|
{'baud':115200,'force_pct':20,'speed_pct':50})
|
|
joined=' '.join(str(part) for part in argv)
|
|
self.assertIn('arm_application.dh_pgea_node', joined)
|
|
self.assertIn('auto_init:=false', joined)
|
|
self.assertNotIn('gripper_node', joined)
|