贴面后不再抬回悬高；MoveL 切回关节后不再把整段位移当前进速度。

搜孔开局会抬到悬高再落回 hole_hover_z_m，螺纹还没咬合臂就离开孔口。没有接触传感器，到位只认 final_gap 算出的法兰高度，贴面后停在这个 Z。09:25 厂家 MoveL 切回 NRT_JOINT_POSITION 的第一拍把整段关节差算成 1.686 rad/s，前瞻再推 0.07 rad，光心从 5 mm 冲到 276 mm 后急停。切回后钉住结束瞬间的关节角并清零速度，JTC 还指着出发前的关节时不放行。

Co-authored-by: Cursor <cursoragent@cursor.com>

install/arm_application/lib/arm_application/assembly_pipeline

@@ -1,10 +1,10 @@
-#!/usr/bin/env python3
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','assembly_pipeline'
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','assembly_pipeline'
 import re
 import sys
 
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application'
+__requires__ = 'arm-application==0.1.0'
 
 try:
     from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 
 if __name__ == '__main__':
     sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application', 'console_scripts', 'assembly_pipeline')())
+    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'assembly_pipeline')())

install/arm_application/lib/arm_application/demo_named_poses

@@ -1,10 +1,10 @@
-#!/usr/bin/env python3
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','demo_named_poses'
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','demo_named_poses'
 import re
 import sys
 
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application'
+__requires__ = 'arm-application==0.1.0'
 
 try:
     from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 
 if __name__ == '__main__':
     sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application', 'console_scripts', 'demo_named_poses')())
+    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'demo_named_poses')())

install/arm_application/lib/arm_application/dh_pgea

@@ -1,10 +1,10 @@
-#!/usr/bin/env python3
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','dh_pgea'
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','dh_pgea'
 import re
 import sys
 
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application'
+__requires__ = 'arm-application==0.1.0'
 
 try:
     from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 
 if __name__ == '__main__':
     sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application', 'console_scripts', 'dh_pgea')())
+    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'dh_pgea')())

install/arm_application/lib/arm_application/dh_pgea_cli

@@ -1,10 +1,10 @@
-#!/usr/bin/env python3
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','dh_pgea_cli'
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','dh_pgea_cli'
 import re
 import sys
 
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application'
+__requires__ = 'arm-application==0.1.0'
 
 try:
     from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 
 if __name__ == '__main__':
     sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application', 'console_scripts', 'dh_pgea_cli')())
+    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'dh_pgea_cli')())

install/arm_application/lib/arm_application/dual_gripper

@@ -1,10 +1,10 @@
-#!/usr/bin/env python3
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','dual_gripper'
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','dual_gripper'
 import re
 import sys
 
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application'
+__requires__ = 'arm-application==0.1.0'
 
 try:
     from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 
 if __name__ == '__main__':
     sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application', 'console_scripts', 'dual_gripper')())
+    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'dual_gripper')())

install/arm_application/lib/arm_application/dual_gripper_cli

@@ -1,10 +1,10 @@
-#!/usr/bin/env python3
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','dual_gripper_cli'
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','dual_gripper_cli'
 import re
 import sys
 
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application'
+__requires__ = 'arm-application==0.1.0'
 
 try:
     from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 
 if __name__ == '__main__':
     sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application', 'console_scripts', 'dual_gripper_cli')())
+    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'dual_gripper_cli')())

install/arm_application/lib/arm_application/gauge_assembly

@@ -1,10 +1,10 @@
-#!/usr/bin/env python3
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','gauge_assembly'
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','gauge_assembly'
 import re
 import sys
 
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application'
+__requires__ = 'arm-application==0.1.0'
 
 try:
     from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 
 if __name__ == '__main__':
     sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application', 'console_scripts', 'gauge_assembly')())
+    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'gauge_assembly')())

install/arm_application/lib/arm_application/goto_ee_plane

@@ -1,10 +1,10 @@
-#!/usr/bin/env python3
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','goto_ee_plane'
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','goto_ee_plane'
 import re
 import sys
 
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application'
+__requires__ = 'arm-application==0.1.0'
 
 try:
     from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 
 if __name__ == '__main__':
     sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application', 'console_scripts', 'goto_ee_plane')())
+    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'goto_ee_plane')())

install/arm_application/lib/arm_application/handeye_calibrate

@@ -1,10 +1,10 @@
-#!/usr/bin/env python3
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','handeye_calibrate'
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','handeye_calibrate'
 import re
 import sys
 
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application'
+__requires__ = 'arm-application==0.1.0'
 
 try:
     from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 
 if __name__ == '__main__':
     sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application', 'console_scripts', 'handeye_calibrate')())
+    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'handeye_calibrate')())

install/arm_application/lib/arm_application/handeye_teach

@@ -1,10 +1,10 @@
-#!/usr/bin/env python3
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','handeye_teach'
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','handeye_teach'
 import re
 import sys
 
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application'
+__requires__ = 'arm-application==0.1.0'
 
 try:
     from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 
 if __name__ == '__main__':
     sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application', 'console_scripts', 'handeye_teach')())
+    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'handeye_teach')())

install/arm_application/lib/arm_application/handeye_waypoints

@@ -1,10 +1,10 @@
-#!/usr/bin/env python3
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','handeye_waypoints'
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','handeye_waypoints'
 import re
 import sys
 
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application'
+__requires__ = 'arm-application==0.1.0'
 
 try:
     from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 
 if __name__ == '__main__':
     sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application', 'console_scripts', 'handeye_waypoints')())
+    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'handeye_waypoints')())

install/arm_application/lib/arm_application/hole_detect

@@ -1,10 +1,10 @@
-#!/usr/bin/env python3
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','hole_detect'
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','hole_detect'
 import re
 import sys
 
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application'
+__requires__ = 'arm-application==0.1.0'
 
 try:
     from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 
 if __name__ == '__main__':
     sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application', 'console_scripts', 'hole_detect')())
+    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'hole_detect')())

install/arm_application/lib/arm_application/move_to_pose_client

@@ -1,10 +1,10 @@
-#!/usr/bin/env python3
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','move_to_pose_client'
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','move_to_pose_client'
 import re
 import sys
 
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application'
+__requires__ = 'arm-application==0.1.0'
 
 try:
     from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 
 if __name__ == '__main__':
     sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application', 'console_scripts', 'move_to_pose_client')())
+    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'move_to_pose_client')())

install/arm_application/lib/arm_application/perception_node

@@ -1,10 +1,10 @@
-#!/usr/bin/env python3
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','perception_node'
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','perception_node'
 import re
 import sys
 
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application'
+__requires__ = 'arm-application==0.1.0'
 
 try:
     from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 
 if __name__ == '__main__':
     sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application', 'console_scripts', 'perception_node')())
+    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'perception_node')())

install/arm_application/lib/arm_application/realsense_color_pub

@@ -1,10 +1,10 @@
-#!/usr/bin/env python3
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','realsense_color_pub'
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','realsense_color_pub'
 import re
 import sys
 
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application'
+__requires__ = 'arm-application==0.1.0'
 
 try:
     from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 
 if __name__ == '__main__':
     sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application', 'console_scripts', 'realsense_color_pub')())
+    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'realsense_color_pub')())

install/arm_application/lib/arm_application/realsense_view

@@ -1,10 +1,10 @@
-#!/usr/bin/env python3
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','realsense_view'
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','realsense_view'
 import re
 import sys
 
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application'
+__requires__ = 'arm-application==0.1.0'
 
 try:
     from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 
 if __name__ == '__main__':
     sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application', 'console_scripts', 'realsense_view')())
+    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'realsense_view')())

install/arm_application/lib/arm_application/run_joint_trajectory

@@ -1,10 +1,10 @@
-#!/usr/bin/env python3
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','run_joint_trajectory'
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','run_joint_trajectory'
 import re
 import sys
 
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application'
+__requires__ = 'arm-application==0.1.0'
 
 try:
     from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 
 if __name__ == '__main__':
     sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application', 'console_scripts', 'run_joint_trajectory')())
+    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'run_joint_trajectory')())

install/arm_application/lib/arm_application/servo_node

@@ -1,10 +1,10 @@
-#!/usr/bin/env python3
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','servo_node'
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','servo_node'
 import re
 import sys
 
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application'
+__requires__ = 'arm-application==0.1.0'
 
 try:
     from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 
 if __name__ == '__main__':
     sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application', 'console_scripts', 'servo_node')())
+    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'servo_node')())

install/arm_application/lib/arm_application/stage2_locate

@@ -1,10 +1,10 @@
-#!/usr/bin/env python3
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','stage2_locate'
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','stage2_locate'
 import re
 import sys
 
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application'
+__requires__ = 'arm-application==0.1.0'
 
 try:
     from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 
 if __name__ == '__main__':
     sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application', 'console_scripts', 'stage2_locate')())
+    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'stage2_locate')())

install/arm_application/lib/arm_application/stage3_search

@@ -1,10 +1,10 @@
-#!/usr/bin/env python3
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','stage3_search'
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','stage3_search'
 import re
 import sys
 
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application'
+__requires__ = 'arm-application==0.1.0'
 
 try:
     from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 
 if __name__ == '__main__':
     sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application', 'console_scripts', 'stage3_search')())
+    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'stage3_search')())

install/arm_application/lib/arm_application/tactile

@@ -1,10 +1,10 @@
-#!/usr/bin/env python3
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','tactile'
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','tactile'
 import re
 import sys
 
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application'
+__requires__ = 'arm-application==0.1.0'
 
 try:
     from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 
 if __name__ == '__main__':
     sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application', 'console_scripts', 'tactile')())
+    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'tactile')())

install/arm_application/lib/arm_application/tactile_cli

@@ -1,10 +1,10 @@
-#!/usr/bin/env python3
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','tactile_cli'
+#!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','tactile_cli'
 import re
 import sys
 
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application'
+__requires__ = 'arm-application==0.1.0'
 
 try:
     from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 
 if __name__ == '__main__':
     sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application', 'console_scripts', 'tactile_cli')())
+    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'tactile_cli')())

install/arm_application/lib/python3.11/site-packages/arm_application/dh_pgea.py

@@ -213,6 +213,34 @@ WIRING_HINT = (
 )
 
 
+def open_failure_text(port: str, exc: BaseException) -> str:
+    """串口打不开时把「被占用」和「没应答」分开。
+
+    独占锁失败（errno 11）以前和真没上电写成同一句「需要 24 V DC」，
+    界面上看起来像缺自动上电，实际是别的进程还握着这个口。
+    """
+    text = str(exc).lower()
+    errno = getattr(exc, "errno", None)
+    busy = errno in (11, 16) or any(
+        token in text
+        for token in (
+            "temporarily unavailable",
+            "device or resource busy",
+            "could not exclusively lock",
+            "resource busy",
+        )
+    )
+    if busy:
+        return (
+            f"无法独占打开 {port}：{exc}。"
+            "串口已被别的进程占用（夹爪驱动或状态读取）。"
+        )
+    return (
+        f"无法打开 {port}：{exc}。"
+        "需要 24 V DC 供电 + RS485 接线，且当前用户在 dialout 组内。"
+    )
+
+
 class DhPgeaGripper:
     """面向 PGE / PGEA 的线程安全 Modbus RTU 客户端（官方寄存器映射）。"""
 
@@ -257,10 +285,7 @@ class DhPgeaGripper:
                 exclusive=True,
             )
         except Exception as exc:
-            raise DhPgeaError(
-                f"无法打开 {self.port}：{exc}。"
-                "需要 24 V DC 供电 + RS485 接线，且当前用户在 dialout 组内。"
-            ) from exc
+            raise DhPgeaError(open_failure_text(self.port, exc)) from exc
         try:
             self._ser.dtr = True
         except Exception:
@@ -562,6 +587,36 @@ def _scan_bus(port: str) -> int:
     return 1
 
 
+def ctrl_values(
+    cmd: str,
+    value: str = "",
+    force: int = 50,
+    speed: int = 50,
+) -> Optional[tuple[float, float, float, float]]:
+    """节点已经占着串口时，改发 ``/gripper/ctrl`` 的四个数。
+
+    顺序是初始化、位置千分比、力度、速度。``probe`` / ``status`` 只读，返回 None。
+    """
+    name = str(cmd or "").strip().lower()
+    if name in ("probe", "status"):
+        return None
+    if name == "init":
+        return (1.0, 0.0, float(force), float(speed))
+    if name == "open":
+        pos = 1000
+    elif name == "close":
+        pos = 0
+    elif name in ("pos", "move"):
+        if str(value).strip() == "":
+            raise ValueError("pos 需要 0–1000")
+        pos = int(round(float(value)))
+        if pos < 0 or pos > 1000:
+            raise ValueError("位置只接受 0–1000")
+    else:
+        raise ValueError(f"节点在线时不支持 {name}")
+    return (0.0, float(pos), float(force), float(speed))
+
+
 def _cli(argv: Optional[list[str]] = None) -> int:
     site = _site_defaults()
     ap = argparse.ArgumentParser(

install/arm_application/lib/python3.11/site-packages/arm_application/handeye_calibrate.py

@@ -137,9 +137,7 @@ class HandeyeCalibrate(TolerantNode):
         self.declare_parameter('pin_hold_sec', 0.45)        # 每拍多长
         self.declare_parameter('skip_unstable_poses', True)
 
-        from arm_application.site_io import ros2_root
-
-        self.declare_parameter('output_dir', str(ros2_root() / 'handeye' / 'tmp'))
+        self.declare_parameter('output_dir', '/home/cc/ros2/handeye/tmp')
         self.declare_parameter('result_file', '')
         self.declare_parameter('cleanup_images', True)
         # 眼在手上：AX=XB 的 gripper 用 Studio 工具 TCP，再折成 cam→法兰写 T_4x4。

install/arm_application/lib/python3.11/site-packages/arm_application/handeye_teach.py

@@ -108,12 +108,10 @@ class RdkBridge:
         self.robot_sn = robot_sn
         self._last_q: Optional[List[float]] = None
         self._io = threading.Lock()
-        from arm_application.site_io import ros2_root
-
-        py = os.environ.get('RDK_PYTHON', '').strip() or str(
-            Path.home() / 'rdk_py310' / 'bin' / 'python'
-        )
-        bridge = ros2_root() / 'scripts' / 'rdk_drag_bridge.py'
+        py = os.environ.get('RDK_PYTHON', '/home/cc/rdk_py310/bin/python')
+        bridge = Path.home() / 'ros2' / 'scripts' / 'rdk_drag_bridge.py'
+        if not bridge.is_file():
+            bridge = Path('/home/cc/ros2/scripts/rdk_drag_bridge.py')
         if not bridge.is_file():
             raise FileNotFoundError(f'找不到 rdk_drag_bridge.py：{bridge}')
 
@@ -222,13 +220,6 @@ class RdkBridge:
                 return self._last_q
         return self._last_q
 
-    def move_joints(self, joints: List[float], speed: float = 0.15) -> dict:
-        """按关节阻抗把臂缓慢送到已记录的 7 个关节角。speed 单位 rad/s。"""
-        return self._ask(
-            {'cmd': 'move_joints', 'q': [float(v) for v in joints[:7]], 'speed': float(speed)},
-            timeout=180.0,
-        )
-
     def close(self) -> None:
         try:
             self._ask({'cmd': 'quit'}, timeout=3.0)
@@ -248,9 +239,7 @@ class RdkBridge:
 class HandeyeTeach(TolerantNode):
     def __init__(self) -> None:
         super().__init__('handeye_teach')
-        from arm_application.site_io import ros2_root
-
-        self.declare_parameter('output_dir', str(ros2_root() / 'handeye' / 'tmp'))
+        self.declare_parameter('output_dir', '/home/cc/ros2/handeye/tmp')
         self.declare_parameter('width', 1280)
         self.declare_parameter('height', 720)
         self.declare_parameter('fps', 15)

install/arm_application/lib/python3.11/site-packages/arm_application/operator_arm_mesh.py

@@ -0,0 +1,339 @@
+"""用非夕给的 Rizon4 外壳网格摆出当前姿态。
+
+网格和安装位姿来自 ``flexiv_description`` 的 ``visual_parameters.yaml``
+与 ``meshes/Rizon4/visual/*.obj``，不另画一套简图。面数按固定步长抽稀，
+只为了界面里能转得动，几何仍是厂家的。
+"""
+
+from __future__ import annotations
+
+import math
+from pathlib import Path
+
+import numpy as np
+import yaml
+
+from arm_application.operator_arm_state import link_frames
+from arm_application.site_io import ros2_root
+
+# 单个连杆原始面数大约 5000。再按 700 抽稀会在壳上留出三角洞，
+# 白壳和灰蓝关节分不出来。这里保留全部面。
+_FACES_PER_LINK = 8000
+_MODELS: dict[str, list[tuple[list[tuple[float, float, float]], tuple]]] | None = None
+
+
+def posed_triangles(q: list[float]) -> list[tuple]:
+    """返回基座系下的三角面 ``(a, b, c, 明暗, rgb)``。
+
+    ``rgb`` 来自同目录 ``.mtl`` 的 ``Kd``。外壳是白的，关节壳体是灰蓝，
+    这是厂家导出的两种材质，不是另涂的颜色。
+    """
+    world, shade, rgb = _posed_arrays(q)
+    return [
+        (
+            (float(world[i, 0, 0]), float(world[i, 0, 1]), float(world[i, 0, 2])),
+            (float(world[i, 1, 0]), float(world[i, 1, 1]), float(world[i, 1, 2])),
+            (float(world[i, 2, 0]), float(world[i, 2, 1]), float(world[i, 2, 2])),
+            float(shade[i]),
+            (float(rgb[i, 0]), float(rgb[i, 1]), float(rgb[i, 2])),
+        )
+        for i in range(shade.shape[0])
+    ]
+
+
+def posed_gl(q: list[float]) -> tuple[bytes, int, tuple[float, float, float], float]:
+    """给显卡的一份网格：交错 ``xyzrgb``、顶点数、中心、半径。
+
+    拖视角只改观察矩阵，不再把四万个面在界面线程里重投影。
+    """
+    world, shade, rgb = _posed_arrays(q)
+    light = (0.55 + 0.45 * shade)[:, None]
+    colored = (rgb * light).astype(np.float32)
+    pos = world.reshape(-1, 3).astype(np.float32)
+    col = np.repeat(colored, 3, axis=0)
+    center = pos.mean(axis=0)
+    radius = float(np.linalg.norm(pos - center, axis=1).max())
+    packed = np.concatenate([pos, col], axis=1).astype(np.float32, copy=False)
+    return (
+        packed.tobytes(),
+        int(pos.shape[0]),
+        (float(center[0]), float(center[1]), float(center[2])),
+        max(radius, 1e-4),
+    )
+
+
+def _posed_arrays(q: list[float]):
+    """每个连杆做一次矩阵乘法，不再对每个顶点走 Python 循环。"""
+    frames = link_frames(q)
+    worlds: list[np.ndarray] = []
+    shades: list[np.ndarray] = []
+    colors: list[np.ndarray] = []
+    for link, parts in _models().items():
+        frame = frames.get(link)
+        if frame is None:
+            continue
+        rot = np.asarray(frame[0], dtype=np.float64)
+        origin = np.asarray(frame[1], dtype=np.float64)
+        for part in parts:
+            local = np.einsum("ij,fvj->fvi", part["erot"], part["verts"]) + part["epos"]
+            world = np.einsum("ij,fvj->fvi", rot, local) + origin
+            ab = world[:, 1] - world[:, 0]
+            ac = world[:, 2] - world[:, 0]
+            normal = np.cross(ab, ac)
+            norm = np.linalg.norm(normal, axis=1)
+            norm = np.where(norm > 0.0, norm, 1.0)
+            facing = np.abs(normal[:, 0] + normal[:, 1]) / (norm * math.sqrt(2.0))
+            worlds.append(world)
+            shades.append(0.28 + 0.72 * facing)
+            colors.append(part["rgb"])
+    if not worlds:
+        empty = np.zeros((0, 3, 3), dtype=np.float64)
+        return empty, np.zeros((0,), dtype=np.float64), np.zeros((0, 3), dtype=np.float64)
+    return np.concatenate(worlds), np.concatenate(shades), np.concatenate(colors)
+
+
+def view_rotation(yaw: float, pitch: float):
+    """先绕竖直轴转，再俯仰。拖拽视角用这一份，不在界面里另写一套。"""
+    return _mul(_rx(pitch), _rz(yaw))
+
+
+def view_scale(radius: float, width: int, height: int) -> tuple[float, float]:
+    """和着色器里的 ``u_sx`` / ``u_sy`` 同一套缩放。宽屏时横向再缩一档。"""
+    fit = 0.85 / max(float(radius), 1e-4)
+    width = max(int(width), 1)
+    height = max(int(height), 1)
+    if width >= height:
+        return fit * height / width, fit
+    return fit, fit * width / height
+
+
+def screen_point(
+    point: tuple[float, float, float],
+    yaw: float,
+    pitch: float,
+    center: tuple[float, float, float],
+    radius: float,
+    width: int,
+    height: int,
+) -> tuple[float, float]:
+    """基座系的点投到控件像素。纵向和着色器里的 ``p.z`` 是同一套，+Z 朝画面上方。"""
+    shifted = (
+        float(point[0]) - float(center[0]),
+        float(point[1]) - float(center[1]),
+        float(point[2]) - float(center[2]),
+    )
+    viewed = _apply(view_rotation(yaw, pitch), shifted)
+    sx, sy = view_scale(radius, width, height)
+    ndc_x = viewed[0] * sx
+    # 着色器写的是 ``p.z * u_sy``。OpenGL 的 y 朝上，这里再换成 Qt 的像素 y。
+    ndc_y = viewed[2] * sy
+    width = max(int(width), 1)
+    height = max(int(height), 1)
+    return (ndc_x * 0.5 + 0.5) * width, (1.0 - (ndc_y * 0.5 + 0.5)) * height
+
+
+def ground_grid(extent: float = 1.0, step: float = 0.25) -> list[tuple[tuple[float, float, float], tuple[float, float, float]]]:
+    """地平面格子，z = 0，单位米。以前这张格子在 RViz 里，现在画在工位仿真上。"""
+    if step <= 0 or extent <= 0:
+        raise ValueError("地平面格子的间距和范围都要大于 0")
+    count = int(round(extent / step))
+    lines = []
+    for index in range(-count, count + 1):
+        coord = index * step
+        lines.append(((-extent, coord, 0.0), (extent, coord, 0.0)))
+        lines.append(((coord, -extent, 0.0), (coord, extent, 0.0)))
+    return lines
+
+
+def axis_tip(
+    origin: tuple[float, float, float],
+    rot: tuple,
+    axis: int,
+    length: float,
+) -> tuple[float, float, float]:
+    """坐标系某一轴的端点。``rot`` 把连杆坐标变到基座坐标，列向量就是三根轴。"""
+    basis = ((1.0, 0.0, 0.0), (0.0, 1.0, 0.0), (0.0, 0.0, 1.0))[axis]
+    direction = _apply(rot, basis)
+    return (
+        origin[0] + direction[0] * length,
+        origin[1] + direction[1] * length,
+        origin[2] + direction[2] * length,
+    )
+
+
+def _place(rot, origin, erot, epos, vertex):
+    local = _add(epos, _apply(erot, vertex))
+    return _add(origin, _apply(rot, local))
+
+
+def _shade(a, b, c) -> float:
+    ux, uy, uz = b[0] - a[0], b[1] - a[1], b[2] - a[2]
+    vx, vy, vz = c[0] - a[0], c[1] - a[1], c[2] - a[2]
+    nx = uy * vz - uz * vy
+    ny = uz * vx - ux * vz
+    nz = ux * vy - uy * vx
+    norm = math.sqrt(nx * nx + ny * ny + nz * nz) or 1.0
+    facing = abs(nx + ny) / (norm * math.sqrt(2.0))
+    return 0.28 + 0.72 * facing
+
+
+def _models() -> dict[str, list]:
+    global _MODELS
+    if _MODELS is not None:
+        return _MODELS
+    root = _description_root()
+    visual = yaml.safe_load((root / "config/Rizon4/visual_parameters.yaml").read_text(encoding="utf-8"))
+    params = (visual or {}).get("visual_parameters") or {}
+    loaded: dict[str, list] = {}
+    for link, spec in params.items():
+        shell = ((spec or {}).get("visual") or {})
+        parts = []
+        for kind in ("shell", "ring"):
+            item = shell.get(kind)
+            if not isinstance(item, dict):
+                continue
+            mesh = item.get("mesh") or {}
+            path = root / str(mesh.get("path") or "")
+            if not path.is_file():
+                continue
+            scale = _scale(mesh.get("scale"))
+            origin = item.get("origin") or {}
+            erot = _rpy(
+                float(origin.get("roll") or 0.0),
+                float(origin.get("pitch") or 0.0),
+                float(origin.get("yaw") or 0.0),
+            )
+            epos = (
+                float(origin.get("x") or 0.0),
+                float(origin.get("y") or 0.0),
+                float(origin.get("z") or 0.0),
+            )
+            parts.append(_part_arrays(_load_obj(path, scale, _materials(path)), erot, epos))
+        if parts:
+            loaded[str(link)] = parts
+    if "base_link" not in loaded or "link7" not in loaded:
+        raise FileNotFoundError(f"Rizon4 外壳网格不完整：{root}")
+    _MODELS = loaded
+    return loaded
+
+
+def _part_arrays(faces: list[tuple], erot, epos) -> dict:
+    """把一个零件收成数组，姿态更新时整块相乘。"""
+    if not faces:
+        verts = np.zeros((0, 3, 3), dtype=np.float64)
+        rgb = np.zeros((0, 3), dtype=np.float64)
+    else:
+        verts = np.asarray([[a, b, c] for a, b, c, _color in faces], dtype=np.float64)
+        rgb = np.asarray([color for _a, _b, _c, color in faces], dtype=np.float64)
+    return {
+        "verts": verts,
+        "rgb": rgb,
+        "erot": np.asarray(erot, dtype=np.float64),
+        "epos": np.asarray(epos, dtype=np.float64),
+    }
+
+
+def _description_root() -> Path:
+    root = ros2_root()
+    candidates = (
+        root / "src/flexiv_description",
+        root / "install/flexiv_description/share/flexiv_description",
+    )
+    for item in candidates:
+        if (item / "config/Rizon4/visual_parameters.yaml").is_file():
+            return item
+    raise FileNotFoundError("找不到 flexiv_description 的 Rizon4 外观")
+
+
+def _materials(path: Path) -> dict[str, tuple[float, float, float]]:
+    mtl = path.with_suffix(".mtl")
+    colors: dict[str, tuple[float, float, float]] = {}
+    name = ""
+    if not mtl.is_file():
+        return colors
+    for line in mtl.read_text(encoding="utf-8", errors="replace").splitlines():
+        if line.startswith("newmtl "):
+            name = line.split(None, 1)[1].strip()
+        elif name and line.startswith("Kd "):
+            parts = line.split()
+            if len(parts) >= 4:
+                colors[name] = (float(parts[1]), float(parts[2]), float(parts[3]))
+    return colors
+
+
+def _load_obj(path: Path, scale: tuple[float, float, float], materials: dict) -> list[tuple]:
+    fallback = (0.75, 0.76, 0.78)
+    rgb = fallback
+    vertices: list[tuple[float, float, float]] = []
+    faces: list[tuple[int, int, int, tuple]] = []
+    for line in path.read_text(encoding="utf-8", errors="replace").splitlines():
+        if line.startswith("usemtl "):
+            rgb = materials.get(line.split(None, 1)[1].strip(), fallback)
+        elif line.startswith("v "):
+            _v, x, y, z, *_rest = line.split()
+            vertices.append((float(x) * scale[0], float(y) * scale[1], float(z) * scale[2]))
+        elif line.startswith("f "):
+            idx = []
+            for token in line.split()[1:]:
+                raw = token.split("/")[0]
+                if not raw:
+                    continue
+                number = int(raw)
+                idx.append(number - 1 if number > 0 else number)
+            for offset in range(1, len(idx) - 1):
+                faces.append((idx[0], idx[offset], idx[offset + 1], rgb))
+    if not faces or not vertices:
+        return []
+    step = max(1, len(faces) // _FACES_PER_LINK)
+    chosen = faces[::step]
+    out = []
+    count = len(vertices)
+    for ia, ib, ic, color in chosen:
+        if not (0 <= ia < count and 0 <= ib < count and 0 <= ic < count):
+            continue
+        out.append((vertices[ia], vertices[ib], vertices[ic], color))
+    return out
+
+
+def _scale(raw) -> tuple[float, float, float]:
+    if raw is None:
+        return (1.0, 1.0, 1.0)
+    if isinstance(raw, (int, float)):
+        value = float(raw)
+        return (value, value, value)
+    text = str(raw).replace(",", " ").split()
+    nums = [float(item) for item in text[:3]]
+    while len(nums) < 3:
+        nums.append(nums[-1] if nums else 1.0)
+    return (nums[0], nums[1], nums[2])
+
+
+def _rpy(roll: float, pitch: float, yaw: float):
+    return _mul(_mul(_rz(yaw), _ry(pitch)), _rx(roll))
+
+
+def _rx(angle: float):
+    c, s = math.cos(angle), math.sin(angle)
+    return ((1.0, 0.0, 0.0), (0.0, c, -s), (0.0, s, c))
+
+
+def _ry(angle: float):
+    c, s = math.cos(angle), math.sin(angle)
+    return ((c, 0.0, s), (0.0, 1.0, 0.0), (-s, 0.0, c))
+
+
+def _rz(angle: float):
+    c, s = math.cos(angle), math.sin(angle)
+    return ((c, -s, 0.0), (s, c, 0.0), (0.0, 0.0, 1.0))
+
+
+def _mul(a, b):
+    return tuple(tuple(sum(a[i][k] * b[k][j] for k in range(3)) for j in range(3)) for i in range(3))
+
+
+def _apply(rot, vec):
+    return tuple(sum(rot[i][k] * vec[k] for k in range(3)) for i in range(3))
+
+
+def _add(a, b):
+    return (a[0] + b[0], a[1] + b[1], a[2] + b[2])

install/arm_application/lib/python3.11/site-packages/arm_application/operator_arm_state.py

@@ -0,0 +1,595 @@
+"""工位界面用的机械臂状态：只读使能 / Remote，并用关节角画出姿态。
+
+不会调用 Enable，也不会在控制栈或拖拽桥已经占着 RDK 时再连一次。
+``robot.mode()`` 看不出 Manual，必须看 ``operational_status``。
+示教器上的 Auto（Remote）没有远程切换接口，这里只负责把这句话摆在界面上。
+"""
+
+from __future__ import annotations
+
+import json
+import math
+import os
+import re
+import subprocess
+import threading
+import time
+from pathlib import Path
+from typing import Any
+
+import yaml
+
+from arm_application.site_io import listed_arms, ros2_root
+
+# 和 scripts/lib/rdk_status.py 的 MANUAL_HINT 同一件事，界面上始终可见。
+REMOTE_PROMPT = (
+    "请在示教器上打开 Auto（Remote）。"
+    "RDK 没有远程拨这个开关的接口，必须人到现场拨。"
+)
+
+_AXES = (
+    (0.0, 0.0, 1.0),
+    (0.0, 1.0, 0.0),
+    (0.0, 0.0, 1.0),
+    (0.0, 1.0, 0.0),
+    (0.0, 0.0, 1.0),
+    (0.0, 1.0, 0.0),
+    (0.0, 0.0, 1.0),
+)
+_JOINTS = tuple(f"joint{i}" for i in range(1, 8))
+_LOCK = threading.Lock()
+_CACHE: dict[str, dict] = {}
+_STOP = False
+_STARTED = False
+_HOLD = 0
+_HOLD_LOCK = threading.Lock()
+
+
+def _empty(arm: str) -> dict:
+    return {
+        "arm": arm,
+        "level": "warn",
+        "enabled": False,
+        "summary": "还没读到这台臂的状态",
+        "prompt": REMOTE_PROMPT,
+        "prompt_hot": True,
+        "mode": "",
+        "op_status": "",
+        "fault": None,
+        "estop_released": None,
+        "q": None,
+        "points": [],
+        "source": "",
+        "stack_up": False,
+        "stamp": 0.0,
+    }
+
+
+def snapshot(arm: str) -> dict:
+    with _LOCK:
+        row = _CACHE.get(arm)
+        return dict(row) if row else _empty(arm)
+
+
+def present(
+    arm: str,
+    *,
+    stack_up: bool,
+    bridge_up: bool,
+    probe: dict | None,
+    joints: list[float] | None,
+    error: str = "",
+) -> dict:
+    """把一次读数收成界面直接能画的字典。纯函数，不连机器人。"""
+    probe = probe or {}
+    q = _seven(joints) or _seven(probe.get("q"))
+    op_status = str(probe.get("op_status") or "")
+    mode = str(probe.get("mode") or "")
+    manual = "MANUAL" in op_status.upper()
+    if stack_up and q is not None:
+        enabled = True
+        level = "ok"
+        summary = "控制栈在发关节角，这台臂已经接上。"
+        if probe.get("_shared_stack"):
+            summary = "控制栈在发关节角，但看不出是 89 还是 91，两台显示的是同一份。"
+        source = "控制栈 /joint_states"
+    elif stack_up:
+        enabled = False
+        level = "fail"
+        summary = "控制栈在跑，但没有关节数据。通常是还没使能，一直在等 operational。"
+        source = "控制栈"
+    elif bridge_up and not probe:
+        enabled = False
+        level = "warn"
+        summary = "软拖拽桥占着 RDK，界面不再另开一条连接。"
+        source = "拖拽桥"
+        q = None
+    elif probe.get("connected"):
+        enabled = bool(probe.get("operational"))
+        level = str(probe.get("level") or ("ok" if enabled else "fail"))
+        summary = str(probe.get("summary") or ("已使能" if enabled else "未使能"))
+        source = "RDK 直读（未使能，只查询）"
+    else:
+        enabled = False
+        level = "fail" if error else "warn"
+        summary = error or str(probe.get("summary") or probe.get("error") or "还没连上 RDK")
+        source = "RDK 直读" if probe else ""
+    hot = (not enabled) or manual
+    return {
+        "arm": arm,
+        "level": level,
+        "enabled": enabled,
+        "summary": summary,
+        "prompt": REMOTE_PROMPT,
+        "prompt_hot": hot,
+        "mode": mode,
+        "op_status": op_status,
+        "fault": probe.get("fault") if probe else None,
+        "estop_released": probe.get("estop_released") if probe else None,
+        "q": q,
+        "points": fk_points(q) if q else fk_points([0.0] * 7),
+        "pose_live": q is not None,
+        "source": source,
+        "stack_up": stack_up,
+        "stamp": time.time(),
+    }
+
+
+def link_frames(q: list[float]) -> dict[str, tuple]:
+    """各连杆坐标系，和 ``rizon_arm.xacro`` 的关节顺序一致。
+
+    返回 ``base_link`` 与 ``link1``…``link7``。每个值是 ``(旋转, 原点)``，
+    旋转把连杆坐标变到基座坐标。厂家网格就画在这些坐标系里。
+    """
+    kin = _kinematics()
+    rot = ((1.0, 0.0, 0.0), (0.0, 1.0, 0.0), (0.0, 0.0, 1.0))
+    pos = (0.0, 0.0, 0.0)
+    frames: dict[str, tuple] = {"base_link": (rot, pos)}
+    for index, angle in enumerate(q[:7]):
+        spec = kin[_JOINTS[index]]
+        pos = _add(pos, _apply(rot, (spec["x"], spec["y"], spec["z"])))
+        rot = _mul(rot, _rpy(spec["roll"], spec["pitch"], spec["yaw"]))
+        rot = _mul(rot, _axis_rot(_AXES[index], float(angle)))
+        frames[f"link{index + 1}"] = (rot, pos)
+    return frames
+
+
+def fk_points(q: list[float]) -> list[tuple[float, float, float]]:
+    """按 Rizon4 的 default_kinematics 做正运动学，返回基座到法兰的点（米）。
+
+    关节轴与 ``rizon_arm.xacro`` 一致：1/3/5/7 绕 z，2/4/6 绕 y。
+    URDF 顺序是先平移（父坐标系），再 rpy，再绕关节轴转 q。
+    """
+    frames = link_frames(q)
+    points = [frames["base_link"][1]]
+    points.extend(frames[f"link{index}"][1] for index in range(1, 8))
+    _rot, pos = _flange_pose(frames)
+    points.append(pos)
+    return points
+
+
+def display_frames(q: list[float], workpiece: tuple | None = None) -> list[dict]:
+    """界面要画的坐标系，都在基座系里。
+
+    第一套是基座。第二套默认是法兰（旋转与 ``link7`` 相同，原点再加
+    ``link7_to_flange``）。传入工件系时，第二套改画在工件上，不再画法兰。
+    """
+    frames = link_frames(q)
+    base_rot, base_pos = frames["base_link"]
+    shown = [{"name": "基座", "rot": base_rot, "pos": base_pos}]
+    if workpiece is None:
+        rot, pos = _flange_pose(frames)
+        shown.append({"name": "法兰", "rot": rot, "pos": pos})
+        return shown
+    rot, pos = workpiece
+    if not isinstance(rot, tuple) or len(rot) != 3 or not isinstance(pos, tuple) or len(pos) != 3:
+        raise ValueError("工件坐标系要有旋转和原点")
+    shown.append({"name": "工件", "rot": rot, "pos": pos})
+    return shown
+
+
+def workpiece_frame(robot: dict | None) -> tuple | None:
+    """有完整工件系时返回 ``(旋转, 原点)``。没有就返回 None。
+
+    ``workpiece_surface_z_m`` 只是上表面高度，没有原点的 XY，也没有朝向，
+    不能当成坐标系。配了 ``gauge_assembly.workpiece_frame`` 却缺 ``xyz`` 或
+    ``rpy`` 时直接报错，不用单位阵把缺的朝向补上。
+    ``xyz`` 单位米，``rpy`` 单位弧度，都写在基座系，旋转顺序与连杆 ``rpy`` 相同。
+    """
+    if not isinstance(robot, dict):
+        return None
+    gauge = robot.get("gauge_assembly")
+    if not isinstance(gauge, dict) or "workpiece_frame" not in gauge:
+        return None
+    raw = gauge.get("workpiece_frame")
+    if not isinstance(raw, dict):
+        raise ValueError("工件坐标系要写成 xyz 和 rpy，各三个数")
+    xyz = raw.get("xyz")
+    rpy = raw.get("rpy")
+    if not _three_floats(xyz) or not _three_floats(rpy):
+        raise ValueError("工件坐标系要有 xyz 和 rpy，各三个数，单位是米和弧度，写在基座系里")
+    return _rpy(float(rpy[0]), float(rpy[1]), float(rpy[2])), (
+        float(xyz[0]), float(xyz[1]), float(xyz[2]),
+    )
+
+
+def _flange_pose(frames: dict) -> tuple:
+    """法兰位姿。旋转就是 ``link7`` 的旋转，平移来自 ``link7_to_flange``。"""
+    rot, pos = frames["link7"]
+    flange = _kinematics()["link7_to_flange"]
+    pos = _add(pos, _apply(rot, (float(flange["x"]), float(flange["y"]), float(flange["z"]))))
+    return rot, pos
+
+
+def _three_floats(raw: Any) -> bool:
+    if not isinstance(raw, (list, tuple)) or len(raw) != 3:
+        return False
+    return all(isinstance(item, (int, float)) and not isinstance(item, bool) for item in raw)
+
+
+def stack_running() -> bool:
+    return _stack_arm() is not None or _alive("ros2_control_node")
+
+
+def control_holds_rdk() -> bool:
+    """有控制命令在用 RDK 时，状态轮询必须让开。"""
+    with _HOLD_LOCK:
+        return _HOLD > 0
+
+
+def release_for_control() -> None:
+    """停掉正在读状态的 RDK 子进程，把机器人交给控制命令。"""
+    with _HOLD_LOCK:
+        global _HOLD
+        _HOLD += 1
+    _stop_status_probes()
+    deadline = time.time() + 3.0
+    while time.time() < deadline and _status_probe_running():
+        time.sleep(0.05)
+
+
+def resume_after_control() -> None:
+    with _HOLD_LOCK:
+        global _HOLD
+        _HOLD = max(0, _HOLD - 1)
+
+
+def hold_until_idle(pid: int, *, until_stack: bool = False) -> None:
+    """后台脚本还在时继续占着 RDK。进程退出，或控制栈已经起来，就放开。"""
+    def _watch() -> None:
+        while True:
+            if until_stack and stack_running():
+                break
+            if not _pid_alive(pid):
+                break
+            time.sleep(0.3)
+        resume_after_control()
+
+    threading.Thread(target=_watch, daemon=True, name="arm-rdk-hold").start()
+
+
+def motion_blocked(action: str, *, stack_up: bool, op_status: str) -> str:
+    """控制栈没起来时，动臂按钮给一句能执行的原因，而不是把脚本挂在那儿。"""
+    needs_stack = {
+        "pipeline", "stage_track", "stage_locate", "stage_screw",
+        "reset_home", "gauge_run", "handeye_calibrate",
+    }
+    if action not in needs_stack or stack_up:
+        return ""
+    manual = "MANUAL" in str(op_status or "").upper()
+    tail = "示教器现在是 Manual，要先拨到 Auto（Remote）。" if manual else ""
+    return f"控制栈没起来，这个动作动不了臂。{tail}请先在操作页执行「上电控制栈」。"
+
+
+def start() -> None:
+    global _STARTED
+    with _LOCK:
+        if _STARTED:
+            return
+        _STARTED = True
+    threading.Thread(target=_loop, daemon=True, name="arm-state").start()
+
+
+def retain_pose(fresh: dict, previous: dict | None, *, now: float | None = None, hold_s: float = 15.0) -> dict:
+    """这一拍没读到关节角时，短时间沿用上一拍，避免模型被打回零位。
+
+    超过 ``hold_s`` 仍没有新读数就放开，界面回到“没连上”，不把过期姿态一直当成当前。
+    """
+    if fresh.get("q") is not None or not previous or previous.get("q") is None:
+        return fresh
+    moment = time.time() if now is None else now
+    if moment - float(previous.get("stamp") or 0.0) > hold_s:
+        return fresh
+    kept = dict(fresh)
+    kept["q"] = list(previous["q"])
+    kept["points"] = list(previous.get("points") or [])
+    kept["pose_live"] = True
+    note = "关节角沿用上次读数。"
+    summary = str(kept.get("summary") or "")
+    if note not in summary:
+        kept["summary"] = f"{summary} {note}".strip()
+    return kept
+
+
+def _loop() -> None:
+    while not _STOP:
+        if control_holds_rdk():
+            time.sleep(0.2)
+            continue
+        try:
+            arms = _arms()
+            if not arms:
+                time.sleep(2.0)
+                continue
+            stack_arm = _stack_arm()
+            stack_up = stack_arm is not None or _alive("ros2_control_node")
+            bridge_up = _alive("rdk_drag_bridge.py")
+            joints = _read_joints() if stack_up else None
+            if stack_up:
+                targets = [row for row in arms if stack_arm is None or row["id"] == stack_arm]
+                if not targets:
+                    targets = arms
+                shared = stack_arm is None
+                for row in targets:
+                    fresh = present(
+                        row["id"],
+                        stack_up=True,
+                        bridge_up=bridge_up,
+                        probe={"_shared_stack": True} if shared else None,
+                        joints=joints,
+                    )
+                    _store(retain_pose(fresh, snapshot(row["id"])))
+            else:
+                for row in arms:
+                    probe: dict | None = None
+                    error = ""
+                    if control_holds_rdk():
+                        break
+                    if not bridge_up:
+                        probe, error = _probe(row["sn"], row["iface"], row["pc_ip"])
+                    fresh = present(
+                        row["id"],
+                        stack_up=False,
+                        bridge_up=bridge_up,
+                        probe=probe,
+                        joints=None,
+                        error=error,
+                    )
+                    _store(retain_pose(fresh, snapshot(row["id"])))
+        except Exception as exc:  # noqa: BLE001 —— 读状态失败不能把界面线程打死
+            for row in _arms():
+                _store(present(
+                    row["id"], stack_up=False, bridge_up=False, probe=None, joints=None, error=str(exc),
+                ))
+        time.sleep(4.0)
+
+
+def _stop_status_probes() -> None:
+    try:
+        subprocess.run(
+            ["pkill", "-f", "scripts/lib/rdk_status.py"],
+            capture_output=True, text=True, timeout=2,
+        )
+    except (OSError, subprocess.TimeoutExpired):
+        return
+
+
+def _status_probe_running() -> bool:
+    return "rdk_status.py" in _pgrep("scripts/lib/rdk_status.py")
+
+
+def _pid_alive(pid: int) -> bool:
+    if pid <= 0:
+        return False
+    try:
+        os.kill(pid, 0)
+    except OSError:
+        return False
+    return True
+
+
+def _store(row: dict) -> None:
+    with _LOCK:
+        _CACHE[str(row.get("arm") or "")] = row
+
+
+def _arms() -> list[dict]:
+    path = ros2_root() / "config" / "site.yaml"
+    if not path.is_file():
+        return []
+    data = yaml.safe_load(path.read_text(encoding="utf-8")) or {}
+    return [
+        {"id": row["id"], "sn": row["sn"], "iface": row["iface"], "pc_ip": row["pc_ip"]}
+        for row in listed_arms(data)
+    ]
+
+
+def _stack_arm() -> str | None:
+    text = _pgrep("run_real.sh")
+    ids = {row["id"] for row in _arms()}
+    for match in re.finditer(r"--arm(?:=|\s+)(\S+)", text):
+        token = match.group(1)
+        if token in ids:
+            return token
+    return None
+
+
+def _alive(pattern: str) -> bool:
+    return bool(_pgrep(pattern).strip())
+
+
+def _pgrep(pattern: str) -> str:
+    try:
+        proc = subprocess.run(
+            ["pgrep", "-af", pattern],
+            capture_output=True, text=True, timeout=2,
+        )
+    except (OSError, subprocess.TimeoutExpired):
+        return ""
+    lines = []
+    for line in proc.stdout.splitlines():
+        if "pgrep" in line:
+            continue
+        lines.append(line)
+    return "\n".join(lines)
+
+
+def _read_joints() -> list[float] | None:
+    try:
+        proc = subprocess.run(
+            ["timeout", "4", "ros2", "topic", "echo", "/joint_states", "--once"],
+            capture_output=True, text=True, timeout=6,
+        )
+    except (OSError, subprocess.TimeoutExpired):
+        return None
+    return _parse_joint_echo(proc.stdout)
+
+
+def _parse_joint_echo(text: str) -> list[float] | None:
+    for chunk in text.split("---"):
+        if "position:" not in chunk:
+            continue
+        try:
+            data = yaml.safe_load(chunk)
+        except yaml.YAMLError:
+            continue
+        if not isinstance(data, dict):
+            continue
+        names = list(data.get("name") or [])
+        pos = list(data.get("position") or [])
+        if names and len(names) == len(pos):
+            by = {str(name): float(value) for name, value in zip(names, pos)}
+            q = []
+            for joint in _JOINTS:
+                key = next((name for name in by if name == joint or name.endswith(joint)), None)
+                if key is None:
+                    q = []
+                    break
+                q.append(by[key])
+            if len(q) == 7:
+                return q
+        if len(pos) >= 7:
+            try:
+                return [float(v) for v in pos[:7]]
+            except (TypeError, ValueError):
+                return None
+    return None
+
+
+def _probe(sn: str, iface: str, pc_ip: str = "") -> tuple[dict | None, str]:
+    python = os.environ.get("RDK_PYTHON", "").strip() or str(Path.home() / "rdk_py310" / "bin" / "python")
+    script = ros2_root() / "scripts" / "lib" / "rdk_status.py"
+    if not Path(python).is_file() or not script.is_file():
+        return None, "找不到 RDK 解释器或 rdk_status.py，无法读使能状态"
+    cmd = _probe_cmd(python, script, sn, iface, pc_ip)
+    try:
+        proc = subprocess.run(cmd, capture_output=True, text=True, timeout=8)
+    except subprocess.TimeoutExpired:
+        return None, "读 RDK 超时。若控制栈没开，检查网线，并确认示教器已开 Remote。"
+    except OSError as exc:
+        return None, f"读 RDK 失败：{exc}"
+    payload = _json_object(proc.stdout) or _json_object(proc.stderr)
+    if not payload:
+        tail = (proc.stdout + proc.stderr).strip()[-240:]
+        return None, tail or "RDK 没有返回状态"
+    return payload, ""
+
+
+def _probe_cmd(python: str, script: Path, sn: str, iface: str, pc_ip: str) -> list[str]:
+    """RDK 白名单要本机 IPv4。只传网卡名 ``eth0`` 时，内核改名叫 enx… 就永远连不上。"""
+    cmd = [python, str(script), "--sn", sn, "--json"]
+    if iface:
+        cmd += ["--iface", iface]
+    if pc_ip:
+        cmd += ["--pc-ip", pc_ip]
+    return cmd
+
+
+def _json_object(text: str) -> dict:
+    decoder = json.JSONDecoder()
+    for index, char in enumerate(text):
+        if char != "{":
+            continue
+        try:
+            data, _end = decoder.raw_decode(text[index:])
+        except json.JSONDecodeError:
+            continue
+        if isinstance(data, dict) and "connected" in data:
+            return data
+    return {}
+
+
+_KIN: dict[str, dict[str, float]] | None = None
+
+
+def _kinematics() -> dict[str, dict[str, float]]:
+    global _KIN
+    if _KIN is not None:
+        return _KIN
+    root = ros2_root()
+    candidates = (
+        root / "src/flexiv_description/config/Rizon4/default_kinematics.yaml",
+        root / "install/flexiv_description/share/flexiv_description/config/Rizon4/default_kinematics.yaml",
+    )
+    path = next((item for item in candidates if item.is_file()), None)
+    if path is None:
+        raise FileNotFoundError("找不到 Rizon4 default_kinematics.yaml")
+    data = yaml.safe_load(path.read_text(encoding="utf-8")) or {}
+    kin = data.get("kinematics") or {}
+    _KIN = kin
+    return kin
+
+
+def _rpy(roll: float, pitch: float, yaw: float) -> tuple:
+    return _mul(_mul(_rz(yaw), _ry(pitch)), _rx(roll))
+
+
+def _rx(angle: float) -> tuple:
+    c, s = math.cos(angle), math.sin(angle)
+    return ((1.0, 0.0, 0.0), (0.0, c, -s), (0.0, s, c))
+
+
+def _ry(angle: float) -> tuple:
+    c, s = math.cos(angle), math.sin(angle)
+    return ((c, 0.0, s), (0.0, 1.0, 0.0), (-s, 0.0, c))
+
+
+def _rz(angle: float) -> tuple:
+    c, s = math.cos(angle), math.sin(angle)
+    return ((c, -s, 0.0), (s, c, 0.0), (0.0, 0.0, 1.0))
+
+
+def _axis_rot(axis: tuple[float, float, float], angle: float) -> tuple:
+    x, y, z = axis
+    if abs(z) > 0.5:
+        return _rz(angle if z > 0 else -angle)
+    if abs(y) > 0.5:
+        return _ry(angle if y > 0 else -angle)
+    return _rx(angle if x > 0 else -angle)
+
+
+def _mul(a: tuple, b: tuple) -> tuple:
+    return tuple(
+        tuple(sum(a[i][k] * b[k][j] for k in range(3)) for j in range(3))
+        for i in range(3)
+    )
+
+
+def _apply(rot: tuple, vec: tuple[float, float, float]) -> tuple[float, float, float]:
+    return tuple(sum(rot[i][k] * vec[k] for k in range(3)) for i in range(3))  # type: ignore[return-value]
+
+
+def _add(a: tuple[float, float, float], b: tuple[float, float, float]) -> tuple[float, float, float]:
+    return (a[0] + b[0], a[1] + b[1], a[2] + b[2])
+
+
+def _seven(raw: Any) -> list[float] | None:
+    if not isinstance(raw, list) or len(raw) < 7:
+        return None
+    try:
+        q = [float(v) for v in raw[:7]]
+    except (TypeError, ValueError):
+        return None
+    if any(v != v or abs(v) > 12.6 for v in q):
+        return None
+    return q

install/arm_application/lib/python3.11/site-packages/arm_application/operator_camera_view.py

@@ -1,6 +1,12 @@
 """工位程序里的相机预览。
 
 只订彩色和已对齐深度，不订左右红外。画面留在本进程，不另开查看器窗口。
+
+回调里先丢掉当前页没在看的流。彩色按相机原分辨率收进来，不按时钟抽帧，
+也不在收图的线程里做颜色转换。另一条线只画最新的一张，所以帧率跟相机走，
+848 和 1280 都不会为了帧率被缩成 480。深度是伪彩色，仍然抽稀，
+而且没打开深度时不订阅，避免大图挤占彩色。
+手眼页要 JPEG 时也按原分辨率编码，只编正在看的那一路。
 """
 
 from __future__ import annotations
@@ -11,33 +17,121 @@ import time
 
 import numpy as np
 
+# 彩色上限。D405 是 848×480，D435 是 1280×720，都不超过它，所以不会被缩小。
+_COLOR_WIDTH = 1280
+# 深度上色不是要看的画质。全幅每帧上色会把刷新占满，所以这一路仍抽稀。
+_DEPTH_WIDTH = 480
+# 手眼只在正在看的那一页编码，宽度跟彩色预览一样，不另缩一档。
+_JPEG_WIDTH = 1280
+# 只留给还在用 ``accept_frame`` 的调用。彩色实况不再用它抽帧：
+# 相机大约 30 帧，再按这个间隔丢一次，画面会掉到十几帧。
+_INTERVAL = 1.0 / 30.0
+_DET_HOLD_S = 1.5
+
 _LOCK = threading.Lock()
 _STOP = threading.Event()
 _THREAD: threading.Thread | None = None
 _JPEG: dict[str, bytes] = {}
+_PREVIEW: dict[str, tuple[bytes, int, int]] = {}
+_PREVIEW_WANT: set[str] = set()
+_JPEG_WANT: set[str] = set()
+_NEXT: dict[str, float] = {}
+_DET: dict[str, tuple[float, list[float]]] = {}
+# 没画框的预览。检测一到就在这张上重画，不必等下一帧相机。
+_CLEAN: dict[str, dict] = {}
+# 每路最近一秒的到达时刻，用来算帧率。值是 (宽, 高, 帧/秒)，宽高是相机原图。
+_META: dict[str, tuple[int, int, float]] = {}
+_STAMPS: dict[str, list[float]] = {}
+# 每路最新原图。收图线程只往这里放，画图线程只取最新的一张。
+_RAW: dict[str, dict] = {}
+_SLOTS: dict[str, list] = {}
+_SLOT_AT: dict[str, int] = {}
+_PAINTED: dict[str, int] = {}
+_WAKE = threading.Event()
+_PAINT_THREAD: threading.Thread | None = None
 _STATE = {"on": False, "error": "", "frames": {}, "stamp": 0.0}
 
 
-def targets() -> list[dict]:
-    """真机腕部相机和手外相机。仿真相机不进预览。"""
-    from arm_application.operator_console import _arms, _site
-    from arm_application.site_io import overhead_camera
+def preview_targets(site: dict) -> list[dict]:
+    """主界面只要左臂腕部相机和共用的手外相机。
+
+    右臂腕部不进这一屏。仿真相机名字以 ``sim`` 开头，也不进。
+    推理框的角色仍是 ``wrist`` / ``overhead``，跟话题对得上。
+    """
+    from arm_application.site_io import listed_arms, overhead_camera
 
     found: list[dict] = []
     seen: set[str] = set()
-    for arm in _arms():
-        name = str(arm.get("camera") or "").strip().strip("/")
-        if not name or name.startswith("sim") or name in seen:
-            continue
+    left = next((row for row in listed_arms(site) if row.get("side") == "left"), None)
+    name = str((left or {}).get("camera") or "").strip().strip("/")
+    if name and not name.startswith("sim"):
+        cams = site.get("cameras") if isinstance(site.get("cameras"), dict) else {}
+        cfg = cams.get(name) if isinstance(cams, dict) and isinstance(cams.get(name), dict) else {}
+        tag = str((cfg or {}).get("model") or "D405").strip().upper() or "D405"
         seen.add(name)
-        found.append({"id": name, "label": f"{arm.get('label') or name} 腕部"})
-    cam, _others = overhead_camera(_site(), None)
-    name = str((cam or {}).get("name") or "").strip().strip("/")
-    if name and not name.startswith("sim") and name not in seen:
-        found.append({"id": name, "label": str((cam or {}).get("label") or "手外相机")})
+        found.append({"id": name, "label": f"左腕 {tag}", "role": "wrist"})
+    cam, _others = overhead_camera(site or {}, None)
+    oname = str((cam or {}).get("name") or "").strip().strip("/")
+    if oname and not oname.startswith("sim") and oname not in seen:
+        tag = str((cam or {}).get("model") or "D435").strip().upper() or "D435"
+        found.append({"id": oname, "label": f"手外 {tag}", "role": "overhead"})
     return found
 
 
+def sim_preview_targets(site: dict) -> list[dict]:
+    """仿真腕部和仿真手外。画面进主界面，不另开 Gazebo 的相机窗。"""
+    from arm_application.site_io import overhead_camera
+
+    found: list[dict] = []
+    robots = ((site or {}).get("robot") or {}).get("robots") or []
+    cams = site.get("cameras") if isinstance(site.get("cameras"), dict) else {}
+    for robot in robots:
+        if not isinstance(robot, dict):
+            continue
+        if str(robot.get("sn") or "").strip().lower() != "sim":
+            continue
+        cam = robot.get("camera") if isinstance(robot.get("camera"), dict) else {}
+        name = str((cam or {}).get("name") or "").strip().strip("/")
+        if not name:
+            continue
+        cfg = cams.get(name) if isinstance(cams, dict) and isinstance(cams.get(name), dict) else {}
+        tag = str((cfg or {}).get("model") or "D405").strip().upper() or "D405"
+        found.append({"id": name, "label": f"仿真左腕 {tag}", "role": "wrist", "source": "sim"})
+        break
+    overhead, _others = overhead_camera(site or {}, {"arm": "sim"})
+    oname = str((overhead or {}).get("name") or "").strip().strip("/")
+    if oname.startswith("sim"):
+        tag = str((overhead or {}).get("model") or "D435").strip().upper() or "D435"
+        found.append({"id": oname, "label": f"仿真手外 {tag}", "role": "overhead", "source": "sim"})
+    return found
+
+
+def stream_targets(site: dict | None = None) -> list[dict]:
+    """真机和仿真都订上。主界面切换看哪一组，没在看的那组回调里直接丢掉。"""
+    if site is None:
+        from arm_application.operator_console import _site
+
+        site = _site()
+    real = preview_targets(site)
+    for item in real:
+        item["source"] = "real"
+    seen = {item["id"] for item in real}
+    return real + [item for item in sim_preview_targets(site) if item["id"] not in seen]
+
+
+def targets() -> list[dict]:
+    """当前站点的主界面真机相机。规则在 ``preview_targets``。"""
+    from arm_application.operator_console import _site
+
+    return preview_targets(_site())
+
+
+def sim_targets() -> list[dict]:
+    from arm_application.operator_console import _site
+
+    return sim_preview_targets(_site())
+
+
 def state() -> dict:
     with _LOCK:
         return {
@@ -53,141 +147,492 @@ def frame(name: str, kind: str) -> bytes | None:
         return _JPEG.get(f"{name}_{kind}")
 
 
+def previews() -> dict[str, tuple[bytes, int, int]]:
+    """当前预览。值是 ``(rgb888, 宽, 高)``，同一帧反复取到的是同一块字节。"""
+    with _LOCK:
+        return dict(_PREVIEW)
+
+
+def preview_meta() -> dict[str, tuple[int, int, float]]:
+    """每路相机原图的宽、高，以及最近一秒的帧率。还没来帧就是空。"""
+    with _LOCK:
+        return dict(_META)
+
+
+def _mark_rate(key: str, width: int, height: int) -> None:
+    """记下这一帧的原图尺寸，并用最近一秒的到达间隔算帧率。"""
+    now = time.monotonic()
+    with _LOCK:
+        stamps = _STAMPS.setdefault(key, [])
+        stamps.append(now)
+        cut = now - 1.0
+        while len(stamps) > 1 and stamps[0] < cut:
+            stamps.pop(0)
+        if len(stamps) >= 2 and stamps[-1] > stamps[0]:
+            fps = (len(stamps) - 1) / (stamps[-1] - stamps[0])
+        else:
+            fps = 0.0
+        _META[key] = (int(width), int(height), float(fps))
+
+
+def watch(preview: set[str], jpeg: set[str] | None = None) -> None:
+    """当前页要哪几路。没点名的流在回调里直接丢掉，不拷图像。
+
+    ``preview`` 走原分辨率 RGB，``jpeg`` 走手眼用的 JPEG。两边都空着就只保持订阅。
+    """
+    global _PREVIEW_WANT, _JPEG_WANT
+    jpeg = set(jpeg or ())
+    preview = set(preview)
+    with _LOCK:
+        if preview == _PREVIEW_WANT and jpeg == _JPEG_WANT:
+            return
+        _PREVIEW_WANT = preview
+        _JPEG_WANT = jpeg
+        for key in preview | jpeg:
+            _NEXT.pop(key, None)
+
+
+def wants(key: str) -> tuple[bool, bool]:
+    """返回 ``(要预览, 要 JPEG)``。"""
+    with _LOCK:
+        return key in _PREVIEW_WANT, key in _JPEG_WANT
+
+
+def offer_color(key: str, image: np.ndarray, *, bgr: bool, role: str) -> None:
+    """收一帧彩色原图。没人看就丢掉。有人看就整幅留下，不按时钟抽帧。"""
+    preview, jpeg = wants(key)
+    if not preview and not jpeg:
+        return
+    if image.ndim != 3 or image.shape[2] != 3 or image.shape[0] < 1 or image.shape[1] < 1:
+        return
+    _offer(key, image, bgr=bgr, role=role, kind="color")
+
+
+def offer_depth(key: str, image: np.ndarray) -> None:
+    """收一帧深度原图。伪彩色仍在画图线程里做，不堵收图。"""
+    preview, _jpeg = wants(key)
+    if not preview:
+        return
+    if image.ndim != 2 or image.shape[0] < 1 or image.shape[1] < 1:
+        return
+    _offer(key, image, bgr=False, role="", kind="depth")
+
+
+def _offer(key: str, image: np.ndarray, *, bgr: bool, role: str, kind: str) -> None:
+    height, width = int(image.shape[0]), int(image.shape[1])
+    with _LOCK:
+        slots = _SLOTS.setdefault(key, [None, None])
+        index = _SLOT_AT.get(key, 0)
+        slot = slots[index]
+        if slot is None or slot.shape != image.shape or slot.dtype != image.dtype:
+            slot = np.empty(image.shape, dtype=image.dtype)
+            slots[index] = slot
+        np.copyto(slot, image)
+        seq = int(_RAW.get(key, {}).get("seq", 0)) + 1
+        _RAW[key] = {
+            "seq": seq,
+            "frame": slot,
+            "bgr": bool(bgr),
+            "role": role,
+            "kind": kind,
+            "w": width,
+            "h": height,
+        }
+        _SLOT_AT[key] = 1 - index
+    _mark_rate(key, width, height)
+    _WAKE.set()
+
+
+def publish_latest() -> None:
+    """把各路还没画的最新原图做成预览。中间帧直接跳过，避免越画越落后。"""
+    with _LOCK:
+        jobs = []
+        for key, item in _RAW.items():
+            if _PAINTED.get(key) == item["seq"]:
+                continue
+            jobs.append((
+                key,
+                int(item["seq"]),
+                np.array(item["frame"], copy=True),
+                bool(item["bgr"]),
+                str(item["role"]),
+                str(item["kind"]),
+                int(item["w"]),
+                int(item["h"]),
+            ))
+    for key, seq, frame, bgr, role, kind, width, height in jobs:
+        try:
+            preview, jpeg = wants(key)
+            if kind == "depth":
+                if preview:
+                    colored = depth_preview(frame)
+                    _store(key, colored, preview=colored, jpeg=False)
+            elif preview or jpeg:
+                limit = _JPEG_WIDTH if jpeg else _COLOR_WIDTH
+                rgb = rgb_preview(frame, bgr=bgr, limit=limit)
+                shown = rgb
+                if preview and int(rgb.shape[1]) > _COLOR_WIDTH:
+                    shown = rgb_preview(rgb, bgr=False, limit=_COLOR_WIDTH)
+                if preview:
+                    _remember_clean(key, shown, role, width, height)
+                    shown = _overlay_detection(shown, role, width, height)
+                _store(key, rgb, preview=shown if preview else None, jpeg=jpeg)
+        except Exception as exc:  # noqa: BLE001 —— 坏的一帧留在状态里，下一帧继续
+            with _LOCK:
+                _STATE["error"] = str(exc)
+        finally:
+            with _LOCK:
+                if _PAINTED.get(key, 0) < seq:
+                    _PAINTED[key] = seq
+
+
+def _paint_loop() -> None:
+    while not _STOP.is_set():
+        _WAKE.wait(0.005)
+        _WAKE.clear()
+        if _STOP.is_set():
+            return
+        try:
+            publish_latest()
+        except Exception as exc:  # noqa: BLE001 —— 画图线程不能因为一帧坏掉就退出
+            with _LOCK:
+                _STATE["error"] = str(exc)
+
+
+def accept_frame(key: str) -> bool:
+    """这一帧要不要做进预览。没人看，或者距上一帧不到间隔，都返回 False。
+
+    彩色实况不再走这里。固定间隔会把相机自己的帧率再砍掉一截。
+    """
+    now = time.monotonic()
+    with _LOCK:
+        if key not in _PREVIEW_WANT and key not in _JPEG_WANT:
+            return False
+        if now < _NEXT.get(key, 0.0):
+            return False
+        _NEXT[key] = now + _INTERVAL
+        return True
+
+
 def _close_external_viewers() -> None:
     """关掉独立的四窗格查看器。它会把左右红外挂出来，本程序不再用它。"""
     subprocess.run(["pkill", "-f", "lib/arm_application/realsense_view"], check=False)
     subprocess.run(["pkill", "-f", "arm_application.realsense_view"], check=False)
 
 
-def _jpeg(img: np.ndarray) -> bytes | None:
+def paint_detection(
+    rgb: np.ndarray,
+    xyxy: list[float] | tuple,
+    src_width: int,
+    src_height: int,
+) -> np.ndarray:
+    """把推理结果画到相机预览上：绿框、画面中心、跟踪红点、置信度和距离。
+
+    ``xyxy`` 前四项是原图像素的框。够长时第 5 项是置信度，第 7、8 项是跟踪点，
+    第 9 项是距离（米）。空框原样返回。
+    """
     import cv2
 
-    if img.size == 0:
+    if rgb.ndim != 3 or src_width <= 0 or src_height <= 0 or len(xyxy) < 4:
+        return rgb
+    scale_x = rgb.shape[1] / float(src_width)
+    scale_y = rgb.shape[0] / float(src_height)
+    x1 = int(round(float(xyxy[0]) * scale_x))
+    y1 = int(round(float(xyxy[1]) * scale_y))
+    x2 = int(round(float(xyxy[2]) * scale_x))
+    y2 = int(round(float(xyxy[3]) * scale_y))
+    height, width = rgb.shape[:2]
+    x1 = max(0, min(width - 1, x1))
+    x2 = max(0, min(width - 1, x2))
+    y1 = max(0, min(height - 1, y1))
+    y2 = max(0, min(height - 1, y2))
+    if x2 <= x1 or y2 <= y1:
+        return rgb
+    out = np.array(rgb, copy=True)
+    thick = max(2, width // 180)
+    cv2.rectangle(out, (x1, y1), (x2, y2), (80, 220, 120), thick)
+    cv2.drawMarker(
+        out,
+        (width // 2, height // 2),
+        (255, 220, 60),
+        cv2.MARKER_CROSS,
+        max(8, width // 24),
+        1,
+    )
+    if len(xyxy) >= 8:
+        px = int(round(float(xyxy[6]) * scale_x))
+        py = int(round(float(xyxy[7]) * scale_y))
+        px = max(0, min(width - 1, px))
+        py = max(0, min(height - 1, py))
+        cv2.circle(out, (px, py), max(3, width // 80), (255, 48, 48), -1)
+    if len(xyxy) >= 5:
+        conf = float(xyxy[4])
+        dist = f"  z={float(xyxy[8]):.3f}m" if len(xyxy) >= 9 else ""
+        cv2.putText(
+            out,
+            f"{conf:.2f}{dist}",
+            (x1, max(14, y1 - 6)),
+            cv2.FONT_HERSHEY_SIMPLEX,
+            0.45,
+            (80, 220, 120),
+            1,
+            cv2.LINE_AA,
+        )
+    return out
+
+
+def inference_status() -> str:
+    """腕部和手外最近一次推理框。超过保持时间就当没有。"""
+    now = time.monotonic()
+    with _LOCK:
+        items = dict(_DET)
+    parts = []
+    for role, title in (("wrist", "腕部"), ("overhead", "手外")):
+        item = items.get(role)
+        if item is None or now - item[0] > _DET_HOLD_S or len(item[1]) < 5:
+            parts.append(f"{title} 没有框")
+            continue
+        parts.append(f"{title} 置信 {float(item[1][4]):.2f}")
+    return "推理  " + "    ".join(parts)
+
+
+def _remember_detection(role: str, data: list[float]) -> None:
+    with _LOCK:
+        if len(data) < 4:
+            _DET.pop(role, None)
+        else:
+            _DET[role] = (time.monotonic(), data)
+    _stamp_role(role)
+
+
+def _remember_clean(key: str, rgb: np.ndarray, role: str, src_w: int, src_h: int) -> None:
+    with _LOCK:
+        _CLEAN[key] = {
+            "rgb": np.array(rgb, copy=True),
+            "role": role,
+            "src_w": int(src_w),
+            "src_h": int(src_h),
+        }
+
+
+def _overlay_detection(rgb: np.ndarray, role: str, src_width: int, src_height: int) -> np.ndarray:
+    now = time.monotonic()
+    with _LOCK:
+        item = _DET.get(role)
+    if item is None or now - item[0] > _DET_HOLD_S or len(item[1]) < 4:
+        return rgb
+    return paint_detection(rgb, item[1], src_width, src_height)
+
+
+def _stamp_role(role: str) -> None:
+    """检测更新后，把框画回这一路相机已经在显示的画面上。"""
+    now = time.monotonic()
+    with _LOCK:
+        frames = [(key, dict(meta)) for key, meta in _CLEAN.items() if meta.get("role") == role]
+        item = _DET.get(role)
+    fresh = item is not None and now - item[0] <= _DET_HOLD_S and len(item[1]) >= 4
+    for key, meta in frames:
+        shown = meta["rgb"]
+        if fresh:
+            shown = paint_detection(shown, item[1], meta["src_w"], meta["src_h"])
+        with _LOCK:
+            _PREVIEW[key] = (np.ascontiguousarray(shown).tobytes(), int(shown.shape[1]), int(shown.shape[0]))
+            _STATE["frames"][key] = True
+            _STATE["stamp"] = time.time()
+
+
+def rgb_preview(image: np.ndarray, *, bgr: bool, limit: int = _COLOR_WIDTH) -> np.ndarray:
+    """缩成宽不超过 ``limit`` 的 RGB。返回的数组自己持有内存，不跟调用方共用缓冲。"""
+    import cv2
+
+    if image.ndim != 3 or image.shape[2] != 3 or image.shape[0] < 1 or image.shape[1] < 1:
+        raise ValueError("彩色预览要的是高×宽×3")
+    view = image
+    width = int(view.shape[1])
+    if width > limit:
+        height = max(1, int(round(int(view.shape[0]) * limit / float(width))))
+        view = cv2.resize(view, (limit, height), interpolation=cv2.INTER_AREA)
+    if bgr:
+        return cv2.cvtColor(view, cv2.COLOR_BGR2RGB)
+    if not view.flags["OWNDATA"] or not view.flags["C_CONTIGUOUS"]:
+        return np.ascontiguousarray(view)
+    return view
+
+
+def depth_preview(depth: np.ndarray, limit: int = _DEPTH_WIDTH) -> np.ndarray:
+    """uint16 深度画成 RGB。百分位只在抽稀后的样本上算，无效像素保持黑色。"""
+    import cv2
+
+    if depth.ndim != 2 or depth.shape[0] < 1 or depth.shape[1] < 1:
+        raise ValueError("深度预览要的是高×宽")
+    step = max(1, int(depth.shape[1]) // limit)
+    small = depth[::step, ::step]
+    valid = small > 0
+    gray = np.zeros(small.shape, np.uint8)
+    if bool(valid.any()):
+        sample = small[valid]
+        if sample.size > 8000:
+            sample = sample[:: max(1, sample.size // 8000)]
+        lo, hi = np.percentile(sample.astype(np.float32, copy=False), [5, 95])
+        hi = max(float(hi), float(lo) + 1.0)
+        norm = np.clip((small.astype(np.float32) - float(lo)) / (hi - float(lo)), 0.0, 1.0)
+        gray = (norm * 255.0).astype(np.uint8)
+        gray[~valid] = 0
+    colored = cv2.applyColorMap(gray, cv2.COLORMAP_JET)
+    colored[~valid] = 0
+    return rgb_preview(colored, bgr=True, limit=limit)
+
+
+def _jpeg_bgr(rgb: np.ndarray) -> bytes | None:
+    import cv2
+
+    if rgb.size == 0:
         return None
-    view = img
-    if view.shape[1] > 640:
-        scale = 640.0 / float(view.shape[1])
-        view = cv2.resize(view, (640, max(1, int(view.shape[0] * scale))))
-    ok, buf = cv2.imencode(".jpg", view, [int(cv2.IMWRITE_JPEG_QUALITY), 70])
+    ok, buf = cv2.imencode(
+        ".jpg",
+        cv2.cvtColor(rgb, cv2.COLOR_RGB2BGR),
+        [int(cv2.IMWRITE_JPEG_QUALITY), 70],
+    )
     if not ok:
         return None
     return buf.tobytes()
 
 
-def _depth_bgr(depth: np.ndarray) -> np.ndarray:
-    import cv2
-
-    vis = np.zeros((depth.shape[0], depth.shape[1], 3), np.uint8)
-    valid = depth > 0
-    if not bool(valid.any()):
-        return vis
-    values = depth.astype(np.float32)
-    lo, hi = np.percentile(values[valid], [5, 95])
-    hi = max(float(hi), float(lo) + 1.0)
-    gray = (np.clip((values - lo) / (hi - lo), 0.0, 1.0) * 255.0).astype(np.uint8)
-    gray[~valid] = 0
-    colored = cv2.applyColorMap(gray, cv2.COLORMAP_JET)
-    colored[~valid] = 0
-    return colored
+def _as_array(data, dtype, count: int) -> np.ndarray:
+    """优先直接看消息缓冲。缓冲不支持时才拷一份。"""
+    try:
+        arr = np.frombuffer(data, dtype=dtype, count=count)
+    except (TypeError, BufferError, ValueError):
+        arr = np.frombuffer(bytes(data), dtype=dtype, count=count)
+    if arr.size < count:
+        raise ValueError("图像字节数不够")
+    return arr
 
 
-def _color_array(msg) -> np.ndarray | None:
+def _color_view(msg) -> np.ndarray | None:
     if msg.encoding not in ("bgr8", "rgb8") or msg.height <= 0 or msg.width <= 0:
         return None
-    arr = np.frombuffer(bytes(msg.data), dtype=np.uint8)
     need = int(msg.height) * int(msg.width) * 3
-    if arr.size < need:
-        return None
-    arr = arr[:need].reshape(int(msg.height), int(msg.width), 3)
-    if msg.encoding == "rgb8":
-        arr = arr[:, :, ::-1]
-    return np.ascontiguousarray(arr)
+    arr = _as_array(msg.data, np.uint8, need)
+    return arr.reshape(int(msg.height), int(msg.width), 3)
 
 
-def _depth_array(msg) -> np.ndarray | None:
+def _depth_view(msg) -> np.ndarray | None:
     if msg.encoding not in ("16UC1", "mono16") or msg.height <= 0 or msg.width <= 0:
         return None
-    arr = np.frombuffer(bytes(msg.data), dtype=np.uint16)
     need = int(msg.height) * int(msg.width)
-    if arr.size < need:
-        return None
-    return np.ascontiguousarray(arr[:need].reshape(int(msg.height), int(msg.width)))
+    arr = _as_array(msg.data, np.uint16, need)
+    return arr.reshape(int(msg.height), int(msg.width))
+
+
+def _store(key: str, rgb: np.ndarray, *, preview: np.ndarray | None, jpeg: bool) -> None:
+    encoded = _jpeg_bgr(rgb) if jpeg else None
+    with _LOCK:
+        if preview is not None:
+            _PREVIEW[key] = (preview.tobytes(), int(preview.shape[1]), int(preview.shape[0]))
+        if encoded:
+            _JPEG[key] = encoded
+        if preview is not None or encoded:
+            _STATE["frames"][key] = True
+            _STATE["stamp"] = time.time()
+            _STATE["error"] = ""
 
 
 def _loop(cams: list[dict]) -> None:
     import rclpy
     from rclpy.node import Node
-    from rclpy.qos import qos_profile_sensor_data
+    from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy, qos_profile_sensor_data
     from sensor_msgs.msg import Image
-
-    latest: dict[str, np.ndarray] = {}
-    lock = threading.Lock()
+    from std_msgs.msg import Float32MultiArray
 
     class _Node(Node):
         def __init__(self) -> None:
             super().__init__("operator_camera_view")
+            det_qos = QoSProfile(
+                history=HistoryPolicy.KEEP_LAST,
+                depth=5,
+                reliability=ReliabilityPolicy.RELIABLE,
+            )
+            self.create_subscription(
+                Float32MultiArray, "workpiece/detection", self._det("wrist"), det_qos
+            )
+            self.create_subscription(
+                Float32MultiArray,
+                "workpiece/detection_overhead",
+                self._det("overhead"),
+                det_qos,
+            )
+            self._depth_subs: dict[str, object] = {}
             for cam in cams:
                 name = cam["id"]
+                role = str(cam.get("role") or "wrist")
                 self.create_subscription(
                     Image,
                     f"/{name}/color/image_raw",
-                    self._color(name),
-                    qos_profile_sensor_data,
-                )
-                self.create_subscription(
-                    Image,
-                    f"/{name}/aligned_depth_to_color/image_raw",
-                    self._depth(name),
+                    self._color(name, role),
                     qos_profile_sensor_data,
                 )
 
-        def _color(self, name: str):
+        def sync_depth(self) -> None:
+            """深度没打开就不订。订上之后每一帧都会整幅拷进进程，彩色帧率会被拖下去。"""
+            with _LOCK:
+                want = {
+                    cam["id"]
+                    for cam in cams
+                    if f"{cam['id']}_depth" in _PREVIEW_WANT
+                }
+            for cam in cams:
+                name = cam["id"]
+                if name in want and name not in self._depth_subs:
+                    self._depth_subs[name] = self.create_subscription(
+                        Image,
+                        f"/{name}/aligned_depth_to_color/image_raw",
+                        self._depth(name),
+                        qos_profile_sensor_data,
+                    )
+                elif name not in want and name in self._depth_subs:
+                    self.destroy_subscription(self._depth_subs.pop(name))
+
+        def _det(self, role: str):
+            def _cb(msg: Float32MultiArray) -> None:
+                _remember_detection(role, [float(v) for v in msg.data])
+            return _cb
+
+        def _color(self, name: str, role: str):
+            key = f"{name}_color"
+
             def _cb(msg: Image) -> None:
-                arr = _color_array(msg)
-                if arr is None:
-                    return
-                with lock:
-                    latest[f"{name}_color"] = arr
+                # 这里只留最新原图。颜色转换和画框放到画图线程，避免把后面的帧堵住。
+                try:
+                    view = _color_view(msg)
+                    if view is None:
+                        return
+                    offer_color(key, view, bgr=(msg.encoding == "bgr8"), role=role)
+                except Exception as exc:  # noqa: BLE001 —— 坏的一帧留在状态里，订阅不停
+                    with _LOCK:
+                        _STATE["error"] = str(exc)
             return _cb
 
         def _depth(self, name: str):
+            key = f"{name}_depth"
+
             def _cb(msg: Image) -> None:
-                arr = _depth_array(msg)
-                if arr is None:
-                    return
-                with lock:
-                    latest[f"{name}_depth"] = arr
+                try:
+                    view = _depth_view(msg)
+                    if view is None:
+                        return
+                    offer_depth(key, view)
+                except Exception as exc:  # noqa: BLE001 —— 坏的一帧留在状态里，订阅不停
+                    with _LOCK:
+                        _STATE["error"] = str(exc)
             return _cb
 
     rclpy.init()
     node = _Node()
-    next_encode = 0.0
     try:
         while not _STOP.is_set():
-            rclpy.spin_once(node, timeout_sec=0.05)
-            now = time.monotonic()
-            if now < next_encode:
-                continue
-            next_encode = now + 0.12
-            with lock:
-                snap = dict(latest)
-                latest.clear()
-            frames = {}
-            blobs = {}
-            for key, arr in snap.items():
-                name, _, kind = key.rpartition("_")
-                img = _depth_bgr(arr) if kind == "depth" else arr
-                blob = _jpeg(img)
-                if not blob:
-                    continue
-                blobs[key] = blob
-                frames[key] = True
-            if blobs:
-                with _LOCK:
-                    _JPEG.update(blobs)
-                    _STATE["frames"].update(frames)
-                    _STATE["stamp"] = time.time()
-                    _STATE["error"] = ""
+            node.sync_depth()
+            rclpy.spin_once(node, timeout_sec=0.01)
     except Exception as exc:
         with _LOCK:
             _STATE["error"] = str(exc)
@@ -200,8 +645,8 @@ def _loop(cams: list[dict]) -> None:
 
 
 def start() -> str:
-    global _THREAD
-    cams = targets()
+    global _THREAD, _PAINT_THREAD
+    cams = stream_targets()
     if not cams:
         with _LOCK:
             _STATE["error"] = "site 里没有可预览的真机相机"
@@ -213,14 +658,18 @@ def start() -> str:
         _STATE["on"] = True
         _STATE["error"] = ""
     _STOP.clear()
+    _WAKE.clear()
     _THREAD = threading.Thread(target=_loop, args=(cams,), daemon=True, name="cam-view")
     _THREAD.start()
+    _PAINT_THREAD = threading.Thread(target=_paint_loop, daemon=True, name="cam-paint")
+    _PAINT_THREAD.start()
     names = "、".join(cam["label"] for cam in cams)
-    return f"预览订了 {names} 的彩色和深度，不含左右红外。"
+    return f"预览订了 {names} 的彩色原图，深度点开才订，不含左右红外。"
 
 
 def stop() -> None:
     _STOP.set()
-    thread = _THREAD
-    if thread is not None:
-        thread.join(timeout=1.5)
+    _WAKE.set()
+    for thread in (_THREAD, _PAINT_THREAD):
+        if thread is not None:
+            thread.join(timeout=1.5)

install/arm_application/lib/python3.11/site-packages/arm_application/operator_console.py

@@ -12,6 +12,7 @@ from __future__ import annotations
 import json
 import os
 import re
+import signal
 import subprocess
 import threading
 import time
@@ -23,7 +24,7 @@ from urllib.parse import urlparse
 import yaml
 
 from arm_application import operator_tactile_view as tactile_view
-from arm_application.site_io import eye_to_hand_result_name, overhead_camera
+from arm_application.site_io import eye_to_hand_result_name, listed_arms, overhead_camera, usb_expectations
 
 HOST = "127.0.0.1"
 PORT = 8766
@@ -36,11 +37,60 @@ else:
     ROOT = Path.home() / "ros2"
 
 LOG_DIR = Path("/tmp/orisys_console")
+
+# 动臂和软拖拽都要这台臂，不能一边下发位置一边再开拖拽。
+# 相机、夹爪、触觉不碰这条，也不该排在它后面等。
+_ROBOT_LANES = frozenset({"arm", "teach"})
+_ARM_ACTIONS = frozenset({
+    "pipeline", "stage_track", "stage_locate", "stage_screw",
+    "reset_home", "gauge_run", "real_up", "handeye_calibrate",
+})
+_LANE_WAIT = {
+    "grip": "夹爪上一条还没到位",
+    "camera": "相机命令还没返回",
+    "tactile": "触觉命令还没返回",
+    "arm": "这台臂上一条运动还没结束",
+    "teach": "软拖拽还没返回",
+}
+
+
+def command_lane(action: str) -> str:
+    """每类命令一条通道。同一条还没返回才拒绝，不让相机去等夹爪。"""
+    if action == "stop_commands":
+        return "stop"
+    if action in ("camera_d405", "camera_d435"):
+        return "camera"
+    if action.startswith("tactile"):
+        return "tactile"
+    if action.startswith("gripper"):
+        return "grip"
+    if action == "handeye_teach":
+        return "teach"
+    if action in _ARM_ACTIONS:
+        return "arm"
+    return action
+
+
+def command_blocked_message(running: set[str], lane: str) -> str:
+    """这条通道，或同一台臂上的另一条运动，还没结束时返回要显示的句子。"""
+    if lane == "stop":
+        return ""
+    if lane in running:
+        return _LANE_WAIT.get(lane, "这一条命令还没返回")
+    if lane in _ROBOT_LANES:
+        other = next((name for name in _ROBOT_LANES if name != lane and name in running), "")
+        if other == "arm":
+            return "这台臂还在动，软拖拽先不开"
+        if other == "teach":
+            return "软拖拽占着这台臂，运动先不开"
+    return ""
 ASSETS = Path(__file__).resolve().parent
 _LOCK = threading.Lock()
 _TACTILE = {"left": {}, "right": {}, "stamp": 0.0}
 _HIST = {"left": deque(maxlen=360), "right": deque(maxlen=360)}
 _PROCS: dict[str, int] = {}
+_LIVE: dict[str, subprocess.Popen] = {}
+_PROC_LOCK = threading.Lock()
 
 
 def _push_force(side: str, sample: dict) -> None:
@@ -76,29 +126,26 @@ def _site() -> dict:
 
 
 def _arms() -> list[dict]:
-    robots = (_site().get("robot") or {}).get("robots") or []
-    out = []
-    for robot in robots:
-        if not isinstance(robot, dict):
-            continue
-        sn = str(robot.get("sn") or "")
-        arm_no = "89" if sn.endswith("063989") else "91" if sn.endswith("063991") else ""
-        if not arm_no:
-            continue
-        out.append({
-            "id": arm_no,
-            "label": str(robot.get("label") or f"臂{arm_no}"),
-            "sn": sn,
-            "tactile": bool(robot.get("tactile")),
-            "camera": str((robot.get("camera") or {}).get("name") or ""),
-            "gripper": str((robot.get("gripper") or {}).get("model") or ""),
-        })
-    return out or [
-        {"id": "89", "label": "左臂89", "sn": "", "tactile": True, "camera": "d405_left", "gripper": ""},
-        {"id": "91", "label": "右臂91", "sn": "", "tactile": False, "camera": "d405_right", "gripper": ""},
+    """真机臂来自 site.yaml。没有清单就空着，不补本机的 89/91。"""
+    return [
+        {
+            "id": row["id"],
+            "label": row["label"],
+            "sn": row["sn"],
+            "tactile": row["tactile"],
+            "camera": row["camera"],
+            "gripper": row["gripper"],
+            "rotary": row["rotary"],
+        }
+        for row in listed_arms(_site())
     ]
 
 
+def _default_arm() -> str:
+    rows = _arms()
+    return str(rows[0]["id"]) if rows else ""
+
+
 def _usb() -> list[dict]:
     found = {"d405": [], "d435": [], "orisys": []}
     want = {
@@ -119,10 +166,11 @@ def _usb() -> list[dict]:
             continue
         speed = (dev / "speed").read_text().strip() if (dev / "speed").is_file() else "?"
         found[key].append({"port": dev.name, "speed": speed})
+    expect = usb_expectations(_site())
     return [
-        {"id": "d405", "label": "腕部 D405", "expect": 2, "items": found["d405"]},
-        {"id": "d435", "label": "手外 D435", "expect": 1, "items": found["d435"]},
-        {"id": "orisys", "label": "视触觉", "expect": 2, "items": found["orisys"]},
+        {"id": "d405", "label": "腕部 D405", "expect": expect["d405"], "items": found["d405"]},
+        {"id": "d435", "label": "手外 D435", "expect": expect["d435"], "items": found["d435"]},
+        {"id": "orisys", "label": "视触觉", "expect": expect["orisys"], "items": found["orisys"]},
     ]
 
 
@@ -214,13 +262,16 @@ def _poll_tactile() -> None:
 
 
 def _robot(arm: str) -> dict:
+    from arm_application.site_io import arm_short_id
+
     robots = (_site().get("robot") or {}).get("robots") or []
     for robot in robots:
         if not isinstance(robot, dict):
             continue
         sn = str(robot.get("sn") or "")
-        short = "89" if sn.endswith("063989") else "91" if sn.endswith("063991") else ""
-        if short == arm:
+        if sn.lower() == "sim":
+            continue
+        if arm_short_id(sn) == arm:
             return robot
     return {}
 
@@ -305,12 +356,30 @@ def _handeye(arm: str) -> dict:
         or shared.get("handeye_waypoints_eye_to_hand")
         or (ROOT / "handeye" / "poses" / f"{arm}_eye_to_hand.yaml")
     ))
+    wrist = _result_card(wrist_file, wrist_pose, boards.get("eye_in_hand") or {})
+    wrist_name = ""
+    if isinstance(robot.get("camera"), dict):
+        wrist_name = str(robot["camera"].get("name") or "").strip().strip("/")
+    wrist["preview_camera"] = wrist_name
+    wrist["preview_label"] = "腕部相机"
+    outside = _result_card(e2h_file, e2h_pose, boards.get("eye_to_hand") or {})
+    outside["preview_camera"] = cam_name.strip().strip("/")
+    outside["preview_label"] = "手外相机"
     return {
-        "eye_in_hand": _result_card(wrist_file, wrist_pose, boards.get("eye_in_hand") or {}),
-        "eye_to_hand": _result_card(e2h_file, e2h_pose, boards.get("eye_to_hand") or {}),
+        "eye_in_hand": wrist,
+        "eye_to_hand": outside,
     }
 
 
+def _by_arm(reader) -> dict:
+    return {row["id"]: reader(row["id"]) for row in _arms()}
+
+
+def _gripper_live() -> dict:
+    from arm_application.operator_gripper_state import snapshot as grip_snapshot
+    return _by_arm(grip_snapshot)
+
+
 def status() -> dict:
     with _LOCK:
         tactile = {
@@ -322,20 +391,23 @@ def status() -> dict:
             "left": list(_HIST["left"]),
             "right": list(_HIST["right"]),
         }
+    from arm_application.operator_arm_state import snapshot as arm_snapshot
     return {
         "arms": _arms(),
+        "arm_live": _by_arm(arm_snapshot),
+        "gripper_live": _gripper_live(),
         "usb": _usb(),
         "services": _services(),
         "grippers": _grippers(),
         "tactile": tactile,
         "history": history,
         "view": tactile_view.state(),
-        "handeye": {"89": _handeye("89"), "91": _handeye("91")},
+        "handeye": _by_arm(_handeye),
         "now": time.time(),
     }
 
 
-# 会动真机的动作必须 confirm。argv 里的 {arm} 只接受 89 或 91。
+# 会动真机的动作必须 confirm。臂号必须是 site.yaml 里某台真机的末两位。
 _ACTIONS = {
     "real_up": {
         "title": "上电控制栈",
@@ -377,21 +449,21 @@ _ACTIONS = {
         "script": "run_gripper.sh",
         "args": ["--arm", "{arm}", "--cli", "probe"],
         "moves": False,
-        "blurb": "读夹爪是否在线，不动手指。",
+        "blurb": "读夹爪是否在线，不动手指。驱动没在跑时先拉起，不回零。",
     },
     "gripper_open": {
         "title": "夹爪张开",
         "script": "run_gripper.sh",
         "args": ["--arm", "{arm}", "--cli", "open"],
         "moves": True,
-        "blurb": "张开当前臂夹爪。",
+        "blurb": "张开当前臂夹爪。驱动没在跑时先拉起，不回零。",
     },
     "gripper_close": {
         "title": "夹爪闭合",
         "script": "run_gripper.sh",
         "args": ["--arm", "{arm}", "--cli", "close"],
         "moves": True,
-        "blurb": "闭合当前臂夹爪。",
+        "blurb": "闭合当前臂夹爪。驱动没在跑时先拉起，不回零。",
     },
     "gripper_rotate": {
         "title": "右爪转 90°",
@@ -401,11 +473,11 @@ _ACTIONS = {
         "blurb": "右臂旋转爪转到 90°。左臂平行爪没有这一轴。",
     },
     "pipeline": {
-        "title": "整体三段",
+        "title": "跟踪并对孔",
         "script": "run_all.sh",
         "args": ["--arm", "{arm}"],
         "moves": True,
-        "blurb": "跟踪、对孔、搜孔轻压一次跑完。会动臂。",
+        "blurb": "跟踪和对孔一次跑完。搜孔轻压已取消，这一步不含旋入。会动臂。",
     },
     "stage_track": {
         "title": "只测跟踪",
@@ -421,12 +493,12 @@ _ACTIONS = {
         "moves": True,
         "blurb": "只跑二阶段：朝下、悬停、拍照、对准孔。会动臂。",
     },
-    "stage_search": {
-        "title": "只测搜孔",
+    "stage_screw": {
+        "title": "只测旋转装配",
         "script": "run_stage3_search.sh",
-        "args": ["--arm", "{arm}"],
+        "args": ["--arm", "{arm}", "--screw"],
         "moves": True,
-        "blurb": "只跑三阶段：触面、搜孔、轻压。会动臂。臂应已停在孔上方。",
+        "blurb": "按现场配置的拧入深度和螺距把螺纹规旋进去。会动臂。臂应已在孔口。",
     },
     "reset_home": {
         "title": "复位到 home",
@@ -462,9 +534,9 @@ _ACTIONS = {
     "handeye_teach": {
         "title": "示教采点",
         "script": "run_handeye.sh",
-        "args": ["--arm", "{arm}", "teach", "--mode", "{mode}", "--points", "12"],
+        "args": ["--arm", "{arm}", "teach", "--mode", "{mode}", "--points", "{points}"],
         "moves": True,
-        "blurb": "软拖拽示教，目标 12 个姿态。要先停控制栈，会动臂。",
+        "blurb": "软拖拽示教。点数和是否接着采由手眼页上的控件决定。要先停控制栈，会动臂。",
     },
     "handeye_merge": {
         "title": "合并示教点",
@@ -507,6 +579,63 @@ def catalog() -> dict:
     }
 
 
+def _remember(action: str, proc: subprocess.Popen) -> None:
+    with _PROC_LOCK:
+        _LIVE[action] = proc
+        _PROCS[action] = proc.pid
+
+
+def _forget(action: str) -> None:
+    with _PROC_LOCK:
+        _LIVE.pop(action, None)
+
+
+def _pid_alive(pid: int) -> bool:
+    if pid <= 0:
+        return False
+    try:
+        os.kill(pid, 0)
+    except OSError:
+        return False
+    return True
+
+
+def stop_started() -> dict:
+    """停掉本界面拉起的命令。只向登记过的进程组发信号，不扫全机。"""
+    with _PROC_LOCK:
+        items = list(_LIVE.items())
+    if not items:
+        return {"ok": True, "message": "没有正在跑的命令"}
+    names: list[str] = []
+    for action, proc in items:
+        pid = int(getattr(proc, "pid", 0) or 0)
+        if not _pid_alive(pid):
+            _forget(action)
+            continue
+        try:
+            os.killpg(os.getpgid(pid), signal.SIGTERM)
+        except ProcessLookupError:
+            _forget(action)
+            continue
+        except OSError as exc:
+            return {"ok": False, "message": f"停不了 {action}：{exc}"}
+        names.append(action)
+    time.sleep(0.3)
+    for action, proc in items:
+        pid = int(getattr(proc, "pid", 0) or 0)
+        if not _pid_alive(pid):
+            _forget(action)
+            continue
+        try:
+            os.killpg(os.getpgid(pid), signal.SIGKILL)
+        except OSError:
+            pass
+        _forget(action)
+    if not names:
+        return {"ok": True, "message": "没有正在跑的命令"}
+    return {"ok": True, "message": "已停止：" + "、".join(names)}
+
+
 def _stop_tactile() -> str:
     subprocess.run(["pkill", "-f", "lib/arm_application/tactile"], check=False)
     subprocess.run(["pkill", "-f", "arm_application/tactile_node.py"], check=False)
@@ -514,7 +643,39 @@ def _stop_tactile() -> str:
     return "已发送停止视触觉节点"
 
 
-def run_action(action: str, arm: str, confirm: bool, mode: str = "eye_in_hand") -> dict:
+def render_action_args(
+    template: list[str], arm: str, mode: str, extras: dict | None = None,
+) -> list[str]:
+    """把动作模板里的臂号、模式、点数换成这次界面上的值。"""
+    extra = extras or {}
+    needs_points = any("{points}" in str(token) for token in template)
+    points_text = ""
+    if needs_points:
+        try:
+            points = int(extra.get("points", 12))
+        except (TypeError, ValueError):
+            raise ValueError("目标点数不是整数") from None
+        if points < 4 or points > 30:
+            raise ValueError("目标点数只接受 4–30")
+        points_text = str(points)
+    repl = {"{arm}": arm, "{mode}": mode, "{points}": points_text}
+    out: list[str] = []
+    for token in template:
+        text = str(token)
+        for key, value in repl.items():
+            text = text.replace(key, value)
+        out.append(text)
+    if extra.get("append"):
+        if "teach" not in out:
+            raise ValueError("只有示教采点能接着采")
+        out.append("--append")
+    return out
+
+
+def run_action(
+    action: str, arm: str, confirm: bool, mode: str = "eye_in_hand",
+    extras: dict | None = None,
+) -> dict:
     if action == "tactile_down":
         tactile_view.stop()
         return {"ok": True, "message": _stop_tactile()}
@@ -528,8 +689,8 @@ def run_action(action: str, arm: str, confirm: bool, mode: str = "eye_in_hand")
     spec = _ACTIONS.get(action)
     if spec is None:
         return {"ok": False, "message": "没有这个动作"}
-    if arm not in ("89", "91"):
-        return {"ok": False, "message": "臂号只接受 89 或 91"}
+    if arm not in {row["id"] for row in _arms()}:
+        return {"ok": False, "message": f"臂号 {arm} 不在 site.yaml 里"}
     if mode not in ("eye_in_hand", "eye_to_hand"):
         return {"ok": False, "message": "手眼模式只接受 eye_in_hand 或 eye_to_hand"}
     if spec["moves"] and not confirm:
@@ -537,40 +698,86 @@ def run_action(action: str, arm: str, confirm: bool, mode: str = "eye_in_hand")
     script = ROOT / "scripts" / spec["script"]
     if not script.is_file():
         return {"ok": False, "message": f"找不到 {script}"}
-    args = [a.replace("{arm}", arm).replace("{mode}", mode) for a in spec["args"]]
+    from arm_application.operator_arm_state import (
+        hold_until_idle,
+        motion_blocked,
+        release_for_control,
+        resume_after_control,
+        snapshot as arm_snapshot,
+        stack_running,
+    )
+    blocked = motion_blocked(
+        action,
+        stack_up=stack_running(),
+        op_status=str((arm_snapshot(arm) or {}).get("op_status") or ""),
+    )
+    if blocked:
+        return {"ok": False, "message": blocked}
+    uses_rdk = action in {
+        "real_up", "handeye_teach", "pipeline", "stage_track", "stage_locate",
+        "stage_screw", "reset_home", "gauge_run", "handeye_calibrate",
+    }
+    if uses_rdk:
+        release_for_control()
+    from arm_application.operator_gripper_state import hold_serial
+    grip_port = action in {
+        "gripper_probe", "gripper_open", "gripper_close", "gripper_rotate", "gripper_up",
+    }
+    try:
+        args = render_action_args(list(spec["args"]), arm, mode, extras)
+    except ValueError as exc:
+        return {"ok": False, "message": str(exc)}
     LOG_DIR.mkdir(parents=True, exist_ok=True)
     log_path = LOG_DIR / f"{action}_{arm}.log"
     shown = " ".join(["bash", f"scripts/{spec['script']}", *args])
-    if spec.get("sync"):
-        try:
-            proc = subprocess.run(
+    try:
+        if spec.get("sync"):
+            proc = subprocess.Popen(
                 ["bash", str(script), *args],
                 cwd=str(ROOT),
-                capture_output=True,
+                stdout=subprocess.PIPE,
+                stderr=subprocess.STDOUT,
                 text=True,
-                timeout=float(spec.get("timeout") or 40),
+                start_new_session=True,
             )
-        except subprocess.TimeoutExpired:
-            return {"ok": False, "message": "命令超时还没返回", "detail": shown}
-        detail = ((proc.stdout or "") + (proc.stderr or "")).strip()
-        log_path.write_text(detail)
-        ok = proc.returncode == 0
-        return {
-            "ok": ok,
-            "message": shown if ok else f"退出码 {proc.returncode}",
-            "detail": detail[-4000:],
-            "log": str(log_path),
-        }
-    log_f = log_path.open("ab")
-    proc = subprocess.Popen(
-        ["bash", str(script), *args],
-        cwd=str(ROOT),
-        stdout=log_f,
-        stderr=subprocess.STDOUT,
-        start_new_session=True,
-    )
-    _PROCS[action] = proc.pid
-    return {"ok": True, "message": shown, "pid": proc.pid, "log": str(log_path)}
+            _remember(action, proc)
+            try:
+                detail, _ = proc.communicate(timeout=float(spec.get("timeout") or 40))
+            except subprocess.TimeoutExpired:
+                try:
+                    os.killpg(os.getpgid(proc.pid), signal.SIGKILL)
+                except OSError:
+                    pass
+                return {"ok": False, "message": "命令超时还没返回", "detail": shown}
+            finally:
+                _forget(action)
+            detail = (detail or "").strip()
+            log_path.write_text(detail)
+            ok = proc.returncode == 0
+            return {
+                "ok": ok,
+                "message": shown if ok else f"退出码 {proc.returncode}",
+                "detail": detail[-4000:],
+                "log": str(log_path),
+            }
+        log_f = log_path.open("ab")
+        proc = subprocess.Popen(
+            ["bash", str(script), *args],
+            cwd=str(ROOT),
+            stdout=log_f,
+            stderr=subprocess.STDOUT,
+            start_new_session=True,
+        )
+        _remember(action, proc)
+        if grip_port:
+            hold_serial(proc.pid)
+        if uses_rdk:
+            hold_until_idle(proc.pid, until_stack=(action == "real_up"))
+            uses_rdk = False
+        return {"ok": True, "message": shown, "pid": proc.pid, "log": str(log_path)}
+    finally:
+        if uses_rdk:
+            resume_after_control()
 
 
 def _bounded(name: str, raw, low: float, high: float) -> float:
@@ -585,12 +792,12 @@ def _bounded(name: str, raw, low: float, high: float) -> float:
 
 def run_gripper_tune(arm: str, kind: str, numbers: dict) -> dict:
     """按界面上的位置、力度、速度下发夹爪。右臂另有角度、圈数。"""
-    if arm not in ("89", "91"):
-        return {"ok": False, "message": "臂号只接受 89 或 91"}
+    if arm not in {row["id"] for row in _arms()}:
+        return {"ok": False, "message": f"臂号 {arm} 不在 site.yaml 里"}
     if kind not in ("pos", "rotate", "rotate_by", "revolutions"):
         return {"ok": False, "message": "没有这种夹爪调节"}
-    if kind != "pos" and arm != "91":
-        return {"ok": False, "message": "左臂平行爪没有旋转轴"}
+    if kind != "pos" and not any(row["id"] == arm and row.get("rotary") for row in _arms()):
+        return {"ok": False, "message": "这台夹爪没有旋转轴"}
     try:
         if kind == "pos":
             pos = int(round(_bounded("位置", numbers.get("pos"), 0, 1000)))
@@ -615,7 +822,17 @@ def run_gripper_tune(arm: str, kind: str, numbers: dict) -> dict:
                 summary = f"转 {turns:.3f} 圈  力矩 {force}%  速度 {speed}%"
     except ValueError as exc:
         return {"ok": False, "message": str(exc)}
+    from arm_application.operator_gripper_state import pause_serial, resume_serial
+
     script = ROOT / "scripts" / "run_gripper.sh"
+    pause_serial()
+    try:
+        return _run_gripper_argv(script, arm, force, speed, cli, summary)
+    finally:
+        resume_serial()
+
+
+def _run_gripper_argv(script, arm: str, force: int, speed: int, cli: list[str], summary: str) -> dict:
     argv = [
         "bash", str(script),
         "--arm", arm,
@@ -624,19 +841,40 @@ def run_gripper_tune(arm: str, kind: str, numbers: dict) -> dict:
         "--cli", *cli,
     ]
     shown = " ".join(argv)
+    proc = subprocess.Popen(
+        argv, cwd=str(ROOT), stdout=subprocess.PIPE, stderr=subprocess.STDOUT,
+        text=True, start_new_session=True,
+    )
+    _remember(f"gripper_{arm}", proc)
     try:
-        proc = subprocess.run(argv, cwd=str(ROOT), capture_output=True, text=True, timeout=90)
-    except subprocess.TimeoutExpired:
-        return {"ok": False, "message": "夹爪 90 秒还没到位", "detail": shown}
-    detail = ((proc.stdout or "") + (proc.stderr or "")).strip()
+        try:
+            detail, _ = proc.communicate(timeout=90)
+        except subprocess.TimeoutExpired:
+            try:
+                os.killpg(os.getpgid(proc.pid), signal.SIGKILL)
+            except OSError:
+                pass
+            return {"ok": False, "message": "夹爪 90 秒还没到位", "detail": shown}
+    finally:
+        _forget(f"gripper_{arm}")
+    detail = (detail or "").strip()
     ok = proc.returncode == 0
     return {
         "ok": ok,
-        "message": summary if ok else f"夹爪退出码 {proc.returncode}",
+        "message": summary if ok else _grip_failure_message(proc.returncode, detail),
         "detail": (summary + "\n" + shown + "\n" + detail)[-4000:],
     }
 
 
+def _grip_failure_message(code: int, detail: str) -> str:
+    """退出码旁边带上脚本打出来的原因，别只剩一个数字。"""
+    for line in reversed(detail.splitlines()):
+        text = line.strip()
+        if text.startswith("ERROR:") or text.startswith("无法"):
+            return text
+    return f"夹爪退出码 {code}"
+
+
 class Handler(BaseHTTPRequestHandler):
     def log_message(self, fmt: str, *args) -> None:
         return
@@ -700,12 +938,12 @@ class Handler(BaseHTTPRequestHandler):
             return
         if path == "/api/tactile/view":
             action = "tactile_view_on" if body.get("on") else "tactile_view_off"
-            result = run_action(action, "89", False)
+            result = run_action(action, _default_arm(), False)
             self._json(200 if result.get("ok") else 400, result)
             return
         result = run_action(
             str(body.get("action") or ""),
-            str(body.get("arm") or "89"),
+            str(body.get("arm") or _default_arm()),
             bool(body.get("confirm")),
             str(body.get("mode") or "eye_in_hand"),
         )
@@ -715,6 +953,10 @@ class Handler(BaseHTTPRequestHandler):
 def main() -> None:
     import atexit
     atexit.register(tactile_view.stop)
+    from arm_application.operator_arm_state import start as start_arm_state
+    from arm_application.operator_gripper_state import start as start_gripper_state
+    start_arm_state()
+    start_gripper_state()
     threading.Thread(target=_poll_tactile, daemon=True).start()
     server = ThreadingHTTPServer((HOST, PORT), Handler)
     print(f"Orisys 工位界面  http://{HOST}:{PORT}", flush=True)

install/arm_application/lib/python3.11/site-packages/arm_application/operator_gripper_mesh.py

@@ -0,0 +1,138 @@
+"""大寰夹爪的一比一外形。
+
+左臂按下载包 ``PGEA-3D-Model-and-2D-Drawing-4`` 里的
+``PGEA-100-26-W-F`` 尺寸图。图签就是这个型号，总长 138、宽 55、厚 29，
+安装端抱闸约 20，主体 78，手指伸出约 40，闭合缝 1.2。
+现场装的是 ``PGEA-100-20-W``，这包里没有 100-20 的图，开口仍用
+``site.yaml`` 的 20 mm，不用图上的 26。
+
+同包另一张完整的带抱闸图是 ``PGEA-100-40-W-F``（手指更长，张开约 40.8）。
+右臂是 ``RGI-100-30``，这包里没有旋转爪的图，仍按手册外形。
+"""
+
+from __future__ import annotations
+
+import math
+
+# 米。来源见模块说明。长指向指尖，宽是开合方向，厚是图上的 29。
+PGEA_SIZE = (0.138, 0.055, 0.029)
+PGEA_STROKE_MM = 20.0
+PGEA_CLOSED_GAP_M = 0.0012
+PGEA_FINGER_M = 0.040
+RGI_SIZE = (0.158, 0.0755, 0.047)
+RGI_STROKE_MM = 30.0
+
+# 深色底上要能分开：银壳体、蓝抱闸、橙手指、绿导轨。
+# 手指以前是 0.10 的黑，乘上未连接时的 0.55 就沉进背景里。
+_BODY = (0.86, 0.88, 0.91)
+_BRAKE = (0.16, 0.42, 0.90)
+_FINGER = (0.95, 0.46, 0.10)
+_RAIL = (0.18, 0.78, 0.28)
+
+
+def triangles(
+    rotary: bool,
+    permille: float | None,
+    rotation_deg: float | None,
+    stroke_mm: float | None,
+) -> tuple[list[tuple], str]:
+    """返回 ``(三角面, 说明)``。三角面是 ``(a, b, c, 明暗占位, rgb)``，明暗由界面再乘。"""
+    if rotary:
+        length, width, thick = RGI_SIZE
+        stroke = (float(stroke_mm) if stroke_mm else RGI_STROKE_MM) / 1000.0
+        note = (
+            f"RGI-100-30  外形 {length*1000:.0f}×{width*1000:.1f}×{thick*1000:.0f} mm"
+            f"  行程 {stroke*1000:.0f} mm。这包图纸里没有旋转爪，外形仍按手册。"
+        )
+        body_end = 0.118
+        brake = None
+    else:
+        length, width, thick = PGEA_SIZE
+        stroke = (float(stroke_mm) if stroke_mm else PGEA_STROKE_MM) / 1000.0
+        note = (
+            f"PGEA-100-26-W-F 尺寸图  外形 {length*1000:.0f}×{width*1000:.0f}×{thick*1000:.0f} mm"
+            f"  现场行程 {stroke*1000:.0f} mm。包里没有 100-20 的图，开口不用图上的 26。"
+        )
+        body_end = length - PGEA_FINGER_M
+        brake = (0.0, 0.020)
+    gap = stroke * (0.5 if permille is None else max(0.0, min(1.0, float(permille) / 1000.0)))
+    if not rotary:
+        gap += PGEA_CLOSED_GAP_M
+    parts = _solid(length, width, thick, body_end, gap, brake, rotary)
+    if rotary and rotation_deg:
+        parts = _spin_fingers(parts, math.radians(float(rotation_deg)), body_end)
+    out = []
+    for a, b, c, rgb in parts:
+        out.append((a, b, c, _face_shade(a, b, c), rgb))
+    if permille is None:
+        note = "没有读数。下面是半开，不是当前开口。 " + note
+    return out, note
+
+
+def _solid(length, width, thick, body_end, gap, brake, rotary: bool):
+    hx, hy = thick / 2.0, width / 2.0
+    faces = []
+    z0 = 0.0
+    if brake is not None:
+        z0 = brake[1]
+        faces.extend(_box(0.0, brake[1], -hy, hy, -hx, hx, _BRAKE))
+    faces.extend(_box(z0, body_end, -hy, hy, -hx, hx, _BODY))
+    if rotary:
+        finger_t = min(0.010, hy * 0.4)
+        finger_h = thick * 0.72
+    else:
+        # 尺寸图端视图：指厚 6.5，指高 16，都落在 55×29 的外形里面。
+        finger_t = 0.0065
+        finger_h = 0.016
+        rail = 0.003
+        faces.extend(_box(z0, body_end, -hy * 0.62, -hy * 0.62 + rail, hx - rail, hx, _RAIL))
+        faces.extend(_box(z0, body_end, hy * 0.62 - rail, hy * 0.62, hx - rail, hx, _RAIL))
+    half = gap / 2.0
+    faces.extend(_box(body_end, length, -(half + finger_t), -half, -finger_h / 2, finger_h / 2, _FINGER))
+    faces.extend(_box(body_end, length, half, half + finger_t, -finger_h / 2, finger_h / 2, _FINGER))
+    return faces
+
+
+def _spin_fingers(parts, angle: float, body_end: float):
+    c, s = math.cos(angle), math.sin(angle)
+    spun = []
+    for a, b, c0, rgb in parts:
+        if a[0] >= body_end - 1e-6 and b[0] >= body_end - 1e-6 and c0[0] >= body_end - 1e-6:
+            a, b, c0 = (_rot_z(a, c, s), _rot_z(b, c, s), _rot_z(c0, c, s))
+        spun.append((a, b, c0, rgb))
+    return spun
+
+
+def _face_shade(a, b, c) -> float:
+    """顶面亮、底面暗。零件颜色已经分开，明暗再把面分出来。"""
+    ux, uy, uz = b[0] - a[0], b[1] - a[1], b[2] - a[2]
+    vx, vy, vz = c[0] - a[0], c[1] - a[1], c[2] - a[2]
+    nx = uy * vz - uz * vy
+    ny = uz * vx - ux * vz
+    nz = ux * vy - uy * vx
+    length = math.sqrt(nx * nx + ny * ny + nz * nz) or 1.0
+    dot = (nx * 0.15 + ny * -0.25 + nz * 0.95) / length
+    return 0.42 + 0.58 * max(0.0, min(1.0, dot))
+
+
+def _rot_z(point, c, s):
+    x, y, z = point
+    return (x, y * c - z * s, y * s + z * c)
+
+
+def _box(x0, x1, y0, y1, z0, z1, rgb) -> list[tuple]:
+    """轴向盒子。x 指向指尖，y 是开合方向，z 是厚度。"""
+    p = (
+        (x0, y0, z0), (x1, y0, z0), (x1, y1, z0), (x0, y1, z0),
+        (x0, y0, z1), (x1, y0, z1), (x1, y1, z1), (x0, y1, z1),
+    )
+    quads = (
+        (0, 1, 2, 3), (4, 7, 6, 5),
+        (0, 4, 5, 1), (3, 2, 6, 7),
+        (0, 3, 7, 4), (1, 5, 6, 2),
+    )
+    out = []
+    for a, b, c, d in quads:
+        out.append((p[a], p[b], p[c], rgb))
+        out.append((p[a], p[c], p[d], rgb))
+    return out

install/arm_application/lib/python3.11/site-packages/arm_application/operator_gripper_state.py

@@ -0,0 +1,436 @@
+"""工位界面用的夹爪状态：只读开口和转角，并交给界面画出来。
+
+夹爪节点已经占着串口时，只订它发布的话题，不再自己打开串口。
+没有节点时才短连一次。连上之后只读寄存器，不写初始化，避免手指回零把工件松开。
+"""
+
+from __future__ import annotations
+
+import os
+import subprocess
+import threading
+import time
+from typing import Any
+
+import yaml
+
+from arm_application.site_io import listed_arms, ros2_root
+
+_GRIP = {0: "运动中", 1: "已到位", 2: "已夹住", 3: "已掉落"}
+_ROT = {0: "转动中", 1: "已到位", 2: "受阻", 3: "曾经受阻"}
+
+_LOCK = threading.Lock()
+_CACHE: dict[str, dict] = {}
+_STARTED = False
+_PORT_HOLD = 0
+_PORT_BUSY = 0
+_PORT_LOCK = threading.Lock()
+
+
+def snapshot(arm: str) -> dict:
+    with _LOCK:
+        row = _CACHE.get(arm)
+        return dict(row) if row else _empty(arm)
+
+
+def _empty(arm: str) -> dict:
+    row = next((item for item in _arms() if item["id"] == arm), None)
+    return present(
+        arm,
+        rotary=bool(row and row["rotary"]),
+        stroke_mm=float(row["stroke_mm"]) if row else 20.0,
+        reading=None,
+        error="还没读到夹爪",
+    )
+
+
+def present(
+    arm: str,
+    *,
+    rotary: bool,
+    stroke_mm: float,
+    reading: dict | None,
+    error: str = "",
+) -> dict:
+    """把一次读数收成界面能画的字典。不打开串口。"""
+    stroke = float(stroke_mm) if stroke_mm and stroke_mm > 0 else 20.0
+    row = reading or {}
+    permille = _unit(row.get("permille"), 0, 1000)
+    connected = bool(row.get("connected")) and permille is not None
+    opening = None if permille is None else permille / 1000.0 * stroke
+    rotation = _finite(row.get("rotation_deg")) if rotary else 0.0
+    grip_state = row.get("grip_state")
+    rot_state = row.get("rotation_state")
+    if not connected:
+        summary = error or str(row.get("error_msg") or "夹爪没连上")
+    else:
+        bits = [_GRIP.get(int(grip_state), "状态未知") if _finite(grip_state) is not None else "状态未知"]
+        bits.append(f"开口 {opening:.1f} mm" if opening is not None else "")
+        if rotary and rotation is not None:
+            bits.append(f"转角 {rotation:.1f}°")
+            if _finite(rot_state) is not None:
+                bits.append(_ROT.get(int(rot_state), ""))
+        summary = "  ".join(bit for bit in bits if bit)
+    return {
+        "arm": arm,
+        "rotary": rotary,
+        "connected": connected,
+        "initialized": bool(row.get("initialized")) if connected else False,
+        "permille": permille,
+        "opening_mm": opening,
+        "stroke_mm": stroke,
+        "rotation_deg": rotation if rotary else None,
+        "force": _finite(row.get("force")),
+        "speed": _finite(row.get("speed")),
+        "grip_state": int(grip_state) if _finite(grip_state) is not None else None,
+        "grip_text": _GRIP.get(int(grip_state), "—") if _finite(grip_state) is not None else "—",
+        "rotation_text": _ROT.get(int(rot_state), "—") if rotary and _finite(rot_state) is not None else "—",
+        "moving": bool(row.get("moving")) if connected else False,
+        "gripped": bool(row.get("gripped")) or (_finite(grip_state) == 2) if connected else False,
+        "summary": summary,
+        "error": "" if connected else (error or str(row.get("error_msg") or "")),
+        "source": str(row.get("source") or ""),
+        "pose_live": connected,
+        "stamp": time.time(),
+    }
+
+
+def retain_reading(
+    fresh: dict,
+    previous: dict | None,
+    *,
+    now: float | None = None,
+    hold_s: float = 15.0,
+) -> dict:
+    """这一拍没读到开口时，短时间沿用上一拍。
+
+    和机械臂同一件事：话题 echo 偶发超时会把刚读到的开口清成“没连上”，
+    模型和顶栏就停在那里。超过 ``hold_s`` 仍没有新读数才放开。
+    """
+    if fresh.get("connected") or not previous or not previous.get("connected"):
+        return fresh
+    moment = time.time() if now is None else now
+    if moment - float(previous.get("stamp") or 0.0) > hold_s:
+        return fresh
+    kept = dict(fresh)
+    for key in (
+        "connected", "initialized", "permille", "opening_mm", "stroke_mm",
+        "rotation_deg", "force", "speed", "grip_state", "grip_text",
+        "rotation_text", "moving", "gripped", "pose_live", "rotary", "source",
+    ):
+        if key in previous:
+            kept[key] = previous[key]
+    kept["summary"] = str(previous.get("summary") or fresh.get("summary") or "")
+    note = "沿用上次读数。"
+    if note not in kept["summary"]:
+        kept["summary"] = f"{kept['summary']} {note}".strip()
+    return kept
+
+
+def pause_serial() -> None:
+    """控制命令要自己开串口，或把驱动拉起来。先拦住状态线程，等它把口放开。
+
+    一次短读要过好几个寄存器，1.5 秒经常还没结束。提前返回会让新驱动
+    去抢还被占着的口，然后以「需要 24 V」失败。
+    """
+    global _PORT_HOLD
+    with _PORT_LOCK:
+        _PORT_HOLD += 1
+    deadline = time.time() + 6.0
+    while time.time() < deadline:
+        with _PORT_LOCK:
+            busy = _PORT_BUSY
+        if not busy:
+            return
+        time.sleep(0.05)
+
+
+def hold_serial(pid: int) -> None:
+    """后台夹爪脚本还活着时，状态线程不要去开串口。"""
+    pause_serial()
+
+    def _watch() -> None:
+        while pid > 0:
+            try:
+                os.kill(pid, 0)
+            except OSError:
+                break
+            time.sleep(0.2)
+        resume_serial()
+
+    threading.Thread(target=_watch, daemon=True, name="gripper-port-hold").start()
+
+
+def resume_serial() -> None:
+    global _PORT_HOLD
+    with _PORT_LOCK:
+        _PORT_HOLD = max(0, _PORT_HOLD - 1)
+
+
+def serial_paused() -> bool:
+    with _PORT_LOCK:
+        return _PORT_HOLD > 0
+
+
+def start() -> None:
+    global _STARTED
+    with _LOCK:
+        if _STARTED:
+            return
+        _STARTED = True
+    threading.Thread(target=_loop, daemon=True, name="gripper-state").start()
+
+
+def _loop() -> None:
+    while True:
+        try:
+            arms = _arms()
+            nodes = _nodes()
+            for arm in arms:
+                reading, error = _read_one(arm, nodes)
+                fresh = present(
+                    arm["id"],
+                    rotary=arm["rotary"],
+                    stroke_mm=arm["stroke_mm"],
+                    reading=reading,
+                    error=error,
+                )
+                _store(retain_reading(fresh, snapshot(arm["id"])))
+        except Exception as exc:  # noqa: BLE001 —— 读夹爪失败不能拖死界面
+            for row in _arms():
+                _store(present(
+                    row["id"], rotary=bool(row.get("rotary")), stroke_mm=float(row.get("stroke_mm") or 20.0),
+                    reading=None, error=str(exc),
+                ))
+        time.sleep(2.5)
+
+
+def _store(row: dict) -> None:
+    with _LOCK:
+        _CACHE[str(row.get("arm") or "")] = row
+
+
+def _arms() -> list[dict]:
+    path = ros2_root() / "config" / "site.yaml"
+    if not path.is_file():
+        return []
+    data = yaml.safe_load(path.read_text(encoding="utf-8")) or {}
+    shared = data.get("gripper") if isinstance(data.get("gripper"), dict) else {}
+    baud = int(shared.get("baud") or 115200)
+    slave = int(shared.get("slave_id") or 1)
+    rts = str(shared.get("rs485_rts") or "")
+    out = []
+    for row in listed_arms(data):
+        item = dict(row)
+        item["baud"] = baud
+        item["slave"] = slave
+        item["rts"] = rts
+        out.append(item)
+    return out
+
+
+def _nodes() -> dict[str, bool]:
+    text = _pgrep("dh_pgea_node|dual_gripper")
+    return {
+        "pgea": "dh_pgea_node" in text or "lib/arm_application/dh_pgea" in text,
+        "dual": "dual_gripper" in text,
+    }
+
+
+def _read_one(arm: dict, nodes: dict[str, bool]) -> tuple[dict | None, str]:
+    if nodes.get("dual") and arm.get("side") in ("left", "right"):
+        topic = f"/dual_gripper_driver/{arm['side']}/status"
+        parsed = _parse_status_echo(_echo(topic))
+        if parsed is not None:
+            parsed["source"] = f"话题 {topic}"
+            return parsed, ""
+        return None, "夹爪节点在跑，但这侧话题没有数据"
+    if not arm.get("rotary") and nodes.get("pgea"):
+        parsed = _parse_states_echo(_echo("/gripper/states"))
+        if parsed is not None:
+            parsed["source"] = "话题 /gripper/states"
+            return parsed, ""
+        return None, "平行爪节点在跑，但 /gripper/states 没有数据"
+    if not arm.get("port"):
+        return None, "site.yaml 里这台夹爪没有串口"
+    if serial_paused():
+        return None, "控制命令正在用串口"
+    return _read_serial(arm)
+
+
+def _read_serial(arm: dict) -> tuple[dict | None, str]:
+    """短连串口只读。auto_initialize 必须关掉，否则旋转爪一连上就会找零。"""
+    global _PORT_BUSY
+    if serial_paused():
+        return None, "控制命令正在用串口"
+    with _PORT_LOCK:
+        _PORT_BUSY += 1
+    try:
+        return _read_serial_open(arm)
+    finally:
+        with _PORT_LOCK:
+            _PORT_BUSY = max(0, _PORT_BUSY - 1)
+
+
+def _read_serial_open(arm: dict) -> tuple[dict | None, str]:
+    try:
+        if arm["rotary"]:
+            from arm_application.dual_gripper import RotaryGripper, RotaryGripperConfig
+            grip = RotaryGripper(RotaryGripperConfig(
+                port=arm["port"],
+                slave_id=int(arm["slave"]),
+                baudrate=int(arm["baud"]),
+                timeout=0.25,
+                auto_initialize=False,
+            ))
+            grip.connect()
+            try:
+                status = grip.get_status()
+            finally:
+                grip.disconnect()
+            if not status.connected:
+                return None, status.error_msg or "旋转爪没有应答"
+            return {
+                "connected": True,
+                "initialized": bool(status.initialized),
+                "permille": float(status.finger_position_per_mille),
+                "rotation_deg": float(status.rotation_deg),
+                "force": float(status.force),
+                "speed": float(status.velocity),
+                "grip_state": int(status.gripper_state),
+                "rotation_state": int(status.rotation_state),
+                "moving": bool(status.is_moving),
+                "gripped": bool(status.is_gripped),
+                "error_msg": status.error_msg or "",
+                "source": f"串口 {arm['port']} 只读",
+            }, ""
+        from arm_application.dh_pgea import DhPgeaError, DhPgeaGripper
+        grip = DhPgeaGripper(
+            port=arm["port"],
+            baudrate=int(arm["baud"]),
+            slave_id=int(arm["slave"]),
+            stroke_mm=float(arm["stroke_mm"]),
+            rs485_rts=str(arm["rts"]),
+        )
+        grip.open()
+        try:
+            state = grip.get_run_states(retries=1)
+        finally:
+            grip.close()
+        return {
+            "connected": True,
+            "initialized": bool(state.initialized),
+            "permille": float(state.position),
+            "rotation_deg": 0.0,
+            "force": float(state.target_force),
+            "speed": None,
+            "grip_state": int(state.grip_state),
+            "rotation_state": None,
+            "moving": int(state.grip_state) == 0,
+            "gripped": int(state.grip_state) == 2,
+            "error_msg": "",
+            "source": f"串口 {arm['port']} 只读",
+        }, ""
+    except Exception as exc:  # noqa: BLE001 —— 串口被节点占着、没上电，都要变成一句人话
+        text = str(exc).strip() or exc.__class__.__name__
+        if "DhPgeaError" in exc.__class__.__name__ or "Permission" in text or "busy" in text.lower():
+            return None, text
+        return None, text
+
+
+def _echo(topic: str) -> str:
+    try:
+        proc = subprocess.run(
+            ["timeout", "3", "ros2", "topic", "echo", topic, "--once"],
+            capture_output=True, text=True, timeout=5,
+        )
+    except (OSError, subprocess.TimeoutExpired):
+        return ""
+    return proc.stdout
+
+
+def _parse_status_echo(text: str) -> dict | None:
+    for chunk in text.split("---"):
+        if "finger_position_per_mille:" not in chunk and "connected:" not in chunk:
+            continue
+        try:
+            data = yaml.safe_load(chunk)
+        except yaml.YAMLError:
+            continue
+        if not isinstance(data, dict) or "connected" not in data:
+            continue
+        return {
+            "connected": bool(data.get("connected")),
+            "initialized": bool(data.get("initialized")),
+            "permille": data.get("finger_position_per_mille"),
+            "rotation_deg": data.get("rotation_deg"),
+            "force": data.get("force"),
+            "speed": data.get("velocity"),
+            "grip_state": data.get("gripper_state"),
+            "rotation_state": data.get("rotation_state"),
+            "moving": bool(data.get("is_moving")),
+            "gripped": bool(data.get("is_gripped")),
+            "error_msg": str(data.get("error_msg") or ""),
+        }
+    return None
+
+
+def _parse_states_echo(text: str) -> dict | None:
+    for chunk in text.split("---"):
+        if "data:" not in chunk:
+            continue
+        try:
+            data = yaml.safe_load(chunk)
+        except yaml.YAMLError:
+            continue
+        if not isinstance(data, dict):
+            continue
+        raw = data.get("data")
+        if not isinstance(raw, list) or len(raw) < 5:
+            continue
+        try:
+            init, grip, pos, _target, force = (float(v) for v in raw[:5])
+        except (TypeError, ValueError):
+            continue
+        return {
+            "connected": True,
+            "initialized": init >= 0.5,
+            "permille": pos,
+            "rotation_deg": 0.0,
+            "force": force,
+            "speed": None,
+            "grip_state": int(grip),
+            "rotation_state": None,
+            "moving": int(grip) == 0,
+            "gripped": int(grip) == 2,
+            "error_msg": "",
+        }
+    return None
+
+
+def _pgrep(pattern: str) -> str:
+    try:
+        proc = subprocess.run(
+            ["pgrep", "-af", pattern],
+            capture_output=True, text=True, timeout=2,
+        )
+    except (OSError, subprocess.TimeoutExpired):
+        return ""
+    return "\n".join(line for line in proc.stdout.splitlines() if "pgrep" not in line)
+
+
+def _finite(raw: Any) -> float | None:
+    try:
+        value = float(raw)
+    except (TypeError, ValueError):
+        return None
+    if value != value:
+        return None
+    return value
+
+
+def _unit(raw: Any, low: float, high: float) -> float | None:
+    value = _finite(raw)
+    if value is None:
+        return None
+    return max(low, min(high, value))

install/arm_application/lib/python3.11/site-packages/arm_application/operator_handeye_board.py

@@ -0,0 +1,91 @@
+"""手眼页的棋盘预览。只在已有彩色画面上画角点，不打开相机，也不动臂。"""
+
+from __future__ import annotations
+
+import threading
+import time
+
+import numpy as np
+
+from arm_application.handeye_lib import BoardConfig, detect_chessboard
+
+
+def coverage_fraction(corners: np.ndarray, width: int, height: int) -> float:
+    """角点外接框占画面的比例。宽或高为 0 时返回 0。"""
+    if width <= 0 or height <= 0:
+        return 0.0
+    pts = np.asarray(corners, dtype=float).reshape(-1, 2)
+    if pts.size == 0:
+        return 0.0
+    area = float(pts[:, 0].max() - pts[:, 0].min()) * float(pts[:, 1].max() - pts[:, 1].min())
+    return area / float(width * height)
+
+
+def coverage_words(fraction: float) -> str:
+    """30%–70% 是示教时棋盘该占的画面。再小对不准，再大姿态拉不开。"""
+    if fraction < 0.30:
+        return "偏小，再靠近一点"
+    if fraction > 0.70:
+        return "偏大，再离远一点"
+    return "大小合适"
+
+
+def annotate_board(jpeg: bytes, cols: int, rows: int) -> tuple[bytes, str]:
+    """返回（可能画了角点的 JPEG，一行中文状态）。检测失败时 JPEG 原样返回。"""
+    import cv2
+
+    if cols <= 0 or rows <= 0:
+        return jpeg, "配置里没有这块板的内角点"
+    buf = np.frombuffer(jpeg, dtype=np.uint8)
+    image = cv2.imdecode(buf, cv2.IMREAD_COLOR)
+    if image is None:
+        return jpeg, "这一帧解不出来"
+    found = detect_chessboard(image, BoardConfig(int(cols), int(rows), 1.0))
+    if found is None:
+        return jpeg, "画面里没有这块板"
+    corners, pattern = found
+    drawn = image.copy()
+    cv2.drawChessboardCorners(drawn, pattern, corners, True)
+    fraction = coverage_fraction(corners, drawn.shape[1], drawn.shape[0])
+    ok, out = cv2.imencode(".jpg", drawn, [int(cv2.IMWRITE_JPEG_QUALITY), 70])
+    blob = out.tobytes() if ok else jpeg
+    percent = int(round(fraction * 100))
+    return blob, f"棋盘已找到，约占画面 {percent}%。{coverage_words(fraction)}"
+
+
+class BoardWatch:
+    """后台画角点。界面线程只提交最新一帧，检测再慢也不卡住窗口。"""
+
+    def __init__(self) -> None:
+        self._lock = threading.Lock()
+        self._pending: tuple | None = None
+        self._result = {"token": 0, "jpeg": b"", "text": ""}
+        self._stop = threading.Event()
+        self._thread = threading.Thread(target=self._loop, daemon=True, name="handeye-board")
+        self._thread.start()
+
+    def submit(self, token: int, jpeg: bytes, cols: int, rows: int) -> None:
+        with self._lock:
+            if self._result["token"] == token and not self._pending:
+                return
+            self._pending = (token, jpeg, int(cols), int(rows))
+
+    def latest(self) -> tuple[bytes, str, int]:
+        with self._lock:
+            return self._result["jpeg"], self._result["text"], int(self._result["token"])
+
+    def _loop(self) -> None:
+        while not self._stop.is_set():
+            with self._lock:
+                job = self._pending
+                self._pending = None
+            if job is None:
+                time.sleep(0.05)
+                continue
+            token, jpeg, cols, rows = job
+            try:
+                blob, text = annotate_board(jpeg, cols, rows)
+            except Exception as exc:  # noqa: BLE001 —— 检测失败不能拖死预览
+                blob, text = jpeg, f"棋盘检测失败：{exc}"
+            with self._lock:
+                self._result = {"token": token, "jpeg": blob, "text": text}

install/arm_application/lib/python3.11/site-packages/arm_application/operator_teach.py

@@ -8,6 +8,8 @@
 
 from __future__ import annotations
 
+import re
+import subprocess
 import threading
 from datetime import datetime, timezone
 from pathlib import Path
@@ -17,13 +19,17 @@ import yaml
 _LOCK = threading.Lock()
 _BRIDGE = None
 _SN = ""
+_YIELDED = False
+_ARM_CONTROLLER = "rizon_arm_controller"
 
 
 def _file(arm: str) -> Path:
     from arm_application.operator_console import ROOT
+    from arm_application.site_io import listed_arms, load_site_dict
 
-    if arm not in ("89", "91"):
-        raise ValueError("臂号只接受 89 或 91")
+    known = {row["id"] for row in listed_arms(load_site_dict())}
+    if arm not in known:
+        raise ValueError(f"臂号 {arm} 不在 site.yaml 里")
     return ROOT / "handeye" / "tmp" / arm / "operator_teach.yaml"
 
 
@@ -36,9 +42,14 @@ def _bridge_for(sn: str):
         return _BRIDGE
     close()
     from arm_application.handeye_teach import RdkBridge
+    from arm_application.operator_arm_state import release_for_control, resume_after_control
 
-    _BRIDGE = RdkBridge(sn)
-    _SN = sn
+    release_for_control()
+    try:
+        _BRIDGE = RdkBridge(sn)
+        _SN = sn
+    finally:
+        resume_after_control()
     return _BRIDGE
 
 
@@ -55,19 +66,95 @@ def close() -> None:
         pass
 
 
+def _switch_arm_controller(activate: bool) -> str:
+    """停或恢复关节轨迹控制器。硬件还在，只是不再下发关节位置。
+
+    控制栈在 ``NRT_JOINT_POSITION`` 里持续 ``SendJointPosition``，
+    模式一旦被改走，写循环两秒内又切回去。软拖拽要低阻抗，必须先停这条控制器。
+    """
+    if activate:
+        payload = (
+            "{activate_controllers: ['%s'], deactivate_controllers: [], "
+            "strictness: 2, activate_asap: true, timeout: {sec: 3, nanosec: 0}}"
+            % _ARM_CONTROLLER
+        )
+    else:
+        payload = (
+            "{activate_controllers: [], deactivate_controllers: ['%s'], "
+            "strictness: 1, activate_asap: true, timeout: {sec: 3, nanosec: 0}}"
+            % _ARM_CONTROLLER
+        )
+    try:
+        proc = subprocess.run(
+            [
+                "ros2", "service", "call",
+                "/controller_manager/switch_controller",
+                "controller_manager_msgs/srv/SwitchController",
+                payload,
+            ],
+            capture_output=True,
+            text=True,
+            timeout=15,
+        )
+    except FileNotFoundError:
+        return "找不到 ros2，停不了关节轨迹控制器"
+    except subprocess.TimeoutExpired:
+        return "关节轨迹控制器没回来" if activate else "停关节轨迹控制器超时"
+    text = ((proc.stdout or "") + (proc.stderr or "")).strip()
+    if proc.returncode == 0 and re.search(r"ok\s*[:=]\s*true", text, re.IGNORECASE):
+        return ""
+    tail = text.splitlines()[-1] if text else f"退出码 {proc.returncode}"
+    verb = "恢复" if activate else "停下"
+    return f"没能{verb}关节轨迹控制器：{tail}"
+
+
+def _yield_position_stream() -> str:
+    """控制栈没起来就什么都不做。起来了就先把位置控制器让开。"""
+    global _YIELDED
+    from arm_application.operator_arm_state import stack_running
+
+    if not stack_running():
+        return ""
+    err = _switch_arm_controller(False)
+    if err:
+        return err + "。位置环还在推，软拖拽切不过去。"
+    _YIELDED = True
+    return ""
+
+
+def _restore_position_stream() -> str:
+    global _YIELDED
+    if not _YIELDED:
+        return ""
+    err = _switch_arm_controller(True)
+    _YIELDED = False
+    return err
+
+
 def drag_on(sn: str) -> dict:
     with _LOCK:
         try:
+            yielded = _yield_position_stream()
+            if yielded:
+                return {"ok": False, "message": yielded}
             bridge = _bridge_for(sn)
             result = bridge.drag_on(mode="soft")
         except Exception as exc:
-            return {"ok": False, "message": str(exc)}
-    if not result.get("ok"):
-        return {"ok": False, "message": str(result.get("error") or result)}
-    return {
-        "ok": True,
-        "message": "软拖拽已开启。轻推机械臂，再记点。这一步要先停掉控制栈。",
-    }
+            restore = _restore_position_stream()
+            message = str(exc)
+            if restore:
+                message = f"{message}；{restore}"
+            return {"ok": False, "message": message}
+        if not result.get("ok"):
+            restore = _restore_position_stream()
+            message = str(result.get("error") or result)
+            if restore:
+                message = f"{message}；{restore}"
+            return {"ok": False, "message": message}
+        note = "软拖拽已开启。轻推机械臂，再记点。"
+        if _YIELDED:
+            note += "关节轨迹控制器已停下，关掉拖拽后会回来。"
+        return {"ok": True, "message": note}
 
 
 def drag_off(sn: str) -> dict:
@@ -76,9 +163,16 @@ def drag_off(sn: str) -> dict:
             bridge = _bridge_for(sn)
             result = bridge.drag_off()
         except Exception as exc:
-            return {"ok": False, "message": str(exc)}
+            restore = _restore_position_stream()
+            message = str(exc)
+            if restore:
+                message = f"{message}；{restore}"
+            return {"ok": False, "message": message}
     if not result.get("ok"):
         return {"ok": False, "message": str(result.get("error") or result)}
+    restore = _restore_position_stream()
+    if restore:
+        return {"ok": False, "message": f"软拖拽已关闭，但{restore}"}
     return {"ok": True, "message": "软拖拽已关闭，臂保持当前位置。"}
 
 

install/arm_application/lib/python3.11/site-packages/arm_application/overhead_oneshot.py

@@ -45,6 +45,11 @@ from arm_application.path_profile import (  # 伺服和测试从本模块再导
 
 # 上一跳实际少走这么多，就认定冻结 J 在当前姿态走不动，不许按同一剩差再刷。
 D435_MIN_PROGRESS_FRAC = 0.40
+# SwitchMode 返回不等于已经进 primitive。连试仍失败必须让一阶段立刻中止，
+# 不许空等 150s 装成"还在粗定位"（2026-10-16 17:27）。
+D435_MOVEL_RETRY = 3
+D435_MOVEL_RETRY_S = 0.3
+D435_MOVEL_GAVE_UP = "厂家 MoveL 连试 3 次没走成"
 
 
 def best_det_conf(dets: Optional[Iterable]) -> Optional[float]:
@@ -154,16 +159,27 @@ def d405_commands_arm(
 ) -> bool:
     """D405 这一帧的点能不能下发给左臂。
 
-    推理一直开着。正在用 D435 那一个点时，进程内 MoveL 结束并且停稳之前不下发。
-    停稳用 ``d435_handback_sec`` 和 ``lock_still_move_m``。赶路时 XY 已经进 15 mm
-    也不算到位。不许在 MoveL 期间下发 D405（两套控制互抢会晃）。
-    ``move_only``：只测 D435 位移，D405 点一律不下发（到位之后也不交）。
+    推理从启动就开着。没有高置信度目标、或 D435 厂家 MoveL 还没结束、
+    法兰还没连续停稳时，这帧**不下发**（输出截断，不许拿去控臂）。
+    厂家移动结束并且法兰停稳（``settled``，时长是 ``d435_handback_sec``）
+    之后才放行，伺服用它追工件框的矩形中心。框心对准停稳后才进精拍下臂。
+    ``move_only``：只测 D435 位移，D405 点一律不下发。
     """
     if move_only:
         return False
-    if not using_d435:
-        return True
-    return bool(arrived) and bool(settled)
+    if using_d435:
+        return bool(arrived) and bool(settled)
+    return True
+
+
+def retract_goal_z(z_now: float, z_target: float) -> float:
+    """收臂目标 Z：已经高于目标就钉在当前高度，不许为了追 5 mm 往下补。
+
+    2026-10-16：贴面 MoveL 之后 JTC 还握着悬停关节，收臂 hop1 计划 +5 mm
+    实际抬了 18–25 mm；hop2 把「抬过头」当成误差 ``Δz=−13 mm`` 往回压，
+    看起来就是进孔再弹出。抬过了 = 成功。
+    """
+    return max(float(z_now), float(z_target))
 
 
 def oneshot_needs_verticalize(

install/arm_application/lib/python3.11/site-packages/arm_application/perception_node.py

@@ -105,7 +105,7 @@ class PerceptionNode(TolerantNode):
         self.declare_parameter("image_topic", "")
         self.declare_parameter("camera_info_topic", "")
         self.declare_parameter("depth_topic", "/d405/aligned_depth_to_color/image_raw")
-        self.declare_parameter("weights", "models/yolo/gongjian0929.onnx")
+        self.declare_parameter("weights", "/home/cc/ros2/models/yolo/gongjian0929.onnx")
         self.declare_parameter("conf", 0.10)
         self.declare_parameter("iou", 0.45)
         self.declare_parameter("imgsz", 640)
@@ -336,9 +336,7 @@ class PerceptionNode(TolerantNode):
             r_pos=float(self.get_parameter("kf_r_pos").value),
         )
 
-        from arm_application.site_io import resolve_repo_path
-
-        weights_raw = resolve_repo_path(str(self.get_parameter("weights").value))
+        weights_raw = str(self.get_parameter("weights").value)
         weights = str(resolve_weights(weights_raw, self.backend_name))
         yolo_cfg = {
             "weights": weights,
@@ -1141,8 +1139,8 @@ class PerceptionNode(TolerantNode):
             filt[2] = z_use
             self._last_filt = filt.copy()
 
-        if handoff:
-            # 画面里没有 conf≥门槛的目标：绿框可以留，三维点不许发 —— 伺服会拿去追。
+        if handoff or self._d435_commanding:
+            # 没有高置信度，或 D435 厂家移动还没交接：推理留着，控制点不许发。
             return
         self._publish_point(filt, stamp)
         self.get_logger().info(
@@ -1427,15 +1425,11 @@ class PerceptionNode(TolerantNode):
         self._z_locked = bool(msg.data)
 
     def _on_d435_commanding(self, msg: Bool) -> None:
-        """伺服还在执行 D435 那一个点时，不再提交下一张。
+        """伺服还在执行 D435 那一个点时，不再提交下一张，D405 控制点也不发。
 
-        这一段结束（到位并停稳）后清掉「已拍过」，漏检重新计 0.5 s。
+        这一轮已经拍过并走完。停稳后打开 D405 框心，不因交接瞬间漏检再 MoveL。
         """
-        commanding = bool(msg.data)
-        if self._d435_commanding and not commanding:
-            self._oneshot_shot = False
-            self._low_since = None
-        self._d435_commanding = commanding
+        self._d435_commanding = bool(msg.data)
 
     def _publish_overhead_k0(self, p_base: np.ndarray, is_k0: bool) -> None:
         pt = PointStamped()

install/arm_application/lib/python3.11/site-packages/arm_application/pipeline_node.py

@@ -15,6 +15,7 @@ import os
 import signal
 import subprocess
 import time
+from pathlib import Path
 from typing import Optional
 
 import numpy as np
@@ -23,6 +24,7 @@ from geometry_msgs.msg import PointStamped
 from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy
 from std_msgs.msg import Bool, String
 
+from arm_application.overhead_oneshot import D435_MOVEL_GAVE_UP
 from arm_application.stage3_search_node import Stage3Search
 
 
@@ -70,6 +72,19 @@ def init_z_refusal(
     return ""
 
 
+def hold_at_insert_z(final, *, skip3: bool) -> bool:
+    """贴面已经按 ``final_gap_mm`` 停下时，不再进三阶段。
+
+    三阶段开局会抬到悬高上方测触觉，再落回 ``hole_hover_z_m``。
+    没有接触传感器。到位高度就是这次的贴面间隙，不另设一个 Z。
+    """
+    if skip3:
+        return True
+    if not isinstance(final, dict):
+        return False
+    return bool(final.get("insert_held"))
+
+
 def track_lock_z(hover_z: float, tool_len_m: float, surface_z_m: float) -> float:
     """一阶段锁高。
 
@@ -85,6 +100,20 @@ def track_lock_z(hover_z: float, tool_len_m: float, surface_z_m: float) -> float
     return hover + TRACK_CLEARANCE_M
 
 
+def stage1_log_fatal(log_path: str) -> str:
+    """一阶段日志里已经写明失败时，不许再空等锁定。空串 = 还没到这一步。"""
+    try:
+        text = Path(log_path).read_text(encoding="utf-8", errors="replace")
+    except OSError:
+        return ""
+    if D435_MOVEL_GAVE_UP in text:
+        return (
+            f"D435 {D435_MOVEL_GAVE_UP}（RDK 控制模式没切到 primitive），"
+            f"一阶段中止。不是视野/标定。看 {log_path}"
+        )
+    return ""
+
+
 def stage1_timeout_reason(points_seen: int, timeout_s: float, log_path: str) -> str:
     """一阶段超时了，到底是"感知没给点"还是"给了点但没收敛"。
 
@@ -99,6 +128,9 @@ def stage1_timeout_reason(points_seen: int, timeout_s: float, log_path: str) ->
     以前两种情况共用一句"没锁定（视野里没工件？）"，于是"perception_node
     因为缺依赖早退了"被读成"算法不收敛"，白查半天。
     """
+    fatal = stage1_log_fatal(log_path)
+    if fatal:
+        return fatal
     if points_seen <= 0:
         return (
             f"一阶段 {timeout_s:.0f}s 没锁定：**一个视觉目标点都没收到**"
@@ -308,6 +340,11 @@ class AssemblyPipeline(Stage3Search):
             self._spin(0.1)
             if self._locked:
                 break
+            fatal = stage1_log_fatal(log_path)
+            if fatal:
+                self._stop_stage1()
+                self.get_logger().error(fatal)
+                return 1
             if self._s1.poll() is not None:
                 self.get_logger().error(f"一阶段进程提前退出，看 {log_path}")
                 return 1
@@ -363,24 +400,42 @@ class AssemblyPipeline(Stage3Search):
                 if code != 0:
                     self.get_logger().error(f"二阶段失败 exit={code}，停在当前位姿")
                     return code
-                self.get_logger().info(
-                    f"二阶段 OK 残差 {final.get('residual_mm', -1)}mm"
-                    + (
-                        " — 跳过搜孔，停在观测高度（看规尖是否对中；进程退出后臂保持）"
-                        if skip3
-                        else " — 立刻下压搜孔并轻压（不拧入）"
+                stay = hold_at_insert_z(final, skip3=skip3)
+                if stay and final.get("insert_held"):
+                    self.get_logger().info(
+                        f"二阶段 OK 残差 {final.get('residual_mm', -1)}mm — "
+                        f"插孔停在法兰 Z={final.get('flange_z_m')}m，"
+                        f"规尖离面 {final.get('tip_gap_mm')}mm"
+                        f"（目标 {final.get('commanded_gap_mm')}mm，来自 final_gap_mm）。"
+                        "没有接触传感器，不另设 Z，不抬回悬高。"
+                    )
+                else:
+                    self.get_logger().info(
+                        f"二阶段 OK 残差 {final.get('residual_mm', -1)}mm"
+                        + (
+                            " — 跳过搜孔，停在观测高度（看规尖是否对中；进程退出后臂保持）"
+                            if skip3
+                            else " — 立刻下压搜孔并轻压（不拧入）"
+                        )
                     )
-                )
             else:
                 self._state("skip_stage2")
+                stay = hold_at_insert_z(final, skip3=skip3)
 
-            if skip3:
+            if stay:
                 self._state("skip_stage3")
                 if self.d435_move_only:
                     self.get_logger().info(
                         "D435 位移测试结束 —— 臂停在当前高度，不交 D405、不对孔；"
                         "15s 后本进程退出，臂保持不动"
                     )
+                elif isinstance(final, dict) and final.get("insert_held"):
+                    if q is not None:
+                        self._pin(q)
+                    self.get_logger().info(
+                        "插孔已停在贴面高度，不进搜孔、不抬回悬高。"
+                        "15s 后本进程退出，臂保持这个 Z。"
+                    )
                 else:
                     self.get_logger().info(
                         "跳过搜孔 —— 臂停在当前高度。请看规尖与孔是否对中；"

install/arm_application/lib/python3.11/site-packages/arm_application/realsense_color_pub.py

@@ -176,6 +176,10 @@ class RealsenseColorPub(TolerantNode):
         self._overlay: Optional[list] = None
         self._overlay_stamp = 0.0
         self._capture_started = False
+        self._cap_lock = threading.Lock()
+        self._cap_event = threading.Event()
+        self._pending = None
+        self._last_frame_mono = 0.0
         self._usb_desc = ''
         self._last_ok = None
         self._site_yaml = _site_yaml_path()
@@ -309,23 +313,26 @@ class RealsenseColorPub(TolerantNode):
         self._info_pub.publish(info)
 
         if depth_u16 is not None:
-            dh, dw = int(depth_u16.shape[0]), int(depth_u16.shape[1])
-            dmsg = Image()
-            dmsg.header.stamp = stamp
-            dmsg.header.frame_id = self._frame_id
-            dmsg.height = dh
-            dmsg.width = dw
-            dmsg.encoding = '16UC1'
-            dmsg.is_bigendian = 0
-            dmsg.step = dw * 2
-            dmsg.data = array.array('B', np.ascontiguousarray(depth_u16).tobytes())
-            self._depth_pub.publish(dmsg)
-            if not self._logged_depth:
-                self._logged_depth = True
-                self.get_logger().info(
-                    f'首个对齐深度 {dw}x{dh} → '
-                    f'/{self._name}/aligned_depth_to_color/image_raw'
-                )
+            self._publish_depth(depth_u16, stamp)
+
+    def _publish_depth(self, depth_u16: np.ndarray, stamp=None) -> None:
+        dh, dw = int(depth_u16.shape[0]), int(depth_u16.shape[1])
+        dmsg = Image()
+        dmsg.header.stamp = stamp or self.get_clock().now().to_msg()
+        dmsg.header.frame_id = self._frame_id
+        dmsg.height = dh
+        dmsg.width = dw
+        dmsg.encoding = '16UC1'
+        dmsg.is_bigendian = 0
+        dmsg.step = dw * 2
+        dmsg.data = array.array('B', np.ascontiguousarray(depth_u16).tobytes())
+        self._depth_pub.publish(dmsg)
+        if not self._logged_depth:
+            self._logged_depth = True
+            self.get_logger().info(
+                f'首个对齐深度 {dw}x{dh} → '
+                f'/{self._name}/aligned_depth_to_color/image_raw'
+            )
 
     def _start_pipeline(self, reconnect: bool = False) -> None:
         try:
@@ -457,7 +464,9 @@ class RealsenseColorPub(TolerantNode):
                         trial.enable_stream(
                             rs.stream.infrared, 2, w, h, rs.format.y8, fps
                         )
-                    profile = pipe.start(trial)
+                    # 回调里只留最新一帧。wait_for_frames 会等彩色、深度、红外齐套，
+                    # 场景一动红外对不齐，彩色帧就在这里被丢掉。
+                    profile = pipe.start(trial, self._on_sdk_frame)
                     have_depth = with_depth
                     have_infra = infra_mode != 'none'
                     self._infra_mode = infra_mode
@@ -508,6 +517,13 @@ class RealsenseColorPub(TolerantNode):
 
         self._pipe = pipe
         self._profile = profile
+        try:
+            sensor = profile.get_device().first_color_sensor()
+            if sensor.supports(rs.option.frames_queue_size):
+                # 队列只留最新一张，避免越积越慢，动起来画面还停在旧帧。
+                sensor.set_option(rs.option.frames_queue_size, 1)
+        except Exception:  # noqa: BLE001
+            pass
         self._have_depth = have_depth
         self._have_infra = have_infra
         self._logged_depth = False
@@ -609,7 +625,7 @@ class RealsenseColorPub(TolerantNode):
 
         if not getattr(self, '_capture_started', False):
             self._capture_started = True
-            self._thread = threading.Thread(target=self._capture_loop, daemon=True)
+            self._thread = threading.Thread(target=self._publish_loop, daemon=True, name="rs-publish")
             self._thread.start()
 
     def _enable_color_ae_fast(self, profile, rs) -> None:
@@ -702,18 +718,60 @@ class RealsenseColorPub(TolerantNode):
             )
             return False
 
-    def _capture_loop(self) -> None:
+    def _on_sdk_frame(self, frame) -> None:
+        """SDK 回调。只拷走彩色并握住这一组帧，立刻返回。
+
+        对齐、深度换算和 ROS 发布若做在这个回调里，下一帧来不及取，
+        动起来时彩色帧号会跳。那些工作放到 ``_publish_loop``。
+        """
+        try:
+            frames = frame.as_frameset()
+            color = frames.get_color_frame()
+            if not color:
+                return
+            bgr = np.array(np.asanyarray(color.get_data()), copy=True)
+            frames.keep()
+            with self._cap_lock:
+                self._pending = (bgr, frames)
+            self._last_frame_mono = time.monotonic()
+            self._cap_event.set()
+        except Exception as exc:  # noqa: BLE001
+            try:
+                self.get_logger().warn(f'收帧：{exc}', throttle_duration_sec=2.0)
+            except Exception:  # noqa: BLE001
+                pass
+
+    def _publish_loop(self) -> None:
         got = False
-        misses = 0
         while self._running and rclpy.ok():
             if self._pipe is None:
                 if not self._reconnect():
                     time.sleep(2.0)
                 continue
+            if not self._cap_event.wait(0.2):
+                silent = self._last_frame_mono and time.monotonic() - self._last_frame_mono > 2.0
+                if silent:
+                    usb = getattr(self, '_usb_desc', '?')
+                    self.get_logger().error(
+                        f'相机超过 2 秒没有新帧（USB={usb}）。正在重连 —— '
+                        '请给 USB 线缆留点余量，别让机械臂把它拽掉。'
+                    )
+                    self._last_frame_mono = 0.0
+                    got = False
+                    self._reconnect()
+                continue
+            self._cap_event.clear()
+            with self._cap_lock:
+                item = self._pending
+                self._pending = None
+            if item is None:
+                continue
+            bgr, frames = item
             try:
-                t_wait0 = time.monotonic()
-                frames = self._pipe.wait_for_frames(timeout_ms=2000)
-                self._t_wait = time.monotonic() - t_wait0
+                t_pub = time.monotonic()
+                # 彩色先发出去。深度对齐再慢，也不能把这一帧彩色拖掉。
+                self._publish_frame(bgr, None)
+                self._t_pub = time.monotonic() - t_pub
                 ir_l = (
                     frames.get_infrared_frame(1)
                     if self._have_infra and self._infra_mode in ('both', 'left')
@@ -724,44 +782,45 @@ class RealsenseColorPub(TolerantNode):
                     if self._have_infra and self._infra_mode in ('both', 'right')
                     else None
                 )
+                aligned = frames
                 if self._align is not None:
                     t_al = time.monotonic()
-                    frames = self._align.process(frames)
+                    aligned = self._align.process(frames)
                     self._t_align = time.monotonic() - t_al
+                depth = aligned.get_depth_frame()
                 color = frames.get_color_frame()
             except Exception as exc:  # noqa: BLE001
-                misses += 1
                 text = str(exc)
                 dropped = (
                     'disconnect' in text.lower()
                     or 'ioctl' in text.lower()
                     or 'errno=5' in text.lower()
                 )
-                if dropped or misses >= 4:
+                if dropped:
                     usb = getattr(self, '_usb_desc', '?')
                     self.get_logger().error(
                         f'相机数据流中断（USB={usb}）：{text[:120]}。'
                         '正在重连 —— 请给 USB 线缆留点余量，别让机械臂把它拽掉。'
                     )
                     got = False
-                    misses = 0
+                    self._last_frame_mono = 0.0
                     self._reconnect()
-                    continue
-                if misses == 1:
-                    self.get_logger().warn(f'wait_for_frames：{exc}')
+                else:
+                    self.get_logger().warn(f'整理帧：{exc}', throttle_duration_sec=2.0)
                 continue
-            if not color:
-                misses += 1
-                continue
-            misses = 0
-            bgr = np.asanyarray(color.get_data())
-            depth = frames.get_depth_frame()
             depth_u16 = np.asanyarray(depth.get_data()) if depth else None
             if depth_u16 is not None and self._depth_to_mm != 1.0:
                 # 原始设备单位 → 真实 mm（D405 原始为 0.1 mm/单位）
                 depth_u16 = np.clip(
                     depth_u16.astype(np.float32) * self._depth_to_mm, 0, 65535
                 ).astype(np.uint16)
+            if depth_u16 is not None:
+                try:
+                    self._publish_depth(depth_u16)
+                except Exception as exc:  # noqa: BLE001
+                    if not self._running:
+                        break
+                    self.get_logger().warn(f'发布深度失败：{exc}', throttle_duration_sec=2.0)
             self._frame_i += 1
             self._fps_meter(color, depth, depth_u16)
             # 预览相关的工作（CPU 上色、IR 拷贝、四窗格绘制）每 N 帧才做一次
@@ -794,7 +853,6 @@ class RealsenseColorPub(TolerantNode):
                     )
             try:
                 t_pub = time.monotonic()
-                self._publish_frame(bgr, depth_u16)
                 if self._ir1_pub is not None and ir_l is not None:
                     self._publish_ir(ir_l, self._ir1_pub)
                 if self._ir2_pub is not None and ir_r is not None:

install/arm_application/lib/python3.11/site-packages/arm_application/servo_node.py

@@ -50,6 +50,9 @@ from arm_application.geom_ik import (
 )
 from arm_application.handeye_lib import cam_point_to_base, cam_to_base_T
 from arm_application.overhead_oneshot import (
+    D435_MOVEL_GAVE_UP,
+    D435_MOVEL_RETRY,
+    D435_MOVEL_RETRY_S,
     D435_NEAR_CRUISE_MPS,
     D435_SETTLE_S,
     D435_VENDOR_MOVEL_VEL_MPS,
@@ -126,7 +129,7 @@ class ServoNode(TolerantNode):
         self.declare_parameter("point_topic", "visual_servo/filtered_point")
         # 默认值只作兜底：真机运行一定由 launch 传入 site.yaml 的 paths.handeye_file。
         # 指向 /tmp 是历史遗留（标定临时产物），会让人误以为"标定结果在那里"。
-        self.declare_parameter("handeye_file", "handeye/results/89_eye_in_hand.yaml")
+        self.declare_parameter("handeye_file", "/home/cc/ros2/handeye/results/89_eye_in_hand.yaml")
         # eye_in_hand = cam→法兰；eye_to_hand = cam→基座。写错会把固定相机当腕部用。
         self.declare_parameter("handeye_mode", "eye_in_hand")
         self.declare_parameter("use_static_handeye_tf", False)
@@ -224,8 +227,7 @@ class ServoNode(TolerantNode):
         self.declare_parameter("lock_hold_sec", 2.0)
         # D405 跟住后法兰静止多久进二阶段。哨兵 -1 = 没从 site 接入。
         # lock_still_sec 仍须从 site 接上（缺了就报错）。
-        # 交回 D405：进程内 MoveL 到位后，再叠 d435_handback_sec（法兰 XY 停稳）。
-        # 旧杀栈冷启动曾把这 3s 垫在控制器重启后面；不杀栈后只留机械停稳。
+        # 厂家 MoveL 结束后，法兰连续停稳 d435_handback_sec 才打开 D405 框心。
         self.declare_parameter("lock_still_sec", -1.0)
         self.declare_parameter("d435_handback_sec", -1.0)
         self.declare_parameter("wrist_align_sec", -1.0)
@@ -329,7 +331,7 @@ class ServoNode(TolerantNode):
             and self.wrist_handoff_conf >= 0.0
             and self.overhead_hold_s >= 0.0
         )
-        # 框心对准进对孔用 wrist_align_sec。D435 交回只用 d435_handback_sec，两段不要合成一个数。
+        # 框心对准进精拍用 wrist_align_sec。D435 交接只用 d435_handback_sec，两段不要合成一个数。
         self._still = StillLock(
             still_sec=max(0.0, self.wrist_align_sec),
             still_move_m=max(0.0, self.lock_still_move_m),
@@ -342,6 +344,8 @@ class ServoNode(TolerantNode):
         self._d435_vendor_done = False
         self._d435_vendor_busy = False
         self._d435_vendor_failed = False
+        self._d435_vendor_tries = 0
+        self._d435_retry_after = 0.0
         self._d435_settled = False
         self._d435_exec_until = 0.0
         self._d435_pinned = False
@@ -374,10 +378,9 @@ class ServoNode(TolerantNode):
         self.ws_min = np.array([float(v) for v in ws_min[:3]], dtype=float)
         self.ws_max = np.array([float(v) for v in ws_max[:3]], dtype=float)
 
-        from arm_application.site_io import resolve_repo_path
-
-        handeye_path = resolve_repo_path(str(self.get_parameter("handeye_file").value))
-        self.T_cam_to_gripper = load_handeye_T(handeye_path)
+        self.T_cam_to_gripper = load_handeye_T(
+            str(self.get_parameter("handeye_file").value)
+        )
         self.handeye_mode = str(
             self.get_parameter("handeye_mode").value or "eye_in_hand"
         ).strip().lower()
@@ -400,7 +403,7 @@ class ServoNode(TolerantNode):
         t_he = np.asarray(self.T_cam_to_gripper[:3, 3], dtype=float)
         axis_z = float(np.clip(R_he[2, 2], -1.0, 1.0))
         self._cam_tilt_deg = float(np.degrees(np.arccos(axis_z)))
-        he_path = handeye_path
+        he_path = str(self.get_parameter("handeye_file").value)
         if self.handeye_mode == "eye_to_hand":
             self.get_logger().warn(
                 f"HANDEYE yaml={he_path} mode=eye_to_hand "
@@ -709,6 +712,8 @@ class ServoNode(TolerantNode):
             self._d435_vendor_done = False
             self._d435_vendor_busy = False
             self._d435_vendor_failed = False
+            self._d435_vendor_tries = 0
+            self._d435_retry_after = 0.0
             self._overhead_t = now_m
             self._last_seen_time = now_m
             self._stamp_sec = now_sec
@@ -732,6 +737,8 @@ class ServoNode(TolerantNode):
         self._d435_vendor_done = False
         self._d435_vendor_busy = False
         self._d435_vendor_failed = False
+        self._d435_vendor_tries = 0
+        self._d435_retry_after = 0.0
         self._publish_d435_commanding(True)
         self.get_logger().info(
             f"D435 一次性设定 feat_base=({p_base[0]:.3f},{p_base[1]:.3f},{p_base[2]:.3f}) "
@@ -759,9 +766,9 @@ class ServoNode(TolerantNode):
             settled=settled,
             move_only=self.d435_move_only,
         ):
-            # 推理服务不关。这一帧的点不下发给臂，目标仍是 D435 那一个点。
+            # 推理不关。没到「法兰停稳」之前，这帧不能进运动控制。
             self.get_logger().info(
-                "D435 位置还没到并且停稳，D405 这帧不下发",
+                "D405 输出已截断：D435 厂家移动还没结束或法兰还没停稳",
                 throttle_duration_sec=1.0,
             )
             return
@@ -1407,9 +1414,12 @@ class ServoNode(TolerantNode):
         return geometric_jacobian(link_R, link_p, p_ee, RIZON_LINK_AXES)
 
     def _d435_oneshot_holds_the_arm(self) -> bool:
-        """D435 到点平移：action 短跳，等结果再重测。D405 主跟踪仍走原来的伺服。"""
-        if self.d435_move_only and self._hybrid and self._track_source == "overhead" and self._d435_settled:
-            # 位移测完了：守着，不许掉进 D405 的 _execute_twist。hold 只在到位那一拍发一次。
+        """D435 粗定位只走厂家 MoveL。停稳后先钉住，等 D405 框心接上再放开。
+
+        这段不许再用流式伺服去追 D435 那个点。D405 点被 ``_on_point`` 接上后
+        跟踪源变成腕部，这里返回 False，框心才开始带臂。
+        """
+        if self._hybrid and self._track_source == "overhead" and self._d435_settled:
             return True
         if not (self._hybrid and self._track_source == "overhead" and not self._d435_settled):
             return False
@@ -1519,7 +1529,7 @@ class ServoNode(TolerantNode):
         """停或重新打开关节轨迹控制器。关节状态广播留着，硬件接口不下线。
 
         用节点里的客户端，不走 ``ros2 service call``：那条命令自己就要一两秒，
-        交回 D405 的预算会被它吃掉。
+        停稳锁定的预算会被它吃掉。
         """
         from controller_manager_msgs.srv import SwitchController
 
@@ -1659,18 +1669,32 @@ class ServoNode(TolerantNode):
         if self._d435_vendor_failed or self._d435_vendor_busy:
             return True
         if not self._d435_vendor_done:
-            # busy 先于调用置位：另一条控制回调不能再开一次 MoveL，也不能提前交回 D405。
+            if time.monotonic() < float(self._d435_retry_after):
+                return True
+            if int(self._d435_vendor_tries) >= int(D435_MOVEL_RETRY):
+                self._d435_vendor_failed = True
+                self.get_logger().error(
+                    f"{D435_MOVEL_GAVE_UP} —— 一阶段中止，D405 也不带臂"
+                )
+                return True
+            # busy 先于调用置位：另一条控制回调不能再开一次 MoveL，也不能提前锁定。
             self._d435_vendor_busy = True
             ok = self._d435_vendor_movel()
             self._d435_vendor_busy = False
             if not ok:
-                self._d435_vendor_failed = True
+                self._d435_vendor_tries = int(self._d435_vendor_tries) + 1
+                self._d435_retry_after = time.monotonic() + float(D435_MOVEL_RETRY_S)
                 self.get_logger().error(
-                    "厂家 MoveL 没走成 —— 停在原地，D405 的点继续不下发"
+                    f"厂家 MoveL 第{self._d435_vendor_tries}次没走成，"
+                    f"{D435_MOVEL_RETRY_S:.1f}s 后再试"
                 )
             else:
                 self._d435_vendor_done = True
                 self._arrive.reset()
+                # MoveL 切回关节后 JTC 还握着出发前的悬停。不钉住的话
+                # 硬件 3 s 窗口一过，臂会按悬停关节走回去（比交回 D405 的 10 cm 更远）。
+                self._send_hold()
+                self._d435_pinned = True
             return True
         return True
 
@@ -1729,7 +1753,7 @@ class ServoNode(TolerantNode):
         ):
             self._d435_pin_and_stop_hops(
                 f"D435 上一跳几乎没走近，剩差还是 {dist*1000:.0f}mm "
-                f"—— 钉住不再补刀，等停稳后交回 D405"
+                f"—— 钉住不再补刀，等停稳后锁定进二阶段"
             )
             return True
         if not at:
@@ -1836,7 +1860,7 @@ class ServoNode(TolerantNode):
         return True
 
     def _tick_d435_arrive(self) -> None:
-        """走到 D435 那个点，并且法兰按一阶段的静止判据停稳，才允许下发 D405。"""
+        """厂家 MoveL 已结束：看法兰还在不在动。连续停稳后才打开 D405 框心。"""
         if not self._hybrid:
             return
         if (
@@ -1858,11 +1882,15 @@ class ServoNode(TolerantNode):
             return
         xy = np.asarray(T[:2, 3], dtype=float)
         dist = float(np.hypot(float(xy[0]) - goal[0], float(xy[1]) - goal[1]))
-        arrived = dist <= float(self.lock_still_move_m) or self._d435_give_up
-        settled = self._arrive.update(
-            xy, time.monotonic(), tracking_ok=arrived
-        )
+        # 到位看法兰是否还在动，不看离视觉点还差几毫米。
+        settled = self._arrive.update(xy, time.monotonic(), tracking_ok=True)
         if not settled:
+            self.get_logger().info(
+                f"D435 厂家 MoveL 已结束，法兰还在动或未满 "
+                f"{self.d435_handback_sec:.1f}s（离点 {dist*1000:.0f}mm），"
+                "D405 输出仍截断",
+                throttle_duration_sec=1.0,
+            )
             return
         with self._lock:
             self._d435_settled = True
@@ -1885,10 +1913,12 @@ class ServoNode(TolerantNode):
                 self._lock_pub_t = now_m
                 self._lock_pub.publish(Bool(data=True))
             return
+        # 法兰已停稳：打开 D405。不在这里锁定；框心对准停稳后才精拍下臂。
         self._publish_d435_commanding(False)
         self.get_logger().info(
-            f"已到 D435 位置（还差 {dist*1000:.0f}mm）并停稳 "
-            f"{self.d435_handback_sec:.1f}s —— 改回下发 D405 框心"
+            f"法兰已连续停稳 {self.d435_handback_sec:.1f}s"
+            f"（离 D435 点 {dist*1000:.0f}mm）"
+            " —— 打开 D405 输出，追工件框矩形中心；对准后才精拍下臂"
         )
 
     def _tick_still_lock(self) -> None:

install/arm_application/lib/python3.11/site-packages/arm_application/site_io.py

@@ -10,83 +10,13 @@ import yaml
 
 
 def ros2_root() -> Path:
-    """仓库根目录。先看环境变量，再从本文件向上找 ``config/site.yaml``。
-
-    不能默认 ``~/ros2``：别人把仓库克隆到别的目录时，写死的家目录路径是空的，
-    启动会报文件不存在。
-    """
     for key in ("SITE_ROS2_ROOT", "ROS2_WS"):
         v = os.environ.get(key, "").strip()
         if v:
             return Path(v).expanduser()
-    here = Path(__file__).resolve()
-    for parent in here.parents:
-        if (parent / "config" / "site.yaml").is_file() and (parent / "scripts" / "env.sh").is_file():
-            return parent
     return Path.home() / "ros2"
 
 
-_PATH_KEYS = frozenset({
-    "named_poses_file",
-    "handeye_file",
-    "handeye_waypoints",
-    "handeye_workdir",
-    "handeye_calibrate",
-    "yolo_weights",
-    "hole_weights",
-    "weights",
-    "workpiece_weights",
-    "config_file",
-    "append_poses_file",
-})
-
-
-def resolve_repo_path(value: str, root: Path | None = None) -> str:
-    """仓库内的相对路径展开成绝对路径。已经是绝对路径的原样保留。"""
-    text = str(value or "").strip()
-    if not text:
-        return ""
-    path = Path(text).expanduser()
-    if path.is_absolute():
-        return str(path)
-    base = root or ros2_root()
-    return str((base / path).resolve())
-
-
-def _is_path_key(key: str) -> bool:
-    return (
-        key in _PATH_KEYS
-        or key.startswith("handeye_file")
-        or key.startswith("handeye_waypoints")
-        or key.endswith("_weights")
-        or key.endswith("_file")
-        or key.endswith("_workdir")
-    )
-
-
-def absolutize_site_paths(data: Any, root: Path | None = None) -> Any:
-    """把 site 里的仓库相对路径展开。只动路径字段，不动别的字符串。"""
-    base = root or ros2_root()
-    if isinstance(data, dict):
-        out: dict[Any, Any] = {}
-        for key, val in data.items():
-            name = str(key)
-            if isinstance(val, str) and _is_path_key(name):
-                out[key] = resolve_repo_path(val, base)
-            elif (
-                name == "config"
-                and isinstance(val, str)
-                and (val.endswith((".yaml", ".yml", ".json")) or "/" in val)
-            ):
-                out[key] = resolve_repo_path(val, base)
-            else:
-                out[key] = absolutize_site_paths(val, base)
-        return out
-    if isinstance(data, list):
-        return [absolutize_site_paths(item, base) for item in data]
-    return data
-
-
 def site_path() -> Path:
     env = os.environ.get("SITE_CONFIG", "").strip()
     if env:
@@ -99,11 +29,7 @@ def load_site_dict(path: str | Path | None = None) -> dict[str, Any]:
     if not p.is_file():
         return {}
     data = yaml.safe_load(p.read_text(encoding="utf-8")) or {}
-    if not isinstance(data, dict):
-        return {}
-    resolved = p.resolve()
-    root = resolved.parent.parent if resolved.parent.name == "config" else ros2_root()
-    return absolutize_site_paths(data, root)
+    return data if isinstance(data, dict) else {}
 
 
 def deep_merge(base: dict[str, Any], overlay: dict[str, Any]) -> dict[str, Any]:

install/arm_application/lib/python3.11/site-packages/arm_application/stage2_locate_node.py

@@ -69,6 +69,7 @@ from arm_application.overhead_oneshot import (
     once_ff_tcp_xyz,
     oneshot_align_step_m,
     pixel_on_base_plane,
+    retract_goal_z,
 )
 from arm_application.handeye_realsense import resolve_realsense_topics
 from arm_application.gauge_boot import perception_source_of
@@ -249,7 +250,7 @@ class Stage2Locate(TolerantNode):
         # _coarse_target_reject / _bbox_on_border 的注释）：粗居中的目的是把臂
         # 挪到工件正上方，**挪错比不挪危险得多** —— 一挪错精定位就再也看不到
         # k1/k2，而流程会把它当"孔检测失败"，现场根本想不到是粗居中把臂带走的。
-        self.coarse_max_offset_m = float(stage2.get("coarse_max_offset_m") or 0.12)
+        self.coarse_max_offset_m = float(stage2.get("coarse_max_offset_m") or 0.08)
         self.coarse_border_px = int(stage2.get("coarse_border_px") or 2)
         # 最后轻降到孔面上方 final_gap_mm（不接触）。启动参数 ≥0 覆盖 site。
         self._final_gap_refused = ""
@@ -901,10 +902,13 @@ class Stage2Locate(TolerantNode):
         min_dur: float = 1.6,
         hold_xy: bool = False,
         strict_xy: bool = False,
+        z_up_only: bool = False,
     ) -> Tuple[bool, np.ndarray, np.ndarray]:
         """闭环点到点：规划短 hop → 执行 → 重新看 TF → 重复，直到容差内。
 
         hold_xy=True：只收 Z/姿态（悬停降锁高），不把下降中漂出的 XY 再拽回去。
+        z_up_only=True：当前已经高于目标 Z 就停，不许为了追 5 mm 往下补
+        （贴面 MoveL 后收臂 hop1 抬过头、hop2 回压进孔，2026-10-16）。
         """
         target = self._clamp_ws(target)
         last_dist: Optional[float] = None
@@ -912,7 +916,14 @@ class Stage2Locate(TolerantNode):
         stall = 0
         for hop in range(max_hops):
             p = Tee[:3, 3]
-            goal = np.array([p[0], p[1], target[2]]) if hold_xy else target
+            z_tgt = float(target[2])
+            if z_up_only:
+                if float(p[2]) >= z_tgt - float(tol_m):
+                    return True, q, Tee
+                z_tgt = retract_goal_z(float(p[2]), z_tgt)
+            goal = np.array([p[0], p[1], z_tgt]) if hold_xy else np.array(
+                [target[0], target[1], z_tgt], dtype=float
+            )
             if hold_xy:
                 goal = self._clamp_ws(goal)
             dp = goal - p
@@ -1491,14 +1502,15 @@ class Stage2Locate(TolerantNode):
 
         1. 目标点落在 ``workspace_xyz_*`` 的 XY 之外（含 5mm 余量）—— 一阶段刚锁
            定时臂就在工件上方，工件**不该**在盒外；
-        2. 目标点离当前夹爪轴线超过 ``coarse_max_offset_m``（缺省 0.12m）—— 粗居中
-           是"把臂挪到工件正上方"，合理位移是厘米级；十几厘米只可能是坏读数。
+        2. 目标点离当前夹爪轴线超过 ``coarse_max_offset_m``（缺省 0.08m）—— 粗居中
+           是"把臂挪到工件正上方"，合理位移是厘米级；十几厘米只可能是坏读数
+           （2026-10-16 三次 110mm 回撤，0.12 放行了）。
 
         注意：判误检后**原地不动**，由精定位（直接量 k1/k2）继续。不动的代价只是
         "少挪一次"，而挪错的代价是精定位彻底看不到孔 —— 所以这里必须选不动。
         """
         if max_offset_m is None:
-            max_offset_m = float(getattr(self, "coarse_max_offset_m", 0.12))
+            max_offset_m = float(getattr(self, "coarse_max_offset_m", 0.08))
         p = np.asarray(p_base, dtype=float).reshape(3)
         if not np.all(np.isfinite(p[:2])):
             return "目标点非有限值（深度/三角化失败）"
@@ -2241,7 +2253,9 @@ class Stage2Locate(TolerantNode):
         )
         fl0 = (float(Tee[0, 3]), float(Tee[1, 3]), float(Tee[2, 3]))
         ok = self._vendor_movel_tcp(movel_xyz, ONCE_FF_MOVEL_VEL_MPS)
-        q2, Tee2 = self._wait_state(2.0)
+        # MoveL 刚切回关节模式：JTC 还握着贴面前的悬停关节。等 2 s 再钉
+        # 等于让 vmax=0.05 往悬停留 0.1 rad（2026-10-16 收臂实测多抬 13–20 mm）。
+        q2, Tee2 = self._wait_state(0.3)
         if q2 is not None and Tee2 is not None:
             q, Tee = q2, Tee2
         self._pin(q)
@@ -3392,14 +3406,36 @@ class Stage2Locate(TolerantNode):
         )
 
         # ---- 5) 锁定保持 ----
+        # 贴面之后就停在这个法兰 Z。没有接触传感器，到位只认 final_gap_mm
+        # （规尖离面）。三阶段开局会抬回悬高，这里标明已经插到位，流水线不再上抬。
+        if (
+            self.descent_final_gap_m is not None
+            and self.final_descent_enable
+            and not self.hover_only
+        ):
+            final = dict(final)
+            final["insert_held"] = True
+            final["flange_z_m"] = round(float(Tee[2, 3]), 4)
+            final["tip_gap_mm"] = round(float(self.descent_final_gap_m) * 1000.0, 2)
+            final["commanded_gap_mm"] = round(float(self.final_gap_m) * 1000.0, 2)
+            final["insert_z_source"] = "final_gap_mm"
         self._state("hold")
         q_fin = self._q()
         self._pin(q_fin if q_fin is not None else q)
-        self.get_logger().info(
-            f"=== 二阶段定位完成: {self.target_hole} 残差 {final.get('residual_mm', -1)}mm，"
-            f"规尖悬在孔面上方 ≈{gap_txt}mm（不接触）。"
-            f"结果: /stage2/result, /tmp/stage2_result.json ==="
-        )
+        if final.get("insert_held"):
+            self.get_logger().info(
+                f"=== 二阶段定位完成: {self.target_hole} 残差 {final.get('residual_mm', -1)}mm，"
+                f"法兰 Z={final.get('flange_z_m')}m，规尖离面 {gap_txt}mm"
+                f"（final_gap {self.final_gap_m * 1000:.1f}mm，无接触传感器）。"
+                "停在这一高度，不抬回悬高。"
+                f"结果: /stage2/result, /tmp/stage2_result.json ==="
+            )
+        else:
+            self.get_logger().info(
+                f"=== 二阶段定位完成: {self.target_hole} 残差 {final.get('residual_mm', -1)}mm，"
+                f"规尖悬在孔面上方 ≈{gap_txt}mm（不接触）。"
+                f"结果: /stage2/result, /tmp/stage2_result.json ==="
+            )
         return 0, q_fin if q_fin is not None else q, Tee, final
 
     def _linger_hold(self, final: Dict[str, Any], seconds: Optional[float] = None) -> None:

install/arm_application/lib/python3.11/site-packages/arm_application/stage3_search_node.py

@@ -526,7 +526,8 @@ class Stage3Search(Stage2Locate):
             tgt[2] += dz_m
             self._goto(q, Tee, tgt, verticalize=True, tol_m=0.001,
                        tol_tilt=self.vert_tol_rad * 3.0, tag="retract",
-                       v=0.03, max_hops=3, settle_s=0.2, min_dur=0.6)
+                       v=0.03, max_hops=2, settle_s=0.2, min_dur=0.6,
+                       hold_xy=True, z_up_only=True)
         except Exception:
             pass
 

install/arm_application/share/arm_application/config/perception.yaml

@@ -12,7 +12,7 @@ perception_node:
     image_topic: ""
     camera_info_topic: ""
     depth_topic: "/d405/aligned_depth_to_color/image_raw"
-    weights: "models/yolo/gongjian0929.onnx"
+    weights: "/home/cc/ros2/models/yolo/gongjian0929.onnx"
     conf: 0.10
     iou: 0.45
     imgsz: 640

install/arm_application/share/arm_application/package.ps1

@@ -112,6 +112,5 @@ $env:COLCON_CURRENT_PREFIX=(Get-Item $PSCommandPath).Directory.Parent.Parent.Ful
 
 colcon_package_source_powershell_script "$env:COLCON_CURRENT_PREFIX/share/arm_application/hook/pythonpath.ps1"
 colcon_package_source_powershell_script "$env:COLCON_CURRENT_PREFIX/share/arm_application/hook/ament_prefix_path.ps1"
-colcon_package_source_powershell_script "$env:COLCON_CURRENT_PREFIX/../../build/arm_application/share/arm_application/hook/pythonpath_develop.ps1"
 
 Remove-Item Env:\COLCON_CURRENT_PREFIX

install/arm_application/share/colcon-core/packages/arm_application

@@ -1 +1 @@
-arm_kinematics:arm_msgs:geometry_msgs:python3-serial:python3-yaml:rclpy:sensor_msgs:std_msgs:std_srvs:tf2_ros:trajectory_msgs
+arm_kinematics:arm_msgs:control_msgs:geometry_msgs:python3-serial:python3-yaml:rclpy:sensor_msgs:std_msgs:std_srvs:tf2_ros:trajectory_msgs