orisys/orisys_arm
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Update workstation UI, ROS task control and device integration

Browse Source
This commit is contained in:
cheng
2026-10-20 15:46:13 +08:00
parent 2b74c50ba8
commit e5b06df6da
141 changed files with 91520 additions and 748 deletions
Download Patch File Download Diff File Expand all files Collapse all files
station/test/test_motion_commands.py
+179 -5
View File
@@ -11,7 +11,7 @@ class Gateway:
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 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):
@@ -25,6 +25,7 @@ class Gateway:
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:
@@ -38,19 +39,51 @@ class MotionCommandsTest(TestCase):
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')])
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):
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('skip_stage3:=true',commands[0]);self.assertIn('d435_move_only:=false',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('no_descent:=true',commands[1]);self.assertIn('skip_stage3:=true',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():
@@ -72,3 +105,144 @@ class MotionCommandsTest(TestCase):
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'))