Initial import of ORISYS arm workstation

2026-10-16 17:27:30 +08:00
commit 2b74c50ba8
745 changed files with 1199101 additions and 0 deletions
@@ -0,0 +1,74 @@
+"""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,[])