"""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()