cheng
249 lines
16 KiB
Python
249 lines
16 KiB
Python
"""Exercise routing and stop semantics without launching ROS or moving hardware."""
|
|
import threading
|
|
from pathlib import Path
|
|
from unittest import TestCase
|
|
from unittest.mock import patch
|
|
from station.control_service import StationService
|
|
|
|
|
|
class Gateway:
|
|
active_arm = '89'
|
|
def __init__(self): self.calls=[];self.fail_stop=False
|
|
def graph(self): return {'nodes': [], 'topics': [], 'services': []}
|
|
def states(self): return {}, {}
|
|
def require_arm(self, aid, event, *, resume=True): self.calls.append(('require',aid) if resume else ('check_only',aid))
|
|
def move_joints(self, arm, joints, speed, event):
|
|
self.calls.append(('home',arm['id'],list(joints),speed));return {'ok':True}
|
|
def stop_arm(self, arm):
|
|
self.calls.append(('stop',arm['id']))
|
|
if self.fail_stop: raise RuntimeError('no acknowledgement')
|
|
def confirm_stopped(self, arm):
|
|
self.calls.append(('confirmed',arm['id']));return {'ok':True,'message':'stopped'}
|
|
def close(self): pass
|
|
|
|
|
|
class MotionCommandsTest(TestCase):
|
|
def setUp(self):
|
|
self.gateway=Gateway();self.service=StationService(gateway=self.gateway)
|
|
self.service.site.pop('remote_camera_preview',None)
|
|
def tearDown(self): self.service.close()
|
|
def test_camera_refresh_checks_both_cameras_without_robot_control(self):
|
|
with patch.object(self.service,'_camera',side_effect=[RuntimeError('D405 missing'),{'ok':True,'message':'ready'}]) as camera:
|
|
result=self.service.execute('camera_refresh','89','eye_in_hand',{},threading.Event())
|
|
self.assertFalse(result['ok']);self.assertEqual(camera.call_count,2)
|
|
self.assertFalse(camera.call_args_list[0].args[1]);self.assertTrue(camera.call_args_list[1].args[1])
|
|
self.assertEqual(self.gateway.calls,[])
|
|
self.assertTrue(result['cameras'][1]['ok'])
|
|
|
|
def test_enable_reuses_running_stack_without_second_driver(self):
|
|
with patch.object(self.gateway,'graph',return_value={'nodes':['controller_manager'],'topics':[],'services':[]}),patch.object(self.service.processes,'start') as start:
|
|
result=self.service.execute('real_up','89','eye_in_hand',{},threading.Event())
|
|
self.assertTrue(result['ok']);start.assert_not_called()
|
|
self.assertEqual(self.gateway.calls,[('check_only','89')])
|
|
self.assertIsNone(self.service._motion_arm)
|
|
|
|
def test_detection_and_alignment_are_separate_stages(self):
|
|
commands=[]
|
|
def run(key, argv, event, env, cwd, **kwargs):
|
|
commands.append(argv);return {'ok':True}
|
|
with patch.object(self.service,'_camera'),patch.object(self.service,'_environment',return_value={}),patch.object(self.service.processes,'run',side_effect=run):
|
|
self.service.execute('stage_track','89','eye_in_hand',{},threading.Event())
|
|
self.service.execute('stage_locate','89','eye_in_hand',{},threading.Event())
|
|
self.assertIn('skip_stage1:=false',commands[0]);self.assertIn('skip_stage2:=true',commands[0])
|
|
self.assertIn('run_a7_after_insert:=false',commands[0]);self.assertIn('completion_linger_sec:=0.3',commands[0]);self.assertIn('skip_stage3:=true',commands[0]);self.assertIn('d435_move_only:=false',commands[0])
|
|
self.assertIn('skip_stage1:=true',commands[1]);self.assertIn('skip_stage2:=false',commands[1])
|
|
self.assertIn('run_a7_after_insert:=false',commands[1]);self.assertIn('no_descent:=false',commands[1]);self.assertIn('skip_stage3:=true',commands[1])
|
|
def test_rotation_is_a7_only_and_does_not_open_cameras(self):
|
|
with patch.object(self.service,'_rotation_ready'),patch.object(self.service,'_camera') as camera, \
|
|
patch.object(self.service,'_environment',return_value={}),patch.object(self.service.processes,'run',return_value={'ok':True}) as run:
|
|
self.assertTrue(self.service.execute('stage_screw','89','eye_in_hand',{},threading.Event())['ok'])
|
|
self.assertEqual(run.call_args.args[1][-2:],['-m','arm_application.a7_rotation_node'])
|
|
camera.assert_not_called()
|
|
|
|
def test_full_flow_runs_rotation_only_after_alignment_success(self):
|
|
from station.tasks import Cancelled
|
|
for first_ok,cancelled,expected_runs in ((True,False,2),(False,False,1),(True,True,1)):
|
|
event=threading.Event();commands=[]
|
|
def run(key,argv,*args,**kwargs):
|
|
commands.append(argv)
|
|
if cancelled:event.set()
|
|
return {'ok':first_ok}
|
|
with patch.object(self.service,'_rotation_ready'),patch.object(self.service,'_camera'), \
|
|
patch.object(self.service,'_environment',return_value={}),patch.object(self.service.processes,'run',side_effect=run):
|
|
if cancelled:
|
|
with self.assertRaises(Cancelled):self.service.execute('pipeline','89','eye_in_hand',{},event)
|
|
else:self.service.execute('pipeline','89','eye_in_hand',{},event)
|
|
self.assertEqual(len(commands),expected_runs)
|
|
self.assertIn('no_descent:=false',commands[0])
|
|
self.assertIn('run_a7_after_insert:=false',commands[0])
|
|
if expected_runs==2:self.assertEqual(commands[1][-1],'arm_application.a7_rotation_node')
|
|
|
|
def test_rotation_requires_own_typed_gripper_before_any_task(self):
|
|
with patch.object(self.service.processes,'run') as run:
|
|
with self.assertRaisesRegex(RuntimeError,'平行夹爪'):
|
|
self.service.execute('stage_screw','89','eye_in_hand',{},threading.Event())
|
|
run.assert_not_called()
|
|
|
|
def test_home_uses_each_arms_named_initial_pose(self):
|
|
import yaml
|
|
for aid,arm in self.service.arms.items():
|
|
robot=next(row for row in self.service.site['robot']['robots'] if row['sn']==arm['sn'])
|
|
path=Path(robot['named_poses_file'])
|
|
if not path.is_absolute():path=self.service.root/path
|
|
expected=yaml.safe_load(path.read_text())['named_pose_server']['ros__parameters']['named_poses.home']
|
|
self.service.execute('reset_home',aid,'eye_in_hand',{},threading.Event())
|
|
self.assertEqual(self.gateway.calls[-1][1:3],(aid,expected))
|
|
def test_stop_targets_running_arm_even_after_ui_selection_changes(self):
|
|
self.service._motion_arm='89'
|
|
result=self.service.stop_commands('91')
|
|
self.assertTrue(result['ok']);self.assertEqual(self.gateway.calls,[('stop','89'),('confirmed','89')])
|
|
def test_failed_stop_blocks_motion_until_stop_confirmed(self):
|
|
self.gateway.fail_stop=True
|
|
self.assertFalse(self.service.stop_commands('89')['ok'])
|
|
with self.assertRaisesRegex(RuntimeError,'停止未确认'):self.service.submit('reset_home','89')
|
|
self.gateway.fail_stop=False
|
|
self.assertTrue(self.service.stop_commands('89')['ok']);self.assertEqual(self.service._stop_error,'')
|
|
def test_shutdown_cancel_does_not_stop_unrelated_controller(self):
|
|
self.assertTrue(self.service.stop_commands()['ok']);self.assertEqual(self.gateway.calls,[])
|
|
|
|
def test_external_camera_is_not_adopted_or_started_again(self):
|
|
env={'SITE_CAM_SERIAL':'test-serial','SITE_CAM_NAME':'d405_left'}
|
|
with patch.object(self.service,'_environment',return_value=env), \
|
|
patch.object(self.gateway,'publisher_count',return_value=1,create=True), \
|
|
patch.object(self.gateway,'publisher_nodes',return_value=['foreign_camera'],create=True), \
|
|
patch.object(self.service.processes,'start') as start:
|
|
with self.assertRaisesRegex(RuntimeError,'外部'):
|
|
self.service._camera(self.service.arms['89'],False,threading.Event())
|
|
start.assert_not_called()
|
|
def test_camera_process_without_frames_is_not_ready(self):
|
|
with patch.object(self.service,'_camera_ownership',return_value={'owned':True,'conflict':False}), \
|
|
patch.object(self.gateway,'camera_health',return_value={'fresh':False},create=True):
|
|
with self.assertRaisesRegex(RuntimeError,'没有新帧'):
|
|
self.service._ensure_camera('d405_left',[],{},threading.Event())
|
|
|
|
def test_camera_refresh_preserves_healthy_matching_stream(self):
|
|
with patch.object(self.service,'_camera_ownership',return_value={'owned':True,'conflict':False}), \
|
|
patch.object(self.gateway,'camera_health',return_value={'fresh':True,'width':1280,'height':720},create=True), \
|
|
patch.object(self.gateway,'publisher_count',return_value=1,create=True), \
|
|
patch.object(self.service.processes,'stop') as stop:
|
|
result=self.service._ensure_camera('d405_left',['-p','width:=1280','-p','height:=720'],{},threading.Event(),refresh=True)
|
|
self.assertTrue(result['ok']);stop.assert_not_called()
|
|
|
|
def test_camera_refresh_keeps_usb2_fallback_resolution(self):
|
|
with patch.object(self.service,'_camera_ownership',return_value={'owned':True,'conflict':False}), \
|
|
patch.object(self.gateway,'camera_health',return_value={'fresh':True,'width':640,'height':480},create=True), \
|
|
patch.object(self.gateway,'publisher_count',return_value=1,create=True), \
|
|
patch.object(self.service.processes,'stop') as stop:
|
|
result=self.service._ensure_camera('d405_left',['-p','width:=1280','-p','height:=720'],{},threading.Event(),refresh=True)
|
|
self.assertTrue(result['ok']);stop.assert_not_called()
|
|
|
|
def test_camera_refresh_reconnects_only_camera_even_while_arm_is_enabling(self):
|
|
from unittest.mock import Mock
|
|
self.service._motion_arm='89'
|
|
active=[True]
|
|
def snapshot():return {'camera:d405_left':{'running':active[0]}}
|
|
def stop(key):
|
|
self.assertEqual(key,'camera:d405_left');active[0]=False
|
|
def start(*args):active[0]=True;return Mock(),Path('/tmp/fake-camera.log')
|
|
with patch.object(self.service,'_camera_ownership',side_effect=[
|
|
{'owned':True,'conflict':False}, {'owned':False,'conflict':False},
|
|
{'owned':True,'conflict':False}]), \
|
|
patch.object(self.gateway,'camera_health',side_effect=[{'fresh':False},{'fresh':True}],create=True), \
|
|
patch.object(self.gateway,'clear_camera_health',create=True), \
|
|
patch.object(self.gateway,'publisher_count',return_value=0,create=True), \
|
|
patch.object(self.service.processes,'snapshot',side_effect=snapshot), \
|
|
patch.object(self.service.processes,'stop',side_effect=stop), \
|
|
patch.object(self.service.processes,'start',side_effect=start):
|
|
self.assertTrue(self.service._ensure_camera('d405_left',[],{},threading.Event(),refresh=True)['ok'])
|
|
self.assertEqual(self.gateway.calls,[])
|
|
|
|
def test_same_name_does_not_prove_camera_ownership(self):
|
|
name='d405_left'
|
|
with patch.object(self.gateway,'publisher_nodes',return_value=[self.service._camera_node_name(name)],create=True):
|
|
self.assertFalse(self.service._camera_ownership(name,{})['owned'])
|
|
self.assertTrue(self.service._camera_ownership(name,{'camera:'+name:{'running':True}})['owned'])
|
|
with patch.object(self.gateway,'publisher_nodes',return_value=[self.service._camera_node_name(name),'foreign_camera'],create=True):
|
|
self.assertFalse(self.service._camera_ownership(name,{'camera:'+name:{'running':True}})['owned'])
|
|
|
|
def test_independent_commands_can_run_during_tracking(self):
|
|
import time
|
|
gate=threading.Event()
|
|
def execute(action,arm,mode,extras,event):
|
|
if action=='stage_track': gate.wait(2)
|
|
return {'ok':True}
|
|
with patch.object(self.service,'execute',side_effect=execute):
|
|
motion=self.service.submit('stage_track','89')
|
|
try:
|
|
with self.assertRaisesRegex(RuntimeError,'检测跟踪'):
|
|
self.service.submit('reset_home','89')
|
|
with self.assertRaisesRegex(RuntimeError,'检测跟踪'):
|
|
self.service.submit('reset_home','91')
|
|
tokens=[self.service.submit('gripper_open','89'),self.service.submit('tactile_reset','89'),
|
|
self.service.submit('camera_refresh','89'),
|
|
self.service.submit('handeye_show','89'),
|
|
self.service.submit('gripper_open','91')]
|
|
deadline=time.monotonic()+1
|
|
while time.monotonic()<deadline and any(self.service.tasks.snapshot(t)['state'] in ('queued','running') for t in tokens): time.sleep(.01)
|
|
self.assertTrue(all(self.service.tasks.snapshot(t)['state']=='succeeded' for t in tokens))
|
|
self.assertIn(self.service.tasks.snapshot(motion)['state'],('queued','running'))
|
|
finally: gate.set()
|
|
|
|
def test_feedback_interruption_cancels_task_before_stop_and_cleans_up(self):
|
|
started=threading.Event(); release=threading.Event()
|
|
def operation(event):
|
|
started.set(); release.wait(2); return {'ok':True}
|
|
token=self.service.tasks.submit('stage_track',{'robot-control'},operation,context={'arm':'89'})
|
|
self.assertTrue(started.wait(1))
|
|
def stop(arm):
|
|
self.assertTrue(self.service.tasks.snapshot(token)['cancel_requested'])
|
|
release.set(); return {'ok':True}
|
|
with patch.object(self.gateway,'motion_interruption',return_value='feedback lost',create=True),patch.object(self.service,'stop_commands',side_effect=stop) as stopped:
|
|
self.service._check_motion_interruption(); stopped.assert_called_once_with('89')
|
|
self.assertEqual(self.service.error,'')
|
|
self.assertEqual(self.service._arm_errors['89'],'feedback lost')
|
|
import time
|
|
deadline=time.monotonic()+2
|
|
while self.service.tasks.snapshot(token)['state']=='running' and time.monotonic()<deadline: time.sleep(.01)
|
|
self.assertEqual(self.service.tasks.snapshot(token)['state'],'cancelled')
|
|
|
|
def test_rejected_command_is_logged_before_returning_error(self):
|
|
with self.assertRaisesRegex(ValueError,'未注册'):
|
|
self.service.submit('invalid_action','89')
|
|
events=self.service.snapshot()['operation_log']['events']
|
|
own=[r for r in events if r['action']=='invalid_action']
|
|
self.assertEqual([r['kind'] for r in own],['received','rejected'])
|
|
self.assertIn('未注册',own[-1]['message'])
|
|
|
|
def test_sensor_refresh_restarts_only_owned_sensor_nodes(self):
|
|
from unittest.mock import Mock
|
|
records={'tactile:left':{'running':True},'tactile:right':{'running':True},
|
|
'stack:89':{'running':True},'gripper:89':{'running':True}}
|
|
with patch.object(self.service.processes,'snapshot',return_value=records), \
|
|
patch.object(self.service.processes,'stop') as stop, \
|
|
patch.object(self.service.processes,'start') as start, \
|
|
patch.object(self.service,'_environment',return_value={}), \
|
|
patch.object(self.gateway,'publisher_nodes',return_value=[],create=True), \
|
|
patch.object(self.gateway,'watch_tactile',create=True):
|
|
result=self.service.execute('tactile_refresh','89','eye_in_hand',{},threading.Event())
|
|
self.assertTrue(result['ok'])
|
|
self.assertEqual([c.args[0] for c in stop.call_args_list],['tactile:left','tactile:right'])
|
|
self.assertEqual(start.call_count,2)
|
|
with patch.object(self.service,'execute',return_value={'ok':True}):
|
|
token=self.service.submit('tactile_refresh','89')
|
|
self.assertIsNotNone(self.service.tasks.snapshot(token))
|
|
|
|
def test_reference_reset_calls_only_owned_sensor_service_without_restarting(self):
|
|
reply=type('Reply',(),{'success':True,'message':'reference reset'})()
|
|
with patch.object(self.service.processes,'snapshot',return_value={'tactile:left':{'running':True}}), \
|
|
patch.object(self.service.processes,'stop') as stop, \
|
|
patch.object(self.service.processes,'start') as start, \
|
|
patch.object(self.gateway,'call',return_value=reply,create=True) as call, \
|
|
patch.object(self.gateway,'clear_tactile_reference',create=True) as clear:
|
|
result=self.service.execute('tactile_reset','89','eye_in_hand',{},threading.Event())
|
|
self.assertTrue(result['ok']);self.assertIn('right',result['message'])
|
|
self.assertEqual(call.call_args.args[1],'/tactile/left/reset_reference')
|
|
clear.assert_called_once_with('left');start.assert_not_called();stop.assert_not_called()
|
|
from station.command_policy import resources_for
|
|
for action in ('stage_track','stage_locate','stage_screw','pipeline'):
|
|
self.assertFalse(resources_for('tactile_reset','89') & resources_for(action,'89'))
|