cheng
75 lines
4.5 KiB
Python
75 lines
4.5 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): self.calls.append(('require',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)
|
|
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,[('require','89')])
|
|
|
|
def test_detection_and_alignment_are_separate_stages(self):
|
|
commands=[]
|
|
def run(key, argv, event, env, cwd):
|
|
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('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('no_descent:=true',commands[1]);self.assertIn('skip_stage3:=true',commands[1])
|
|
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,[])
|