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

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

Browse Source 
搜孔开局会抬到悬高再落回 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>
This commit is contained in:
arm_dev
2026-10-19 11:30:49 +08:00
parent 8c4dccccb1
commit 7af9e3d4b6
103 changed files with 5594 additions and 2498 deletions
Download Patch File Download Diff File Expand all files Collapse all files
config/site.example.yaml
+42 -52
View File
@@ -10,8 +10,8 @@
# 用法：复制本文件为 config/site.yaml，然后把所有 <占位符> 换成现场实际值。
# 现场值包括：机械臂 SN / IP、相机序列号、夹爪串口、传感器 USB 路径。
#
# 说明：仓库里的文件用相对仓库根的路径（例如 models/yolo/...）。
# 不要改回某个用户的绝对家目录，换台电脑就会找不到文件。
# 说明：site.yaml 里的路径出现 /home/USER 是因为生成时匿名化了用户名；
# 部署时请改成实际用户的 home 目录。
# =============================================================================
# =============================================================================
@@ -61,17 +61,15 @@ robot:
# （status 里的 init=need_init 只说明"绝对位置标定没做"，不影响夹持）
# 想让开机必初始化（无人值守的上电流程）就显式改 true，并确认爪里是空的。
auto_init: false
named_poses_file: "src/arm_control/config/named_poses_89.yaml"
named_poses_file: "/home/USER/ros2/src/arm_control/config/named_poses_89.yaml"
paths:
handeye_file: "handeye/results/89_eye_in_hand.yaml"
handeye_waypoints: "handeye/poses/89_eye_in_hand.yaml"
handeye_workdir: "handeye/tmp/89"
handeye_file: "/home/USER/ros2/handeye/results/89_eye_in_hand.yaml"
handeye_waypoints: "/home/USER/ros2/handeye/poses/89_eye_in_hand.yaml"
handeye_workdir: "/home/USER/ros2/handeye/tmp/89"
handeye:
# Studio tcp_location（法兰系，米 + qwxyz）。与 rdk Tool.params 必须一致。
# 眼在手上：AX=XB 的手是这份 TCP，再折成 cam→法兰写 T_4x4。
# 眼在手外：AX=XB 的手同样是这份 TCP（板夹在工具上）；T_4x4 仍是 cam→基座，不折。
gripper_tool: "tool10120938"
gripper_tcp_xyz: [-0.0005, 0.0067, 0.2245]
gripper_tcp_xyz: [0.000537, -0.000264, 0.226587]
gripper_tcp_qwxyz: [1.0, 0.0, 0.0, 0.0]
# 左臂跟踪高度（只在 --arm 89 时生效，改 91 的 servo 不影响这里）
# 锚点 `&servo_left`：下面的"仿真台"条目直接引用它 —— 仿真建的就是左臂，
@@ -179,9 +177,8 @@ robot:
# 落在本文件下方注释记录的合理区间 0.204~0.233 内。
# 复核：python3 scripts/sense_tool_length.py --sn Rizon4-XXXXXX
# python3 scripts/lib/real_geometry_check.py --sn Rizon4-XXXXXX
# 2026-10-12：下臂安全距离改按 Studio 工具 tool10120938 的 TCP +Z。
# 上面 0.223 是旧 tool1（夹爪 TCP 0.147 + 规露出 0.076），这把工具不再用。
tool_tip_below_flange_m: 0.2245
# 下臂安全距离按 tool10120938 的 TCP +Z，不再用旧 tool1 的 0.223。
tool_tip_below_flange_m: 0.226587
# >>> 仿真那台臂（Gazebo）也登记成 robots[] 里的一台 <<<
# 为什么必须登记：应用是按"当前臂"的名字去查 frame / joint / 相机话题的
#（`sim_env.sh` 注释里说的"仿真与真机共用同一套 site.yaml"）。仿真的 TF 与关节
@@ -211,7 +208,7 @@ robot:
# 为什么必须给文件而不能留空：visual_servo 的 servo_node 需要
# T_flange_cam 矩阵做像素→笛卡尔换算；留空它就直接拒绝启动
#（"手眼标定结果缺失"），S04 跟踪起不来。
handeye_file: "handeye/results/sim_eye_in_hand.yaml"
handeye_file: "/home/USER/ros2/handeye/results/sim_eye_in_hand.yaml"
# 运动参数：**大部分**与 89 共用（锚点），但**高度类**必须按仿真台的几何改写。
#
# 为什么不能直接共用：真机是"机械臂装在台面同一高度"，所以"法兰相对基座"
@@ -305,11 +302,11 @@ robot:
port: "/dev/ttyUSB1"
stroke_mm: 30.0
auto_init: false
named_poses_file: "src/arm_control/config/named_poses_91.yaml"
named_poses_file: "/home/USER/ros2/src/arm_control/config/named_poses_91.yaml"
paths:
handeye_file: "handeye/results/91_eye_in_hand.yaml"
handeye_waypoints: "handeye/poses/91_eye_in_hand.yaml"
handeye_workdir: "handeye/tmp/91"
handeye_file: "/home/USER/ros2/handeye/results/91_eye_in_hand.yaml"
handeye_waypoints: "/home/USER/ros2/handeye/poses/91_eye_in_hand.yaml"
handeye_workdir: "/home/USER/ros2/handeye/tmp/91"
# >>> 右臂跟踪高度：只改这里，89 仍用上面那段 <<<
# 法兰 Z；RGI 比平行爪高，通常要比 89 略抬一点。改完重启：
# bash ~/ros2/scripts/run_visual_servo_rt.sh --arm 91 --control
@@ -330,7 +327,7 @@ robot:
paths:
# >>> YOLO 权重（标准放法，不必改代码）<<<
# 1) 把 .onnx / .pt 放到 models/yolo/
# 1) 把 .onnx / .pt 放到 /home/USER/ros2/models/yolo/
# 2) 这一行改成那个绝对路径（写 .onnx 即可；TensorRT 会找同名 .engine）
# 3) backend=tensorrt 时必须先导出引擎（只需做一次）：
# bash ~/ros2/scripts/weights.sh convert <源文件> --name <名字> --group yolo
@@ -344,18 +341,18 @@ paths:
# ultralytics 解不了 pose 的 TensorRT engine，运行时走
# arm_application/pose_detector.py 的自解码（见该模块头）。
# 仍然用 .engine（实测 ~2.6 ms/帧；onnx 只有 CPU provider，约 27 ms）。
yolo_weights: "models/yolo/gongjian0929.onnx"
yolo_weights: "/home/USER/ros2/models/yolo/gongjian0929.onnx"
# 换模型：把 models/yolo/ 下别的 .onnx / .pt 绝对路径填进来即可。
# 未在用的模型已移出仓库（体积大且干扰 `weights.sh list`），要恢复见 models/README.md。
# 入孔模型（backend=yolo 时把孔当框）。two_stage 用下面的工件权重。
# pose 模型两处共用同一份 —— 它既能框工件，又直接给 k1/k2 关键点。
# 查悬空引用：bash ~/ros2/scripts/weights.sh doctor
hole_weights: "models/yolo/gongjian0929.onnx"
hole_weights: "/home/USER/ros2/models/yolo/gongjian0929.onnx"
# 下面是缺省（未匹配到 robots[] 时）。真机以 robots[].paths 为准：89 / 91 各一份。
handeye_file: "handeye/results/89_eye_in_hand.yaml"
handeye_waypoints: "handeye/poses/89_eye_in_hand.yaml"
handeye_calibrate: "handeye/calibrate.yaml"
handeye_workdir: "handeye/tmp/89"
handeye_file: "/home/USER/ros2/handeye/results/89_eye_in_hand.yaml"
handeye_waypoints: "/home/USER/ros2/handeye/poses/89_eye_in_hand.yaml"
handeye_calibrate: "/home/USER/ros2/handeye/calibrate.yaml"
handeye_workdir: "/home/USER/ros2/handeye/tmp/89"
# 空=默认 DDS(SHM)；勿强制 UDP-only，否则相机预览收不到 /workpiece/detection
fastdds_profile: ""
@@ -595,9 +592,8 @@ servo:
lock_hold_sec: 0.7 # 旧死区驻留（混合模式不再用它进二阶段）
# 混合模式必填。交回 D405 的等待不再用它，见 d435_handback_sec。
lock_still_sec: 3.0
# 进程内 MoveL 到位后，法兰 XY 连续停稳这么久才交回 D405。
# 旧 3s 是杀栈冷启动的垫时间；不杀栈后只留机械停稳。晃了把数加大，不许再杀栈。
d435_handback_sec: 0.5
# 厂家 MoveL 结束后，法兰连续这么久没再动，才打开 D405 框心。
d435_handback_sec: 2.0
# 框心对准并且法兰停稳这么久，才精拍一次小孔关键点、进对孔。实时点到此为止。
wrist_align_sec: 0.5
lock_still_move_m: 0.015 # 15 mm；超过就重计时，避免"晃一下刚好满 3 秒"
@@ -679,10 +675,10 @@ gripper:
hole_detect:
backend: "two_stage" # two_stage | yolo | hough
# backend=yolo 时用。
weights: "models/yolo/gongjian0929.onnx"
weights: "/home/USER/ros2/models/yolo/gongjian0929.onnx"
# 二阶段工件检测权重。pose 模型既框工件、又直接给 k1/k2 关键点，
# 所以和上面的跟踪权重是同一份。
workpiece_weights: "models/yolo/gongjian0929.onnx"
workpiece_weights: "/home/USER/ros2/models/yolo/gongjian0929.onnx"
# pose 关键点优先：检测器给了 k1/k2 就直接用，不用再找圆。
# 关键点不可信（低于 keypoint_min_conf / 两点几乎重合）时自动回落到
# 亮环 + Hough 那套老路径。
@@ -690,7 +686,7 @@ hole_detect:
keypoint_min_conf: 0.30
keypoint_min_dist_px: 15.0
keypoint_hole_radius_px: 12.0 # 关键点没有半径；反投影时按这个半径取孔周深度环
config: "src/arm_application/config/hole_detect.yaml"
config: "/home/USER/ros2/src/arm_application/config/hole_detect.yaml"
conf: 0.25
iou: 0.7
imgsz: 640
@@ -768,26 +764,22 @@ hole_detect:
# bbox=(0,142,97,222) 反投影出盒外的 (0.280,-0.747)（偏差 211.5mm），旧逻辑
# **不判误检、只把目标截断到盒边**，臂照样横移 56mm 离开工件 ⇒ 精定位再也
# 看不到 k1/k2 ⇒ exit=4。判据改成"盒外/偏移过大 ⇒ 判误检、原地不动"。
coarse_max_offset_m: 0.12 # 目标离夹爪轴线超过此值就不像粗居中量 [m]
coarse_max_offset_m: 0.08 # 目标离夹爪轴线超过此值就不像粗居中量 [m]；0.12 放行了 2026-10-16 的 110mm 回撤
coarse_border_px: 2 # bbox 贴边（被画面切）即弃用 [px]；见 _bbox_on_border
# —— 柔顺运动（夹爪持规，防急加速/急停/过冲把规震脱）：速度放慢 + 平滑刹车 ——
align_speed: 0.014 # 对准段笛卡尔速度 [m/s]（原0.03，放缓防冲）
move_speed: 0.022 # 升降/粗居中段速度 [m/s]（原0.05，放缓防震规）
# 对小孔关键点：只用像素射线交到 workpiece_surface_z_m，不读深度图。
hole_visual_only: true
# 本分支实验（exp/hole-keypoint-once）：true=悬停拍一次，框心→k1 一次走完
# （40mm 上限）。main 保存点是 false（多走多拍）。
# 本分支实验：true=悬停拍一次并走完到孔；main 保存点应保持 false。
hole_keypoint_once: true
# true=悬停走完后一条厂家 MoveL 贴面（不补拍）。对照 37b85aa 改回 false。
# true=悬停走完后一条厂家 MoveL 贴面。对照观察高补交时改 false。
hole_once_lock_xy_descend: true
# 贴面相对悬停：沿 -look 走 k×剩余下降 [m/m]。0=笛卡尔锁 XY 只降 Z。
# 0.018 在框心改平面交点后两条链路都 +1.3mm X 过补，插不进。
# 0=笛卡尔锁 XY 只降 Z。k>0 才沿 -look 加前馈。
once_ff_look_per_m: 0.0
final_gap_mm: 2.0 # 对准后贴面目标：规尖离面 [mm]（不接触）。笛卡尔锁 XY
# 时法兰 Z = 工具长 + 工件面 + 本值。CLI --final-gap 覆盖。
final_gap_mm: 2.0 # 对准后贴面目标：规尖离面 [mm]。CLI --final-gap 覆盖。
observe_gap_mm: 15.0 # --no-descent 时的观测下降：只降到离面这么高 [mm]，
# 方便看规尖偏没偏。必须 ≥ final_gap_mm。
# 左臂89：悬停间隙约 73.5mm（0.320−0.2245−0.022）⇒ 观测下降约 58mm。
# 左臂89：悬停间隙约 75mm（0.320−0.223−0.022）⇒ 大约下降 60mm。
final_descent_speed: 0.005 # 最后轻降速度 [m/s]（很慢，柔顺）
# ✅ 2026-10-08 定稿：下降量**只由几何决定**（法兰 FK − 工具长 − 工件面 − final_gap），
# 深度读数只做交叉核对、不再参与任何决策。两段真机事故方向相反、都指向这条：
@@ -800,7 +792,7 @@ hole_detect:
# ⇒ 读数往**小**错一次就永远到不了目标。
# 同一套读数两个方向各错一次、幅度 150mm 量级、随位姿乱跳 —— 它对"能降多少"
# 和"该不该停"都没有发言权。现在它只在偏差 >10mm 时喊一句"深度测距与几何不符"。
# 现行几何：悬停 0.320 − 工具长 0.2245 − 工件面 0.022 = 73.5mm 间隙 ⇒ 降 71.5mm 到 2mm。
# 现行几何：悬停 0.320 − 工具长 0.223 − 工件面 0.022 = 75mm 间隙 ⇒ 降 73mm 到 2mm。
# 三道**物理/几何**保险：descent_budget_m（配置上限）+ 腕部 FT 接触检测（停+退 1mm）
# + 任一步 _goto 走不到位就停。
# 缺几何（工具长/工件面没配）时**拒绝下降**并报错（保持悬停），绝不拿测距去赌。
@@ -812,9 +804,11 @@ hole_detect:
max_depth_m: 0.60
linger_sec: 8.0 # 完成后继续发布结果/叠图的时长
# ===== 旋转装配仍读这里的力阈值（stage3_search_node）=====
# 搜孔轻压已取消：不带 --screw 的入口会直接拒绝，整体流程也不再进入这一步。
# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --screw
# ===== 三阶段柔顺搜孔（stage3_search_node，Orisys 触觉 + 力闭环螺旋）=====
# 前置：二阶段已完成，臂悬停在目标孔上方。先 run_tactile.sh --cli 看力读数量级再调阈值。
# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 # 搜孔+落座
# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --depth 0 # 只搜孔定心不插入
# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --depth 6 --pitch 1.0 # 螺纹规旋入
stage3:
# —— 力阈值全是「相对静止均值的增量」（Orisys 读数是形变任意单位且基线会漂移，
# 节点启动时自动测 1.2s 静止均值 μ0/方差 σ0，看日志「触觉基线/派生阈值」两行）——
@@ -841,12 +835,9 @@ hole_detect:
drop_frames: 2 # 跌落需持续的帧数（防毛刺）
center_scan: false # 入孔后十字微扫；默认关（0.8mm 扫会把已入孔的规刮偏）
center_scan_mm: 0.3
insert_depth_mm: 0.0 # 默认只搜孔+轻压，不拧入。旋转装配不读这项
insert_depth_mm: 0.0 # 默认只搜孔+轻压，不拧入；拧入阶段再用 --depth
insert_retry: 3
screw_pitch_mm: 0.0 # 搜孔轻压不旋。旋转装配不读这项
# 控制页「旋转装配」单独用。数值就是手册里的 M6 旋入：深度 6 mm、螺距 1.0 mm/圈。
screw_stage_depth_mm: 6.0
screw_stage_pitch_mm: 1.0
screw_pitch_mm: 0.0 # 螺纹通止规螺距：>0 右旋 / <0 左旋 / 0=不旋（光面规）
search_speed: 0.03 # 搜孔笛卡尔速度 [m/s]
settle_sec: 0.25 # 每个微步后的静止时间
tactile_timeout_s: 3.0 # 触觉话题断流多久中止
@@ -1102,7 +1093,7 @@ tactile:
# 传感器型号。必须与 devices/sensors/orisys/models/ 下的子目录名一致。
model: "dd03"
# 型号配置随 SDK 一起放：devices/sensors/orisys/models/<型号>/config.json
config_file: "devices/sensors/orisys/models/dd03/config.json"
config_file: "/home/USER/ros2/devices/sensors/orisys/models/dd03/config.json"
# 用 by-path 稳定路径（插哪个 USB 口就认哪个口，不受 video 编号漂移影响）。
# 当前：3.4.1=左指 3.4.2=右指；若左右反了，互换两行即可。
left_video: "/dev/v4l/by-path/pci-0000:00:14.0-usb-0:X.Y.Z:1.0-video-index0"
@@ -1129,8 +1120,7 @@ go_home:
network:
# 2026-09-29 改线：两台臂 → 交换机 → USB-C 千兆网卡（ASIX AX88179）
# MAC 6c:1f:f7:d0:6f:17。内核名会在 eth0 和 enxXXXXXXXXXXXX 之间变，
# NetworkManager 按 MAC 绑定，不按名字。
# MAC 6c:1f:f7:d0:6f:17。内核现在叫 eth0（不再用 enxXXXXXXXXXXXX）。
# 本机 <PC_IP> ↔ <左臂SN>(左89)=.101 / <右臂SN>(右91)=.100
# NM 配置名 robot-usb-lan（静态 .10/24，不当默认路由）。
# 说明：enp4s0 仍是办公网（192.168.1.x）；两台臂已不在 enp4s0 上。
config/site.yaml
+40 -41
View File
@@ -23,7 +23,7 @@ robot:
# 本臂装了 Orisys 视触觉（只有左臂有）。脚本靠这一项决定要不要拉触觉服务，
# 而不是靠"SN 等于某个具体值"这种写死的判断（换台臂就会失效）。
tactile: true
# 2026-10-12 示教的工具 TCP（tool + 1012 + 0938）。眼在手上 AX=XB 用这份 TCP。
# 2026-10-16 下午 RDK 重读 Studio tool10120938（用户再次更新 TCP）。
tool_name: "tool10120938"
camera:
name: d405_left # 话题 /d405_left/... 与 91 分开
@@ -45,17 +45,17 @@ robot:
# （status 里的 init=need_init 只说明"绝对位置标定没做"，不影响夹持）
# 想让开机必初始化（无人值守的上电流程）就显式改 true，并确认爪里是空的。
auto_init: false
named_poses_file: "src/arm_control/config/named_poses_89.yaml"
named_poses_file: "/home/cc/ros2/src/arm_control/config/named_poses_89.yaml"
paths:
handeye_file: "handeye/results/89_eye_in_hand.yaml"
handeye_waypoints: "handeye/poses/89_eye_in_hand.yaml"
handeye_workdir: "handeye/tmp/89"
handeye_file: "/home/cc/ros2/handeye/results/89_eye_in_hand.yaml"
handeye_waypoints: "/home/cc/ros2/handeye/poses/89_eye_in_hand.yaml"
handeye_workdir: "/home/cc/ros2/handeye/tmp/89"
handeye:
# Studio tcp_location（法兰系，米 + qwxyz）。与 rdk Tool.params 必须一致。
# 眼在手上：AX=XB 的手是这份 TCP，再折成 cam→法兰写 T_4x4。
# 眼在手外：AX=XB 的手同样是这份 TCP（板夹在工具上）；T_4x4 仍是 cam→基座，不折。
gripper_tool: "tool10120938"
gripper_tcp_xyz: [-0.0005, 0.0067, 0.2245]
gripper_tcp_xyz: [0.000537, -0.000264, 0.226587]
gripper_tcp_qwxyz: [1.0, 0.0, 0.0, 0.0]
# 左臂跟踪高度（只在 --arm 89 时生效，改 91 的 servo 不影响这里）
# 锚点 `&servo_left`：下面的"仿真台"条目直接引用它 —— 仿真建的就是左臂，
@@ -163,9 +163,11 @@ robot:
# 落在本文件下方注释记录的合理区间 0.204~0.233 内。
# 复核：python3 scripts/sense_tool_length.py --sn Rizon4-063989
# python3 scripts/lib/real_geometry_check.py --sn Rizon4-063989
# 2026-10-12：下臂安全距离改按 Studio 工具 tool10120938 的 TCP +Z。
# 上面 0.223 是旧 tool1（夹爪 TCP 0.147 + 规露出 0.076），这把工具不再用。
tool_tip_below_flange_m: 0.2245
# 2026-10-16 下午：RDK Tool.params("tool10120938").tcp_location
# [x,y,z,qw,qx,qy,qz] = [0.000537, -0.000264, 0.226587, 1, 0, 0, 0]
# 示教器拟合误差：平均 0.58 / 最大 0.99 / 最小 0.29 mm。
# 下臂安全距离用 TCP +Z。上午 0.226274；再往前 0.2245 是 10-12；0.223 是 tool1。
tool_tip_below_flange_m: 0.226587
# >>> 仿真那台臂（Gazebo）也登记成 robots[] 里的一台 <<<
# 为什么必须登记：应用是按"当前臂"的名字去查 frame / joint / 相机话题的
#（`sim_env.sh` 注释里说的"仿真与真机共用同一套 site.yaml"）。仿真的 TF 与关节
@@ -195,7 +197,7 @@ robot:
# 为什么必须给文件而不能留空：visual_servo 的 servo_node 需要
# T_flange_cam 矩阵做像素→笛卡尔换算；留空它就直接拒绝启动
#（"手眼标定结果缺失"），S04 跟踪起不来。
handeye_file: "handeye/results/sim_eye_in_hand.yaml"
handeye_file: "/home/cc/ros2/handeye/results/sim_eye_in_hand.yaml"
# 运动参数：**大部分**与 89 共用（锚点），但**高度类**必须按仿真台的几何改写。
#
# 为什么不能直接共用：真机是"机械臂装在台面同一高度"，所以"法兰相对基座"
@@ -289,11 +291,11 @@ robot:
port: "/dev/ttyUSB1"
stroke_mm: 30.0
auto_init: false
named_poses_file: "src/arm_control/config/named_poses_91.yaml"
named_poses_file: "/home/cc/ros2/src/arm_control/config/named_poses_91.yaml"
paths:
handeye_file: "handeye/results/91_eye_in_hand.yaml"
handeye_waypoints: "handeye/poses/91_eye_in_hand.yaml"
handeye_workdir: "handeye/tmp/91"
handeye_file: "/home/cc/ros2/handeye/results/91_eye_in_hand.yaml"
handeye_waypoints: "/home/cc/ros2/handeye/poses/91_eye_in_hand.yaml"
handeye_workdir: "/home/cc/ros2/handeye/tmp/91"
# >>> 右臂跟踪高度：只改这里，89 仍用上面那段 <<<
# 法兰 Z；RGI 比平行爪高，通常要比 89 略抬一点。改完重启：
# bash ~/ros2/scripts/run_visual_servo_rt.sh --arm 91 --control
@@ -314,7 +316,7 @@ robot:
paths:
# >>> YOLO 权重（标准放法，不必改代码）<<<
# 1) 把 .onnx / .pt 放到 models/yolo/
# 1) 把 .onnx / .pt 放到 /home/cc/ros2/models/yolo/
# 2) 这一行改成那个绝对路径（写 .onnx 即可；TensorRT 会找同名 .engine）
# 3) backend=tensorrt 时必须先导出引擎（只需做一次）：
# bash ~/ros2/scripts/weights.sh convert <源文件> --name <名字> --group yolo
@@ -328,18 +330,18 @@ paths:
# ultralytics 解不了 pose 的 TensorRT engine，运行时走
# arm_application/pose_detector.py 的自解码（见该模块头）。
# 仍然用 .engine（实测 ~2.6 ms/帧；onnx 只有 CPU provider，约 27 ms）。
yolo_weights: "models/yolo/gongjian0929.onnx"
yolo_weights: "/home/cc/ros2/models/yolo/gongjian0929.onnx"
# 换模型：把 models/yolo/ 下别的 .onnx / .pt 绝对路径填进来即可。
# 未在用的模型已移出仓库（体积大且干扰 `weights.sh list`），要恢复见 models/README.md。
# 入孔模型（backend=yolo 时把孔当框）。two_stage 用下面的工件权重。
# pose 模型两处共用同一份 —— 它既能框工件，又直接给 k1/k2 关键点。
# 查悬空引用：bash ~/ros2/scripts/weights.sh doctor
hole_weights: "models/yolo/gongjian0929.onnx"
hole_weights: "/home/cc/ros2/models/yolo/gongjian0929.onnx"
# 下面是缺省（未匹配到 robots[] 时）。真机以 robots[].paths 为准：89 / 91 各一份。
handeye_file: "handeye/results/89_eye_in_hand.yaml"
handeye_waypoints: "handeye/poses/89_eye_in_hand.yaml"
handeye_calibrate: "handeye/calibrate.yaml"
handeye_workdir: "handeye/tmp/89"
handeye_file: "/home/cc/ros2/handeye/results/89_eye_in_hand.yaml"
handeye_waypoints: "/home/cc/ros2/handeye/poses/89_eye_in_hand.yaml"
handeye_calibrate: "/home/cc/ros2/handeye/calibrate.yaml"
handeye_workdir: "/home/cc/ros2/handeye/tmp/89"
# 空=默认 DDS(SHM)；勿强制 UDP-only，否则相机预览收不到 /workpiece/detection
fastdds_profile: ""
@@ -579,9 +581,8 @@ servo:
lock_hold_sec: 0.7 # 旧死区驻留（混合模式不再用它进二阶段）
# 混合模式必填。交回 D405 的等待不再用它，见 d435_handback_sec。
lock_still_sec: 3.0
# 进程内 MoveL 到位后，法兰 XY 连续停稳这么久才交回 D405。
# 旧 3s 是杀栈冷启动的垫时间；不杀栈后只留机械停稳。晃了把数加大，不许再杀栈。
d435_handback_sec: 0.5
# 厂家 MoveL 结束后，法兰连续这么久没再动，才打开 D405 框心。
d435_handback_sec: 2.0
# 框心对准并且法兰停稳这么久，才精拍一次小孔关键点、进对孔。实时点到此为止。
wrist_align_sec: 0.5
lock_still_move_m: 0.015 # 15 mm；超过就重计时，避免"晃一下刚好满 3 秒"
@@ -663,10 +664,10 @@ gripper:
hole_detect:
backend: "two_stage" # two_stage | yolo | hough
# backend=yolo 时用。
weights: "models/yolo/gongjian0929.onnx"
weights: "/home/cc/ros2/models/yolo/gongjian0929.onnx"
# 二阶段工件检测权重。pose 模型既框工件、又直接给 k1/k2 关键点，
# 所以和上面的跟踪权重是同一份。
workpiece_weights: "models/yolo/gongjian0929.onnx"
workpiece_weights: "/home/cc/ros2/models/yolo/gongjian0929.onnx"
# pose 关键点优先：检测器给了 k1/k2 就直接用，不用再找圆。
# 关键点不可信（低于 keypoint_min_conf / 两点几乎重合）时自动回落到
# 亮环 + Hough 那套老路径。
@@ -674,7 +675,7 @@ hole_detect:
keypoint_min_conf: 0.30
keypoint_min_dist_px: 15.0
keypoint_hole_radius_px: 12.0 # 关键点没有半径；反投影时按这个半径取孔周深度环
config: "src/arm_application/config/hole_detect.yaml"
config: "/home/cc/ros2/src/arm_application/config/hole_detect.yaml"
conf: 0.25
iou: 0.7
imgsz: 640
@@ -752,7 +753,7 @@ hole_detect:
# bbox=(0,142,97,222) 反投影出盒外的 (0.280,-0.747)（偏差 211.5mm），旧逻辑
# **不判误检、只把目标截断到盒边**，臂照样横移 56mm 离开工件 ⇒ 精定位再也
# 看不到 k1/k2 ⇒ exit=4。判据改成"盒外/偏移过大 ⇒ 判误检、原地不动"。
coarse_max_offset_m: 0.12 # 目标离夹爪轴线超过此值就不像粗居中量 [m]
coarse_max_offset_m: 0.08 # 目标离夹爪轴线超过此值就不像粗居中量 [m]；0.12 放行了 2026-10-16 的 110mm 回撤
coarse_border_px: 2 # bbox 贴边（被画面切）即弃用 [px]；见 _bbox_on_border
# —— 柔顺运动（夹爪持规，防急加速/急停/过冲把规震脱）：速度放慢 + 平滑刹车 ——
align_speed: 0.014 # 对准段笛卡尔速度 [m/s]（原0.03，放缓防冲）
@@ -771,7 +772,7 @@ hole_detect:
# 时法兰 Z = 工具长 + 工件面 + 本值。CLI --final-gap 覆盖。
observe_gap_mm: 15.0 # --no-descent 时的观测下降：只降到离面这么高 [mm]，
# 方便看规尖偏没偏。必须 ≥ final_gap_mm。
# 左臂89：悬停间隙约 73.5mm（0.320−0.2245−0.022）⇒ 观测下降约 58mm。
# 左臂89：悬停间隙约 71.4mm（0.320−0.226587−0.022）⇒ 观测下降约 56.4mm。
final_descent_speed: 0.005 # 最后轻降速度 [m/s]（很慢，柔顺）
# ✅ 2026-10-08 定稿：下降量**只由几何决定**（法兰 FK − 工具长 − 工件面 − final_gap），
# 深度读数只做交叉核对、不再参与任何决策。两段真机事故方向相反、都指向这条：
@@ -784,7 +785,7 @@ hole_detect:
# ⇒ 读数往**小**错一次就永远到不了目标。
# 同一套读数两个方向各错一次、幅度 150mm 量级、随位姿乱跳 —— 它对"能降多少"
# 和"该不该停"都没有发言权。现在它只在偏差 >10mm 时喊一句"深度测距与几何不符"。
# 现行几何：悬停 0.320 − 工具长 0.2245 − 工件面 0.022 = 73.5mm 间隙 ⇒ 降 71.5mm 到 2mm。
# 现行几何：悬停 0.320 − 工具长 0.226587 − 工件面 0.022 = 71.4mm 间隙 ⇒ 降 69.4mm 到 2mm。
# 三道**物理/几何**保险：descent_budget_m（配置上限）+ 腕部 FT 接触检测（停+退 1mm）
# + 任一步 _goto 走不到位就停。
# 缺几何（工具长/工件面没配）时**拒绝下降**并报错（保持悬停），绝不拿测距去赌。
@@ -796,9 +797,11 @@ hole_detect:
max_depth_m: 0.60
linger_sec: 8.0 # 完成后继续发布结果/叠图的时长
# ===== 旋转装配仍读这里的力阈值（stage3_search_node）=====
# 搜孔轻压已取消：不带 --screw 的入口会直接拒绝，整体流程也不再进入这一步。
# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --screw
# ===== 三阶段柔顺搜孔（stage3_search_node，Orisys 触觉 + 力闭环螺旋）=====
# 前置：二阶段已完成，臂悬停在目标孔上方。先 run_tactile.sh --cli 看力读数量级再调阈值。
# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 # 搜孔+落座
# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --depth 0 # 只搜孔定心不插入
# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --depth 6 --pitch 1.0 # 螺纹规旋入
stage3:
# —— 力阈值全是「相对静止均值的增量」（Orisys 读数是形变任意单位且基线会漂移，
# 节点启动时自动测 1.2s 静止均值 μ0/方差 σ0，看日志「触觉基线/派生阈值」两行）——
@@ -825,12 +828,9 @@ hole_detect:
drop_frames: 2 # 跌落需持续的帧数（防毛刺）
center_scan: false # 入孔后十字微扫；默认关（0.8mm 扫会把已入孔的规刮偏）
center_scan_mm: 0.3
insert_depth_mm: 0.0 # 默认只搜孔+轻压，不拧入。旋转装配不读这项
insert_depth_mm: 0.0 # 默认只搜孔+轻压，不拧入；拧入阶段再用 --depth
insert_retry: 3
screw_pitch_mm: 0.0 # 搜孔轻压不旋。旋转装配不读这项
# 控制页「旋转装配」单独用。数值就是手册里的 M6 旋入：深度 6 mm、螺距 1.0 mm/圈。
screw_stage_depth_mm: 6.0
screw_stage_pitch_mm: 1.0
screw_pitch_mm: 0.0 # 螺纹通止规螺距：>0 右旋 / <0 左旋 / 0=不旋（光面规）
search_speed: 0.03 # 搜孔笛卡尔速度 [m/s]
settle_sec: 0.25 # 每个微步后的静止时间
tactile_timeout_s: 3.0 # 触觉话题断流多久中止
@@ -1086,7 +1086,7 @@ tactile:
# 传感器型号。必须与 devices/sensors/orisys/models/ 下的子目录名一致。
model: "dd03"
# 型号配置随 SDK 一起放：devices/sensors/orisys/models/<型号>/config.json
config_file: "devices/sensors/orisys/models/dd03/config.json"
config_file: "/home/cc/ros2/devices/sensors/orisys/models/dd03/config.json"
# 用 by-path 稳定路径（插哪个 USB 口就认哪个口，不受 video 编号漂移影响）。
# 当前：3.4.1=左指 3.4.2=右指；若左右反了，互换两行即可。
left_video: "/dev/v4l/by-path/pci-0000:00:14.0-usb-0:3.4.1:1.0-video-index0"
@@ -1113,8 +1113,7 @@ go_home:
network:
# 2026-09-29 改线：两台臂 → 交换机 → USB-C 千兆网卡（ASIX AX88179）
# MAC 6c:1f:f7:d0:6f:17。内核名会在 eth0 和 enx6c1ff7d06f17 之间变，
# NetworkManager 按 MAC 绑定，不按名字。
# MAC 6c:1f:f7:d0:6f:17。内核现在叫 eth0（不再用 enx6c1ff7d06f17）。
# 本机 192.168.2.10 ↔ 063989(左89)=.101 / 063991(右91)=.100
# NM 配置名 robot-usb-lan（静态 .10/24，不当默认路由）。
# 说明：enp4s0 仍是办公网（192.168.1.x）；两台臂已不在 enp4s0 上。
docs/HANDBOOK.md
+17 -11
View File
@@ -34,7 +34,7 @@
### 1.3 一次完整任务分几步
代码里跟踪和对孔串成一条命令。搜孔轻压已取消，`run_all.sh` 不会再进入那一步，也不能用参数打开。旋入是单独的 `run_stage3_search.sh --screw`。
代码里用 **stage1 / stage2 / stage3** 三段串起来，一条命令跑完：
```
run_all.sh
@@ -46,13 +46,18 @@ run_all.sh
│ YOLO 找工件外框 → Hough 找左右两个孔心 →
│ 深度 + 内参 + 手眼 → 换算出孔的基座坐标 →
│ 移动夹爪轴线到目标孔正上方 → 复拍验证 → 锁定
│
└─ stage3_search 柔顺搜孔：力闭环下的螺旋搜索，直到规的尖端"掉进"孔里，
再十字微扫定心，最后在力限内下压（可选按螺距旋入）
```
跟踪和对孔在同一个进程里交接。盯日志：
三段之间**在同一个进程里交接**，所以相机、触觉、手眼 TF 只初始化一次。
盯日志就能知道跑到哪了 —— 三行关键输出：
```
[pipeline] === stage1_track === → 在等跟踪锁定
[pipeline] === stage2_locate === → 在拍照对孔
[pipeline] === stage3_search === → 触面 → ★ 入孔 → 轻压到位
```
日常入口（三个终端，**必须同一个 `--arm`**）：
@@ -60,7 +65,7 @@ run_all.sh
```bash
bash ~/ros2/scripts/run_real.sh --arm 89 # T1 控制栈，一直开着
bash ~/ros2/scripts/run_camera_d405.sh --arm 89 --preview # T2 相机预览，一直开着
bash ~/ros2/scripts/run_all.sh --arm 89 # T3 跟踪 + 对孔（不含搜孔、不含旋入）
bash ~/ros2/scripts/run_all.sh --arm 89 # T3 三段串联（默认不拧入）
bash ~/ros2/scripts/run_all.sh --arm 89 --init-z 0.35 --hover-z 0.34
```
@@ -68,15 +73,15 @@ bash ~/ros2/scripts/run_all.sh --arm 89 --init-z 0.35 --hover-z 0.34
`D405_MISS_BEFORE_D435_S`（0.5 s）没有 conf≥`perception.wrist_handoff_conf`（0.5）
的目标，才用顶视 D435 **拍一张**，把点用眼在手外 T 变到基座（不再乘法兰）。
这一段运动是进程内厂家 `MoveL`（不杀 `run_real.sh`）：当前厂家 `tcp_pose` 加
ROS 相对 XY，`keep_z`。硬件在 MoveL 期间不灌关节指令。到位后法兰 XY 连续停稳
`servo.d435_handback_sec`（0.5 s）才交回 D405。赶路期间不下发 D405（两套控制互抢会晃）。
停稳之后改回 D405，跟踪点是工件框的矩形中心，不追 k0。
ROS 相对 XY，`keep_z`。硬件在 MoveL 期间不灌关节指令。D435 这一点变到基座系后，只下发厂家 `MoveL`。赶路期间 D405 推理开着，
控制点截断。法兰连续停稳 `servo.d435_handback_sec`（2 s）后才打开 D405，
追工件框的矩形中心。框心对准并停稳后才精拍 k1/k2、下臂。
启动高度用 `--init-z`（法兰 Z，米），对孔悬停用 `--hover-z`。两个都是法兰高度。
下臂贴面用 `--final-gap`（规尖离面，毫米；site 默认 `final_gap_mm` 2）。笛卡尔锁 XY
时只改 Z：法兰 = 工具长 + 工件面 + 间隙。不能和 `--no-descent` 一起写。
会让工具尖进入工件、或超出工作盒的值直接拒绝，不会改成另一个高度再往下走。
不写则一阶段锁高仍按 site，对孔悬停仍是 `hole_hover_z_m`（89 为 0.320）。
框心对准连续 `servo.wrist_align_sec`（0.5 s）→ `/visual_servo/locked`。
框心对准连续 `servo.wrist_align_sec`（0.5 s）才 `/visual_servo/locked`，然后才精拍下臂。
本分支（`exp/hole-keypoint-once`）二阶段是**单次精拍**：悬停拍一次 k1/k2，
把框心到目标孔一次走完（步长上限 40mm）。现行实验 `hole_once_lock_xy_descend: true`：
走完后不补拍，一条厂家 MoveL 收到贴面。`once_ff_look_per_m: 0` 时 MoveL 只降 Z
@@ -84,7 +89,8 @@ ROS 相对 XY，`keep_z`。硬件在 MoveL 期间不灌关节指令。到位后
不是 ROS 绝对坐标。保存点 `37b85aa` 是观察高 15mm 再斜线补 XY。看 `/tmp/stage2_result.json`。
`main` 保存点仍是多走多拍。
孔坐标把关键点像素交到 `workpiece_surface_z_m`，不读深度图。下臂停在
`tool10120938` 的 TCP 长度（0.2245 m）加上 `final_gap_mm` 之上，不按旧 tool1 的 0.223 m。
`tool10120938` 的 TCP 长度（0.226587 m，2026-10-16 下午 RDK 重读）加上 `final_gap_mm` 之上，不按旧 tool1 的 0.223 m。
贴面之后就停在这个法兰高度，不抬回 `hole_hover_z_m`。没有接触传感器，到位只认 `final_gap_mm`，不另设一个 Z。
只测 D435 这一段位移时加 `--d435-only`：不交 D405、不进二阶段，并且要求先停 `run_real.sh`。
```bash
@@ -107,8 +113,8 @@ bash ~/ros2/scripts/run_visual_servo_rt.sh --arm 89 --control
| 能力 | 状态 | 怎么自己确认 |
|------|------|--------------|
| 跟踪 → 对孔 | ✅ 真机跑通过。搜孔轻压已取消，这条命令不再下压搜孔 | `run_all.sh --arm 89` |
| 按螺距旋入（螺纹规） | ✅ 单独阶段。不带 `--screw` 的旧搜孔命令会直接拒绝 | `run_stage3_search.sh --arm 89 --screw` |
| 三段串联（跟踪 → 对孔 → 搜孔轻压） | ✅ 真机跑通 | `run_all.sh --arm 89` |
| 按螺距旋入（螺纹规） | ✅ 有参数，`--depth`/`--pitch` 控制 | `run_stage3_search.sh --arm 89 --depth 6 --pitch 1.0` |
| Orisys 双指触觉（法向 + 两路切向） | ⚠️ 能读，但**轴向映射尚未实测确认** | 见 [`TACTILE.md`](TACTILE.md) |
| Orisys 的深度图 / 力矢量图 | ⚠️ 消息结构与话题已在，真机值待确认 | 同上 |
| D435 手外相机（一阶段一次性 + S03） | ⚠️ 89 已按 `tool10120938` 重标（24 视角，10-09 旧结果在 `results/backup/`）；S03 粗定位真机未对过 | `handeye/results/89_d435_eye_to_hand.yaml` |
@@ -505,7 +511,7 @@ bash scripts/run_unit_tests.sh -k gauge # 只跑名字带 gauge 的
| **89 / 91** | 左臂 / 右臂的现场编号，**各有独立的手眼、相机、夹爪、锁高，禁止混用** |
| **法兰** | 机械臂末端安装面；手眼 `T_4x4` 仍是相机→法兰。左臂 89 现行按 Studio `tool10120938` 的 TCP 求解，再折回法兰 |
| **eye-in-hand / eye-to-hand** | 相机装在手上 / 装在手上之外（D405 在手上，D435 在外部） |
| **stage1/2** | 跟踪 / 对孔，见 1.3。搜孔轻压已取消 |
| **stage1/2/3** | 跟踪 / 对孔 / 搜孔，见 1.3 |
| **S03 / S04 / S07 / S08** | 整体装配任务书里的阶段编号（S03 粗定位、S04 跟踪、S07 柔顺下压、S08 夹爪旋转循环）。代码里对应的是 stage1/2/3 的拆分实现，**没有一一对应**，别硬套 |
| **基线** | 触觉静止时的读数均值。力的语义是"相对基线的增量"，不是绝对牛顿值 |
| **R1.1 / M1 …** | [`FRAMEWORK_RULES.md`](FRAMEWORK_RULES.md) 里的规则编号，文档和提交信息里直接引用 |
docs/HOLE_DETECT.md
+15 -14
View File
@@ -57,7 +57,8 @@ hole_detect:
## 三阶段串联（推荐日常入口）
一条命令跑完：**视觉跟踪锁定 → 拍照对孔**。搜孔轻压已取消，这条命令不再下压，也不能用参数重新打开。旋入是后面的单独命令。
一条命令跑完：**视觉跟踪锁定 → 拍照对孔 → 搜孔并轻轻按压**。默认**不拧入**
（拧入是后面单独阶段）。三个阶段在同一个节点里交接，相机 / 触觉 / 手眼 TF 只起一次。
```bash
# 终端 1 —— 控制栈（一直开着）
@@ -66,12 +67,12 @@ bash ~/ros2/scripts/run_real.sh --arm 89
# 终端 2 —— 相机预览（一直开着，看跟踪画面）
bash ~/ros2/scripts/run_camera_d405.sh --arm 89 --preview
# 终端 3 —— 跟踪 + 对孔（到这里停，不搜孔）
bash ~/ros2/scripts/run_all.sh --arm 89 # 跟踪 + 对孔
# 终端 3 —— 串联（搜到孔后轻压保持，不拧）
bash ~/ros2/scripts/run_all.sh --arm 89 # 跟踪 + 对孔 + 搜孔轻压
bash ~/ros2/scripts/run_all.sh --arm 89 --hole k2 # 换 k2
bash ~/ros2/scripts/run_all.sh --arm 89 --skip-stage1 # 臂已在工件上方，跳过跟踪
# 旋入不写在这条命令里：
bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --screw
# 若这一次就要拧入（一般留给后续阶段）：
bash ~/ros2/scripts/run_all.sh --arm 89 --depth 6 --pitch 1.0
```
`run_pipeline.sh` 是 `run_all.sh` 的别名。
@@ -84,9 +85,9 @@ bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --screw
1. `[pipeline] === stage1_track ===` → 等跟踪锁定
2. `一阶段已锁定 — 停跟踪、钉住当前关节，直接进二阶段`
3. `[pipeline] === stage2_locate ===` → 降到孔悬停高，拍照对准
对孔结束后臂停在当前高度。搜孔轻压不再接着跑。
4. `[pipeline] === stage3_search ===` → `触面` → `★ 入孔` → `轻压到位`（不拧入）
急停：终端 3 Ctrl-C。失败时臂保持当前位姿，看
急停：终端 3 Ctrl-C。搜孔中会先抬 5mm。失败时臂保持当前位姿，看
`/tmp/pipeline_stage1.log`、`/tmp/stage2_result.json`、`/tmp/stage3_result.json`。
高度接口：`robots[89].servo.ee_z_m`（跟踪锁高）、
@@ -148,10 +149,7 @@ bash ~/ros2/scripts/run_all.sh --arm 89 --final-gap 8 # 锁 XY，规
| `/stage2/result` | `std_msgs/String` JSON：k1/k2 基座坐标、残差、悬停高 |
| `/stage2/overlay` | `sensor_msgs/Image` 标注图（同时落盘 /tmp/stage2_overlay.png） |
## 旋转装配（原三阶段节点；搜孔轻压入口已取消）
不带 `--screw` 时 `run_stage3_search.sh` 直接退出，不会触面，也不会螺旋搜孔。
下面是这个节点在旋入时仍会走的力控步骤，不是一条还能单独打开的搜孔命令。
## 三阶段柔顺搜孔（stage3，Orisys 触觉 + 力闭环螺旋）
前置：二阶段刚跑完（臂悬停在目标孔上方、垂直朝下、夹持通止规）。流程：
@@ -169,9 +167,12 @@ bash ~/ros2/scripts/run_all.sh --arm 89 --final-gap 8 # 锁 XY，规
7. **hold** — 锁定关节，发布结果
```bash
# 前置：T1 run_real.sh --arm 89；对孔已完成（臂保持不动）
# 不带 --screw 会直接退出，不会搜孔。
bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --screw
# 前置：T1 run_real.sh --arm 89；二阶段已完成（臂保持不动）
bash ~/ros2/scripts/run_stage3_search.sh --arm 89 # 搜孔+落座
bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --depth 0 # 只搜孔定心，不插入
bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --depth 6 --pitch 1.0 # M6 螺纹规旋入
bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --r-max 5 # 临时改搜索半径
bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --resnap # 搜前用 D405 复拍对准一次
```
**力阈值是「相对静止均值的增量」，不用标定牛顿**：Orisys 读数是形变任意单位且
handeye/poses/89_eye_in_hand.yaml
+33 -23
View File
@@ -8,49 +8,59 @@
/**:
ros__parameters:
num_poses: 21
# 2026-09-02 通过 handeye_teach 示教（RDK 自由驱动）
num_poses: 26
# 2026-10-16 重新示教 26 点（旧 21 点在 poses/backup/）；A5 11×8，tool10120938
waypoint_positions: [
# pose 0
-0.928421, -1.237477, 1.528823, 1.526334, -1.230146, 0.098562, 0.538531,
-0.211261, -0.571428, 0.247301, 1.552793, -0.119748, 0.614676, -0.023410,
# pose 1
-0.970203, -1.315554, 1.553230, 1.669383, -1.407869, 0.125907, 0.578173,
-0.169531, -0.647538, 0.404777, 1.568624, -0.268686, 0.700171, 0.395947,
# pose 2
-0.967819, -1.525572, 1.706057, 1.357062, -1.211051, 0.023232, 0.193541,
-0.759204, -0.846289, 0.735872, 1.387307, -0.539553, 0.576374, -0.060241,
# pose 3
-1.065106, -1.624218, 1.793644, 1.279402, -1.120929, 0.019151, 0.139658,
-0.925104, -1.394748, 1.532423, 0.826872, -0.686369, -0.074315, -0.145523,
# pose 4
-0.386279, -1.551838, 1.563578, 1.552444, -1.775337, -0.341974, 0.957445,
-1.004515, -1.491910, 1.596406, 1.245659, -0.864236, 0.212990, 0.460589,
# pose 5
-0.227779, -1.219414, 1.026882, 0.828801, -0.916809, -0.166985, 0.034154,
-0.373052, -1.441561, 1.624783, 0.325602, -1.361228, -0.404116, 0.511430,
# pose 6
-0.324968, -1.273082, 1.092344, 0.723132, -0.922095, -0.206413, 0.152082,
-0.281212, -1.452318, 1.627866, 0.517637, -1.412135, -0.420614, 0.217023,
# pose 7
-0.421966, -1.309101, 1.143065, 0.606456, -0.906673, -0.219282, 0.237953,
-0.184868, -1.473244, 1.670229, 0.827772, -1.522330, -0.529014, -0.212158,
# pose 8
-0.851598, -1.260600, 1.302895, 1.127033, -0.969809, 0.032856, 0.642673,
-0.246577, -1.532828, 1.679171, 1.204498, -1.697638, -0.607779, -0.517785,
# pose 9
-0.951418, -0.550124, 0.848598, 2.139787, -1.025778, 1.184418, 0.598394,
-0.463435, -1.549157, 1.650964, 1.948746, -1.841124, -0.638016, -0.842437,
# pose 10
-0.834498, -0.635767, 0.970038, 2.058410, -1.035138, 1.032040, 0.738619,
-1.573539, -0.359801, 1.261534, 2.423352, -0.405671, 1.620875, -0.045014,
# pose 11
-0.519528, -0.883321, 1.135035, 1.830274, -1.100901, 0.234065, -0.069068,
-1.549088, -0.710364, 1.347227, 2.111776, -0.606133, 1.116345, -0.567788,
# pose 12
-0.347358, -1.081426, 1.192233, 1.514557, -1.244594, -0.116668, -0.564362,
-1.512218, -0.983202, 1.445454, 1.808168, -0.602205, 0.699420, -0.803771,
# pose 13
-0.364832, -1.102689, 1.224844, 1.520320, -1.265760, -0.136855, -0.582459,
-1.023248, -1.342205, 1.465909, 1.092911, -0.843418, 0.099672, 1.287399,
# pose 14
-1.130796, -1.316186, 1.711089, 1.821409, -1.278003, 0.010885, -1.337210,
-0.870257, -1.473283, 1.657987, 0.831271, -1.034030, -0.144608, 1.263203,
# pose 15
-0.732222, -1.376363, 1.812339, 1.293854, -1.410826, -0.226771, -0.217230,
-0.384368, -1.417984, 1.403442, 2.075614, -1.952106, -0.394139, 1.022592,
# pose 16
-0.885017, -1.493416, 1.894070, 1.379021, -1.520015, -0.257115, 1.025331,
-0.320712, -1.513967, 1.508547, 1.652983, -1.854378, -0.409816, 1.024834,
# pose 17
-0.911769, -1.532695, 1.944123, 1.427787, -1.560513, -0.267425, 1.004398,
-0.046066, -1.630857, 1.639221, 1.105078, -1.971509, -0.601654, 1.076452,
# pose 18
-1.436271, -1.768595, 1.559397, 2.244717, -2.057112, 0.341785, 0.749883,
-0.183628, -1.588663, 1.989621, 0.918793, -1.873979, -0.629910, 0.092012,
# pose 19
-1.511828, -1.759337, 1.620075, 2.131574, -1.946595, 0.328227, 0.584657,
-1.300709, -1.159500, 1.611790, 1.761984, -0.963027, 0.391619, -0.468980,
# pose 20
-1.180482, -1.643241, 1.865851, 1.597699, -1.464982, 0.097605, -0.217780,
-0.750548, -1.221231, 1.566283, 1.128755, -1.163176, -0.102841, 0.958420,
# pose 21
-0.874185, -1.297230, 1.655940, 1.303348, -1.334815, -0.047425, 2.047690,
# pose 22
-0.671314, -1.510973, 1.813908, 1.235398, -1.778996, -0.462958, 2.899902,
# pose 23
-0.710795, -1.522124, 1.653705, 1.316372, -1.574330, 0.086268, 1.453877,
# pose 24
-0.533642, -1.545775, 1.837151, 1.467081, -1.463448, -0.161183, 0.387563,
# pose 25
-0.850451, -1.523689, 1.982673, 1.082314, -1.771622, -0.354609, 0.583659,
]
handeye/poses/89_eye_to_hand.yaml
+17 -27
View File
@@ -7,48 +7,38 @@
/**:
ros__parameters:
num_poses: 21
num_poses: 16
waypoint_positions: [
# pose 0
-0.798667, -0.611079, 0.054912, 2.246988, 0.049199, 1.389219, 0.196135,
-0.850450, -1.523690, 1.982676, 1.082314, -1.771616, -0.354603, 0.583665,
# pose 1
-0.787304, -0.531383, 0.240108, 2.183440, 0.367026, 1.370311, 0.472489,
-2.098137, -0.606148, 1.213612, 2.290328, -0.795715, 0.618633, 0.213293,
# pose 2
-1.615896, -0.687070, 1.187600, 2.243693, -0.162789, 1.650045, 1.026121,
-2.115870, -0.711154, 1.204674, 2.128285, -0.937034, 0.514766, -0.356714,
# pose 3
-1.787431, -0.668004, 1.408195, 2.025431, -0.064718, 0.565415, 1.324635,
-2.017820, -0.798105, 1.516945, 2.002096, -0.392371, -0.035888, 0.657789,
# pose 4
-2.024343, -0.863793, 1.349640, 2.359469, -0.720629, 0.084467, 0.821412,
-2.021874, -0.805058, 1.533201, 2.001407, -0.399671, -0.045924, 0.653138,
# pose 5
-1.884100, -1.117584, 1.183093, 2.127937, -1.258013, 0.151934, 0.985555,
-2.135665, -1.020864, 1.346306, 2.334297, -1.268378, 0.406336, 0.897832,
# pose 6
-1.866465, -1.194137, 1.143263, 1.872811, -1.320028, 0.280719, 0.777663,
-1.793944, -1.143422, 1.244557, 1.942177, -1.315611, 0.473616, 1.619695,
# pose 7
-1.729402, -1.139685, 1.127574, 1.913544, -1.329630, -0.194162, 1.383834,
-2.075950, -1.215149, 1.322761, 2.142359, -1.372597, 0.398777, 1.008202,
# pose 8
-1.611937, -1.158158, 1.064049, 1.918313, -1.480676, -0.392994, 1.833894,
-2.153500, -1.215892, 1.345656, 2.306931, -1.412319, 0.415145, 0.997235,
# pose 9
-2.252978, -1.387014, 1.686186, 2.101298, -1.333349, -0.111994, 0.036817,
-2.127777, -1.256654, 1.370562, 2.009440, -1.369584, 0.219094, -0.132912,
# pose 10
-2.179722, -1.297659, 1.497171, 2.167675, -1.272158, 0.100548, 0.673603,
-2.138686, -1.270867, 1.591854, 2.150744, -0.898729, 0.079835, 0.840791,
# pose 11
-1.921897, -1.543024, 1.000863, 1.897273, -1.455972, 1.437388, -0.140968,
-2.063605, -1.300562, 1.582835, 2.080463, -0.904827, 0.243915, 1.110180,
# pose 12
-1.884059, -1.063347, 1.439248, 2.253200, -1.164963, -0.423306, 0.907197,
-1.867772, -1.316320, 1.376713, 1.888199, -1.274029, 0.284609, 1.405712,
# pose 13
-1.987022, -1.235459, 1.354108, 2.370862, -1.354044, 0.065378, 0.923079,
-1.829092, -1.355347, 1.315742, 1.808403, -1.413826, 0.338349, 1.459711,
# pose 14
-1.770869, -1.333861, 1.241996, 1.800723, -1.395819, -0.255590, 1.389660,
-2.192461, -1.374082, 1.330037, 2.154137, -1.528020, 0.279283, 0.657702,
# pose 15
-1.761759, -1.369860, 1.213137, 1.791639, -1.467956, -0.170537, 1.473557,
# pose 16
-1.504294, -1.346227, 1.206550, 1.660186, -1.320782, -0.393676, 1.657215,
# pose 17
-1.846546, -1.476930, 0.946255, 1.910615, -2.435429, 1.535168, 2.151544,
# pose 18
-1.700067, -1.452760, 0.909035, 1.758222, -2.103930, 1.445249, 2.174743,
# pose 19
-2.173713, -0.983647, 1.399849, 2.384939, -1.379509, -0.216471, 0.253561,
# pose 20
-2.160187, -0.967904, 1.376288, 2.451713, -1.393524, -0.212278, 0.473606,
-2.137444, -1.468886, 1.525289, 2.348419, -1.025385, 1.011855, 1.148855,
]
handeye/results/89_d435_eye_to_hand.yaml
+9 -9
View File
@@ -1,22 +1,22 @@
# 手眼标定结果
# mode: eye_to_hand
# camera_source: d435
# views: 24
# utc: 2026-10-12T05:20:30.137123+00:00
# views: 15
# utc: 2026-10-16T03:02:49.214344+00:00
# board: 11x8 square_m=0.02
# gripper_tool: tool10120938
# T_4x4 = cam→基座（AX=XB 的手是 TCP，不折到法兰）
# 旧结果备份在 results/backup/
gripper_tool: tool10120938
T_flange_to_tool:
- [1.00000000, 0.00000000, 0.00000000, -0.00050000]
- [0.00000000, 1.00000000, 0.00000000, 0.00670000]
- [0.00000000, 0.00000000, 1.00000000, 0.22450000]
- [1.00000000, 0.00000000, 0.00000000, 0.00075900]
- [0.00000000, 1.00000000, 0.00000000, -0.00008800]
- [0.00000000, 0.00000000, 1.00000000, 0.22627400]
- [0.00000000, 0.00000000, 0.00000000, 1.00000000]
# T_cam_to_base (eye_to_hand)
translation_m: [0.37094529, -0.52805928, 0.73745585]
translation_m: [0.38078822, -0.52208142, 0.70359964]
T_4x4:
- [-0.00990083, 0.99909706, -0.04131632, 0.37094529]
- [0.99991380, 0.01024832, 0.00820703, -0.52805928]
- [0.00862304, -0.04123150, -0.99911241, 0.73745585]
- [-0.03086969, 0.99833255, -0.04877684, 0.38078822]
- [0.99951823, 0.03067553, -0.00472440, -0.52208142]
- [-0.00322027, -0.04889918, -0.99879853, 0.70359964]
- [0.00000000, 0.00000000, 0.00000000, 1.00000000]
handeye/results/89_eye_in_hand.yaml
+12 -13
View File
@@ -1,28 +1,27 @@
# 手眼标定结果
# mode: eye_in_hand
# camera_source: d405_left
# views: 21
# utc: 2026-10-12T03:29:51.243487+00:00
# views: 25
# utc: 2026-10-16T02:29:07.572249+00:00
# board: 11x8 square_m=0.015
# gripper_tool: tool10120938
# T_4x4 = cam→法兰（AX=XB 在 TCP 上解，再左乘 T_flange_to_tool）
# T_cam_to_tool 是原始求解；旧的法兰/tool1 备份在 results/backup/
# 2026-10-12 第一次写出的 T_4x4 乘序反了（tz≈-296 mm），已按同一份 T_cam_to_tool 重折。
gripper_tool: tool10120938
T_flange_to_tool:
- [1.00000000, 0.00000000, 0.00000000, -0.00050000]
- [0.00000000, 1.00000000, 0.00000000, 0.00670000]
- [0.00000000, 0.00000000, 1.00000000, 0.22450000]
- [1.00000000, 0.00000000, 0.00000000, 0.00075900]
- [0.00000000, 1.00000000, 0.00000000, -0.00008800]
- [0.00000000, 0.00000000, 1.00000000, 0.22627400]
- [0.00000000, 0.00000000, 0.00000000, 1.00000000]
T_cam_to_tool:
- [-0.03435184, -0.86951953, 0.49270249, -0.05961995]
- [0.99932767, -0.03620511, 0.00577974, -0.01523625]
- [0.01281276, 0.49256977, 0.87017863, -0.09761517]
- [0.01117708, -0.86715374, 0.49791511, -0.06192365]
- [0.99992647, 0.01203574, -0.00148504, -0.00762539]
- [-0.00470501, 0.49789509, 0.86722450, -0.09779711]
- [0.00000000, 0.00000000, 0.00000000, 1.00000000]
# T_cam_to_flange (eye_in_hand，由工具 TCP 折算)
translation_m: [-0.06011995, -0.00853625, 0.12688483]
translation_m: [-0.06116465, -0.00771339, 0.12847689]
T_4x4:
- [-0.03435184, -0.86951953, 0.49270249, -0.06011995]
- [0.99932767, -0.03620511, 0.00577974, -0.00853625]
- [0.01281276, 0.49256977, 0.87017863, 0.12688483]
- [0.01117708, -0.86715374, 0.49791511, -0.06116465]
- [0.99992647, 0.01203574, -0.00148504, -0.00771339]
- [-0.00470501, 0.49789509, 0.86722450, 0.12847689]
- [0.00000000, 0.00000000, 0.00000000, 1.00000000]
install/arm_application/lib/arm_application/assembly_pipeline
+4 -4
View File
@@ -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
+4 -4
View File
@@ -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
+4 -4
View File
@@ -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
+4 -4
View File
@@ -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
+4 -4
View File
@@ -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
+4 -4
View File
@@ -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
+4 -4
View File
@@ -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
+4 -4
View File
@@ -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
+4 -4
View File
@@ -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
+4 -4
View File
@@ -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
+4 -4
View File
@@ -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
+4 -4
View File
@@ -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
+4 -4
View File
@@ -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
+4 -4
View File
@@ -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
+4 -4
View File
@@ -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
+4 -4
View File
@@ -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
+4 -4
View File
@@ -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
+4 -4
View File
@@ -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
+4 -4
View File
@@ -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
+4 -4
View File
@@ -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
+4 -4
View File
@@ -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
+4 -4
View File
@@ -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
+59 -4
View File
@@ -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
+1 -3
View File
@@ -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
+5 -16
View File
@@ -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_app.py
+1765 -355
View File
File diff suppressed because it is too large Load Diff
install/arm_application/lib/python3.11/site-packages/arm_application/operator_arm_mesh.py
+339
View File
@@ -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
+595
View File
@@ -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
+541 -92
View File
@@ -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),
self._color(name, role),
qos_profile_sensor_data,
)
self.create_subscription(
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 _color(self, name: str):
def _cb(msg: Image) -> None:
arr = _color_array(msg)
if arr is None:
# 这里只留最新原图。颜色转换和画框放到画图线程，避免把后面的帧堵住。
try:
view = _color_view(msg)
if view is None:
return
with lock:
latest[f"{name}_color"] = arr
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:
try:
view = _depth_view(msg)
if view is None:
return
with lock:
latest[f"{name}_depth"] = arr
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
_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
+302 -60
View File
@@ -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,22 +698,60 @@ 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(
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,
)
_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}
detail = ((proc.stdout or "") + (proc.stderr or "")).strip()
finally:
_forget(action)
detail = (detail or "").strip()
log_path.write_text(detail)
ok = proc.returncode == 0
return {
@@ -569,8 +768,16 @@ def run_action(action: str, arm: str, confirm: bool, mode: str = "eye_in_hand")
stderr=subprocess.STDOUT,
start_new_session=True,
)
_PROCS[action] = proc.pid
_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)
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}
detail = ((proc.stdout or "") + (proc.stderr or "")).strip()
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
+138
View File
@@ -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
+436
View File
@@ -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
+91
View File
@@ -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
+103 -9
View File
@@ -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
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)}
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)}
return {
"ok": True,
"message": "软拖拽已开启。轻推机械臂，再记点。这一步要先停掉控制栈。",
}
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
+22 -6
View File
@@ -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
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
+7 -13
View File
@@ -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
+56 -1
View File
@@ -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,6 +400,16 @@ class AssemblyPipeline(Stage3Search):
if code != 0:
self.get_logger().error(f"二阶段失败 exit={code}，停在当前位姿")
return code
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"
+ (
@@ -373,14 +420,22 @@ class AssemblyPipeline(Stage3Search):
)
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
+80 -22
View File
@@ -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,9 +313,12 @@ class RealsenseColorPub(TolerantNode):
self._info_pub.publish(info)
if depth_u16 is not None:
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
dmsg.header.stamp = stamp or self.get_clock().now().to_msg()
dmsg.header.frame_id = self._frame_id
dmsg.height = dh
dmsg.width = dw
@@ -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()
else:
self.get_logger().warn(f'整理帧：{exc}', throttle_duration_sec=2.0)
continue
if misses == 1:
self.get_logger().warn(f'wait_for_frames：{exc}')
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
+56 -26
View File
@@ -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
+1 -75
View File
@@ -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
+42 -6
View File
@@ -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,9 +3406,31 @@ 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)
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（不接触）。"
install/arm_application/lib/python3.11/site-packages/arm_application/stage3_search_node.py
+2 -1
View File
@@ -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
+1 -1
View File
@@ -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
-1
View File
@@ -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
View File
@@ -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
install/flexiv_hardware/include/flexiv_hardware/flexiv_dual_hardware_interface.hpp
-139
View File
@@ -1,139 +0,0 @@
/**
* @file flexiv_dual_hardware_interface.hpp
* @brief Hardware interface to a pair of Flexiv robots for ROS 2 control.
* @copyright Copyright (C) 2016-2025 Flexiv Ltd. All Rights Reserved.
* @author Flexiv
*/
#ifndef FLEXIV_HARDWARE__FLEXIV_DUAL_HARDWARE_INTERFACE_HPP_
#define FLEXIV_HARDWARE__FLEXIV_DUAL_HARDWARE_INTERFACE_HPP_
#include <memory>
#include <string>
#include <vector>
#include <map>
// ROS
#include <rclcpp/clock.hpp>
#include <rclcpp/duration.hpp>
#include <rclcpp/macros.hpp>
#include <rclcpp/logger.hpp>
#include <rclcpp/time.hpp>
#include <rclcpp_lifecycle/state.hpp>
// ros2_control hardware_interface
#include <hardware_interface/hardware_info.hpp>
#include <hardware_interface/system_interface.hpp>
#include <hardware_interface/types/hardware_interface_return_values.hpp>
#include "flexiv_hardware/visibility_control.h"
// Flexiv
#include "flexiv/drdk/robot_pair.hpp"
namespace flexiv_hardware {
enum StoppingInterface
{
NONE,
STOP_POSITION,
STOP_VELOCITY,
STOP_EFFORT
};
class FlexivDualHardwareInterface : public hardware_interface::SystemInterface
{
public:
RCLCPP_SHARED_PTR_DEFINITIONS(FlexivDualHardwareInterface)
FLEXIV_HARDWARE_PUBLIC
hardware_interface::CallbackReturn on_init(
const hardware_interface::HardwareInfo& info) override;
FLEXIV_HARDWARE_PUBLIC
std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
FLEXIV_HARDWARE_PUBLIC
std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
FLEXIV_HARDWARE_PUBLIC
hardware_interface::return_type prepare_command_mode_switch(
const std::vector<std::string>& start_interfaces,
const std::vector<std::string>& stop_interfaces) override;
FLEXIV_HARDWARE_PUBLIC
hardware_interface::return_type perform_command_mode_switch(
const std::vector<std::string>& start_interfaces,
const std::vector<std::string>& stop_interfaces) override;
FLEXIV_HARDWARE_PUBLIC
hardware_interface::CallbackReturn on_activate(
const rclcpp_lifecycle::State& previous_state) override;
FLEXIV_HARDWARE_PUBLIC
hardware_interface::CallbackReturn on_deactivate(
const rclcpp_lifecycle::State& previous_state) override;
FLEXIV_HARDWARE_PUBLIC
hardware_interface::return_type read(
const rclcpp::Time& time, const rclcpp::Duration& period) override;
FLEXIV_HARDWARE_PUBLIC
hardware_interface::return_type write(
const rclcpp::Time& time, const rclcpp::Duration& period) override;
private:
// Flexiv DRDK
std::unique_ptr<flexiv::drdk::RobotPair> robot_pair_;
// RDK control mode for joint position and velocity interfaces
flexiv::rdk::Mode rdk_control_mode_;
// External axis type
std::string external_axis_type_ = "";
// Joint commands
std::vector<double> hw_commands_joint_positions_;
std::vector<double> hw_commands_joint_velocities_;
std::vector<double> hw_commands_joint_efforts_;
// Joint states
std::vector<double> hw_states_joint_positions_;
std::vector<double> hw_states_joint_velocities_;
std::vector<double> hw_states_joint_efforts_;
// Robot States
flexiv::rdk::RobotStates hw_flexiv_robot_states_left_;
flexiv::rdk::RobotStates hw_flexiv_robot_states_right_;
flexiv::rdk::RobotStates* hw_flexiv_robot_states_addr_left_ = &hw_flexiv_robot_states_left_;
flexiv::rdk::RobotStates* hw_flexiv_robot_states_addr_right_ = &hw_flexiv_robot_states_right_;
// GPIO commands and states
std::vector<double> hw_commands_gpio_out_;
std::vector<double> hw_states_gpio_in_;
// Current digital output map
std::map<unsigned int, bool> current_digital_outputs_left_;
std::map<unsigned int, bool> current_digital_outputs_right_;
// Joint mapping
struct JointMap
{
int robot_index; // 0: Left, 1: Right
int dof_index; // Index in the robot's q vector
};
std::vector<JointMap> joint_map_;
static rclcpp::Logger getLogger();
// Control modes
bool controllers_initialized_;
std::vector<uint> stop_modes_;
std::vector<std::string> start_modes_;
bool position_controller_running_;
bool velocity_controller_running_;
bool torque_controller_running_;
};
} /* namespace flexiv_hardware */
#endif /* FLEXIV_HARDWARE__FLEXIV_DUAL_HARDWARE_INTERFACE_HPP_ */
install/flexiv_hardware/include/flexiv_hardware/flexiv_dual_hardware_interface.hpp
Symlink
+1
View File
@@ -0,0 +1 @@
/home/cc/ros2/src/flexiv_hardware/include/flexiv_hardware/flexiv_dual_hardware_interface.hpp
install/flexiv_hardware/include/flexiv_hardware/flexiv_hardware_interface.hpp
-156
View File
@@ -1,156 +0,0 @@
/**
* @file flexiv_hardware_interface.hpp
* @brief Hardware interface to Flexiv robots for ROS 2 control. Adapted from
* ros2_control_demos/example_3/hardware/include/ros2_control_demo_example_3/rrbot_system_multi_interface.hpp
* @copyright Copyright (C) 2016-2024 Flexiv Ltd. All Rights Reserved.
* @author Flexiv
*/
#ifndef FLEXIV_HARDWARE__FLEXIV_HARDWARE_INTERFACE_HPP_
#define FLEXIV_HARDWARE__FLEXIV_HARDWARE_INTERFACE_HPP_
#include <chrono>
#include <memory>
#include <string>
#include <vector>
// ROS
#include <rclcpp/clock.hpp>
#include <rclcpp/duration.hpp>
#include <rclcpp/macros.hpp>
#include <rclcpp/logger.hpp>
#include <rclcpp/time.hpp>
#include <rclcpp_lifecycle/state.hpp>
// ros2_control hardware_interface
#include <hardware_interface/hardware_info.hpp>
#include <hardware_interface/system_interface.hpp>
#include <hardware_interface/types/hardware_interface_return_values.hpp>
#include "flexiv_hardware/visibility_control.h"
// Flexiv
#include "flexiv/rdk/robot.hpp"
namespace flexiv_hardware {
enum StoppingInterface
{
NONE,
STOP_POSITION,
STOP_VELOCITY,
STOP_EFFORT
};
class FlexivHardwareInterface : public hardware_interface::SystemInterface
{
public:
RCLCPP_SHARED_PTR_DEFINITIONS(FlexivHardwareInterface)
FLEXIV_HARDWARE_PUBLIC
hardware_interface::CallbackReturn on_init(
const hardware_interface::HardwareInfo& info) override;
FLEXIV_HARDWARE_PUBLIC
std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
FLEXIV_HARDWARE_PUBLIC
std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
FLEXIV_HARDWARE_PUBLIC
hardware_interface::return_type prepare_command_mode_switch(
const std::vector<std::string>& start_interfaces,
const std::vector<std::string>& stop_interfaces) override;
FLEXIV_HARDWARE_PUBLIC
hardware_interface::return_type perform_command_mode_switch(
const std::vector<std::string>& start_interfaces,
const std::vector<std::string>& stop_interfaces) override;
FLEXIV_HARDWARE_PUBLIC
hardware_interface::CallbackReturn on_activate(
const rclcpp_lifecycle::State& previous_state) override;
FLEXIV_HARDWARE_PUBLIC
hardware_interface::CallbackReturn on_deactivate(
const rclcpp_lifecycle::State& previous_state) override;
FLEXIV_HARDWARE_PUBLIC
hardware_interface::return_type read(
const rclcpp::Time& time, const rclcpp::Duration& period) override;
FLEXIV_HARDWARE_PUBLIC
hardware_interface::return_type write(
const rclcpp::Time& time, const rclcpp::Duration& period) override;
private:
// Flexiv RDK
std::unique_ptr<flexiv::rdk::Robot> robot_;
// RDK control mode for joint position and velocity interfaces
flexiv::rdk::Mode rdk_control_mode_;
// Joint commands
std::vector<double> hw_commands_joint_positions_;
std::vector<double> hw_commands_joint_velocities_;
std::vector<double> hw_commands_joint_efforts_;
// Joint states
std::vector<double> hw_states_joint_positions_;
std::vector<double> hw_states_joint_velocities_;
std::vector<double> hw_states_joint_efforts_;
// Robot States
flexiv::rdk::RobotStates hw_flexiv_robot_states_;
flexiv::rdk::RobotStates* hw_flexiv_robot_states_addr_ = &hw_flexiv_robot_states_;
// GPIO commands and states
std::vector<double> hw_commands_gpio_out_;
std::vector<double> hw_states_gpio_in_;
// Map from RDK joint index to ROS joint index
// RDK expects: [ext_axis_1, ..., ext_axis_N, arm_joint_1, ..., arm_joint_7]
std::vector<size_t> rdk_to_ros_map_;
// Current digital output map
std::map<unsigned int, bool> current_digital_outputs_;
static rclcpp::Logger getLogger();
// Control modes
bool controllers_initialized_;
std::vector<uint> stop_modes_;
std::vector<std::string> start_modes_;
bool position_controller_running_;
bool velocity_controller_running_;
bool torque_controller_running_;
// ===== 连接存活看门狗（2026-09-28 真机踩到）=====
//
// 现象：RDK 断连后 `operational()` 会**一直保持 true**，于是 read() 照跑却只
// 返回最后一帧缓存（关节状态永久冻结），write() 把命令发进虚空也不抛异常，
// 而 rizon_arm_controller 是本地插值、不校验反馈 —— 最终表现为
// **「控制器报 Goal reached/success、关节状态纹丝不动、臂一步没动」**。
//
// 实测：14:53:44 RDK 打印 "Disconnected from the robot, waiting for reconnection"，
// 此后 50 分钟里 /joint_states 的 robot_timestamp 推进量恒为 0，而 `op=1` 没变过。
// 根因是两台臂出厂 MAC 相同（88:88:88:88:87:88）在同一 L2 段抢转发，链路丢包 40~53%。
//
// 这里用 connected() + states().timestamp 双重判活：任一失效就返回 ERROR，
// 让 ros2_control 把栈停下来报错，而不是静默地"成功"。
int64_t last_state_stamp_ns_ = 0; // 上一帧的机器人时间戳
int64_t last_state_change_ns_ = 0; // 上一帧时间戳发生变化的本地时刻
// D435 粗定位的 MoveL 走已经连上的这份 RDK。write() 里一步一步推进，
// 不另开线程，也不在这段里往关节接口灌点。请求文件是 /tmp/flexiv_movel_req。
enum class VendorMovelPhase { kIdle, kRunning };
VendorMovelPhase vendor_movel_phase_ = VendorMovelPhase::kIdle;
long vendor_movel_seq_ = -1;
long vendor_movel_done_seq_ = -1;
std::chrono::steady_clock::time_point vendor_movel_deadline_ {};
bool poll_vendor_movel();
void finish_vendor_movel(long seq, bool ok, const std::string& detail);
};
} /* namespace flexiv_hardware */
#endif /* FLEXIV_HARDWARE__FLEXIV_HARDWARE_INTERFACE_HPP_ */
install/flexiv_hardware/include/flexiv_hardware/flexiv_hardware_interface.hpp
Symlink
+1
View File
@@ -0,0 +1 @@
/home/cc/ros2/src/flexiv_hardware/include/flexiv_hardware/flexiv_hardware_interface.hpp
install/flexiv_hardware/include/flexiv_hardware/visibility_control.h
-35
View File
@@ -1,35 +0,0 @@
#ifndef FLEXIV_HARDWARE__VISIBILITY_CONTROL_H_
#define FLEXIV_HARDWARE__VISIBILITY_CONTROL_H_
// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
// https://gcc.gnu.org/wiki/Visibility
#if defined _WIN32 || defined __CYGWIN__
#ifdef __GNUC__
#define FLEXIV_HARDWARE_EXPORT __attribute__((dllexport))
#define FLEXIV_HARDWARE_IMPORT __attribute__((dllimport))
#else
#define FLEXIV_HARDWARE_EXPORT __declspec(dllexport)
#define FLEXIV_HARDWARE_IMPORT __declspec(dllimport)
#endif
#ifdef FLEXIV_HARDWARE_BUILDING_DLL
#define FLEXIV_HARDWARE_PUBLIC FLEXIV_HARDWARE_EXPORT
#else
#define FLEXIV_HARDWARE_PUBLIC FLEXIV_HARDWARE_IMPORT
#endif
#define FLEXIV_HARDWARE_PUBLIC_TYPE FLEXIV_HARDWARE_PUBLIC
#define FLEXIV_HARDWARE_LOCAL
#else
#define FLEXIV_HARDWARE_EXPORT __attribute__((visibility("default")))
#define FLEXIV_HARDWARE_IMPORT
#if __GNUC__ >= 4
#define FLEXIV_HARDWARE_PUBLIC __attribute__((visibility("default")))
#define FLEXIV_HARDWARE_LOCAL __attribute__((visibility("hidden")))
#else
#define FLEXIV_HARDWARE_PUBLIC
#define FLEXIV_HARDWARE_LOCAL
#endif
#define FLEXIV_HARDWARE_PUBLIC_TYPE
#endif
#endif // FLEXIV_HARDWARE__VISIBILITY_CONTROL_H_
install/flexiv_hardware/include/flexiv_hardware/visibility_control.h
Symlink
+1
View File
@@ -0,0 +1 @@
/home/cc/ros2/src/flexiv_hardware/include/flexiv_hardware/visibility_control.h
install/flexiv_hardware/lib/libflexiv_hardware.so
BIN
View File
Binary file not shown.
install/flexiv_hardware/lib/libflexiv_hardware.so
Symlink
+1
View File
@@ -0,0 +1 @@
/home/cc/ros2/build/flexiv_hardware/libflexiv_hardware.so
install/flexiv_hardware/share/ament_index/resource_index/hardware_interface__pluginlib__plugin/flexiv_hardware
-1
View File
@@ -1 +0,0 @@
share/flexiv_hardware/flexiv_hardware_interface_plugin.xml
install/flexiv_hardware/share/ament_index/resource_index/hardware_interface__pluginlib__plugin/flexiv_hardware
Symlink
+1
View File
@@ -0,0 +1 @@
/home/cc/ros2/build/flexiv_hardware/ament_cmake_index/share/ament_index/resource_index/hardware_interface__pluginlib__plugin/flexiv_hardware
install/flexiv_hardware/share/ament_index/resource_index/package_run_dependencies/flexiv_hardware
-1
View File
@@ -1 +0,0 @@
rclcpp;rclcpp_lifecycle;hardware_interface;pluginlib;ament_lint_auto;ament_lint_common;ament_cmake_gtest
install/flexiv_hardware/share/ament_index/resource_index/package_run_dependencies/flexiv_hardware
Symlink
+1
View File
@@ -0,0 +1 @@
/home/cc/ros2/build/flexiv_hardware/ament_cmake_index/share/ament_index/resource_index/package_run_dependencies/flexiv_hardware
install/flexiv_hardware/share/ament_index/resource_index/packages/flexiv_hardware
View File
install/flexiv_hardware/share/ament_index/resource_index/packages/flexiv_hardware
Symlink
+1
View File
@@ -0,0 +1 @@
/home/cc/ros2/build/flexiv_hardware/ament_cmake_index/share/ament_index/resource_index/packages/flexiv_hardware
install/flexiv_hardware/share/ament_index/resource_index/parent_prefix_path/flexiv_hardware
-1
View File
@@ -1 +0,0 @@
/home/cc/ros2/install/gpio_controller:/home/cc/ros2/install/flexiv_bringup:/home/cc/ros2/install/flexiv_test_nodes:/home/cc/ros2/install/flexiv_robot_states_broadcaster:/home/cc/ros2/install/flexiv_gripper:/home/cc/ros2/install/flexiv_msgs:/home/cc/ros2/install/flexiv_moveit_config:/home/cc/ros2/install/flexiv_hardware:/home/cc/ros2/install/arm_sim_gazebo:/home/cc/ros2/install/arm_bringup:/home/cc/ros2/install/arm_moveit_config:/home/cc/ros2/install/arm_description:/home/cc/ros2/install/flexiv_description:/home/cc/ros2/install/arm_driver_flexiv:/home/cc/ros2/install/arm_control:/home/cc/ros2/install/arm_application:/home/cc/ros2/install/arm_msgs:/home/cc/ros2/install/arm_kinematics:/home/cc/ros2/install/arm_interfaces:/home/cc/anaconda3/envs/ros_flexiv
install/flexiv_hardware/share/ament_index/resource_index/parent_prefix_path/flexiv_hardware
Symlink
+1
View File
@@ -0,0 +1 @@
/home/cc/ros2/build/flexiv_hardware/ament_cmake_index/share/ament_index/resource_index/parent_prefix_path/flexiv_hardware
install/flexiv_hardware/share/flexiv_hardware/cmake/ament_cmake_export_dependencies-extras.cmake
-94
View File
@@ -1,94 +0,0 @@
# generated from ament_cmake_export_dependencies/cmake/ament_cmake_export_dependencies-extras.cmake.in
set(_exported_dependencies "hardware_interface;pluginlib;rclcpp;rclcpp_lifecycle")
find_package(ament_cmake_libraries QUIET REQUIRED)
# find_package() all dependencies
# and append their DEFINITIONS INCLUDE_DIRS, LIBRARIES, and LINK_FLAGS
# variables to flexiv_hardware_DEFINITIONS, flexiv_hardware_INCLUDE_DIRS,
# flexiv_hardware_LIBRARIES, and flexiv_hardware_LINK_FLAGS.
# Additionally collect the direct dependency names in
# flexiv_hardware_DEPENDENCIES as well as the recursive dependency names
# in flexiv_hardware_RECURSIVE_DEPENDENCIES.
if(NOT _exported_dependencies STREQUAL "")
find_package(ament_cmake_core QUIET REQUIRED)
set(flexiv_hardware_DEPENDENCIES ${_exported_dependencies})
set(flexiv_hardware_RECURSIVE_DEPENDENCIES ${_exported_dependencies})
set(_libraries)
foreach(_dep ${_exported_dependencies})
if(NOT ${_dep}_FOUND)
find_package("${_dep}" QUIET REQUIRED)
endif()
# if a package provides modern CMake interface targets use them
# exclusively assuming the classic CMake variables only exist for
# backward compatibility
set(use_modern_cmake FALSE)
if(NOT "${${_dep}_TARGETS}" STREQUAL "")
foreach(_target ${${_dep}_TARGETS})
# only use actual targets
# in case a package uses this variable for other content
if(TARGET "${_target}")
get_target_property(_include_dirs ${_target} INTERFACE_INCLUDE_DIRECTORIES)
if(_include_dirs)
list_append_unique(flexiv_hardware_INCLUDE_DIRS "${_include_dirs}")
endif()
get_target_property(_imported_configurations ${_target} IMPORTED_CONFIGURATIONS)
if(_imported_configurations)
string(TOUPPER "${_imported_configurations}" _imported_configurations)
if(DEBUG_CONFIGURATIONS)
string(TOUPPER "${DEBUG_CONFIGURATIONS}" _debug_configurations_uppercase)
else()
set(_debug_configurations_uppercase "DEBUG")
endif()
foreach(_imported_config ${_imported_configurations})
get_target_property(_imported_implib ${_target} IMPORTED_IMPLIB_${_imported_config})
if(_imported_implib)
set(_imported_implib_config "optimized")
if(${_imported_config} IN_LIST _debug_configurations_uppercase)
set(_imported_implib_config "debug")
endif()
list(APPEND _libraries ${_imported_implib_config} ${_imported_implib})
else()
get_target_property(_imported_location ${_target} IMPORTED_LOCATION_${_imported_config})
if(_imported_location)
list(APPEND _libraries "${_imported_location}")
endif()
endif()
endforeach()
endif()
get_target_property(_link_libraries ${_target} INTERFACE_LINK_LIBRARIES)
if(_link_libraries)
list(APPEND _libraries "${_link_libraries}")
endif()
set(use_modern_cmake TRUE)
endif()
endforeach()
endif()
if(NOT use_modern_cmake)
if(${_dep}_DEFINITIONS)
list_append_unique(flexiv_hardware_DEFINITIONS "${${_dep}_DEFINITIONS}")
endif()
if(${_dep}_INCLUDE_DIRS)
list_append_unique(flexiv_hardware_INCLUDE_DIRS "${${_dep}_INCLUDE_DIRS}")
endif()
if(${_dep}_LIBRARIES)
list(APPEND _libraries "${${_dep}_LIBRARIES}")
endif()
if(${_dep}_LINK_FLAGS)
list_append_unique(flexiv_hardware_LINK_FLAGS "${${_dep}_LINK_FLAGS}")
endif()
if(${_dep}_RECURSIVE_DEPENDENCIES)
list_append_unique(flexiv_hardware_RECURSIVE_DEPENDENCIES "${${_dep}_RECURSIVE_DEPENDENCIES}")
endif()
endif()
if(_libraries)
ament_libraries_deduplicate(_libraries "${_libraries}")
list(APPEND _libraries ${flexiv_hardware_LIBRARIES})
list(REMOVE_DUPLICATES _libraries)
set(flexiv_hardware_LIBRARIES ${_libraries})
endif()
endforeach()
endif()
install/flexiv_hardware/share/flexiv_hardware/cmake/ament_cmake_export_dependencies-extras.cmake
Symlink
+1
View File
@@ -0,0 +1 @@
/home/cc/ros2/build/flexiv_hardware/ament_cmake_export_dependencies/ament_cmake_export_dependencies-extras.cmake
install/flexiv_hardware/share/flexiv_hardware/cmake/ament_cmake_export_include_directories-extras.cmake
-16
View File
@@ -1,16 +0,0 @@
# generated from ament_cmake_export_include_directories/cmake/ament_cmake_export_include_directories-extras.cmake.in
set(_exported_include_dirs "${flexiv_hardware_DIR}/../../../include")
# append include directories to flexiv_hardware_INCLUDE_DIRS
# warn about not existing paths
if(NOT _exported_include_dirs STREQUAL "")
find_package(ament_cmake_core QUIET REQUIRED)
foreach(_exported_include_dir ${_exported_include_dirs})
if(NOT IS_DIRECTORY "${_exported_include_dir}")
message(WARNING "Package 'flexiv_hardware' exports the include directory '${_exported_include_dir}' which doesn't exist")
endif()
normalize_path(_exported_include_dir "${_exported_include_dir}")
list(APPEND flexiv_hardware_INCLUDE_DIRS "${_exported_include_dir}")
endforeach()
endif()
install/flexiv_hardware/share/flexiv_hardware/cmake/ament_cmake_export_include_directories-extras.cmake
Symlink
+1
View File
@@ -0,0 +1 @@
/home/cc/ros2/build/flexiv_hardware/ament_cmake_export_include_directories/ament_cmake_export_include_directories-extras.cmake
install/flexiv_hardware/share/flexiv_hardware/cmake/ament_cmake_export_libraries-extras.cmake
-141
View File
@@ -1,141 +0,0 @@
# generated from ament_cmake_export_libraries/cmake/template/ament_cmake_export_libraries.cmake.in
set(_exported_libraries "flexiv_hardware")
set(_exported_library_names "")
# populate flexiv_hardware_LIBRARIES
if(NOT _exported_libraries STREQUAL "")
# loop over libraries, either target names or absolute paths
list(LENGTH _exported_libraries _length)
set(_i 0)
while(_i LESS _length)
list(GET _exported_libraries ${_i} _arg)
# pass linker flags along
if("${_arg}" MATCHES "^-" AND NOT "${_arg}" MATCHES "^-[l|framework]")
list(APPEND flexiv_hardware_LIBRARIES "${_arg}")
math(EXPR _i "${_i} + 1")
continue()
endif()
if("${_arg}" MATCHES "^(debug|optimized|general)$")
# remember build configuration keyword
# and get following library
set(_cfg "${_arg}")
math(EXPR _i "${_i} + 1")
if(_i EQUAL _length)
message(FATAL_ERROR "Package 'flexiv_hardware' passes the build configuration keyword '${_cfg}' as the last exported library")
endif()
list(GET _exported_libraries ${_i} _library)
else()
# the value is a library without a build configuration keyword
set(_cfg "")
set(_library "${_arg}")
endif()
math(EXPR _i "${_i} + 1")
if(NOT IS_ABSOLUTE "${_library}")
# search for library target relative to this CMake file
set(_lib "NOTFOUND")
find_library(
_lib NAMES "${_library}"
PATHS "${flexiv_hardware_DIR}/../../../lib"
NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH
)
if(NOT _lib)
# warn about not existing library and ignore it
message(FATAL_ERROR "Package 'flexiv_hardware' exports the library '${_library}' which couldn't be found")
elseif(NOT IS_ABSOLUTE "${_lib}")
# the found library must be an absolute path
message(FATAL_ERROR "Package 'flexiv_hardware' found the library '${_library}' at '${_lib}' which is not an absolute path")
elseif(NOT EXISTS "${_lib}")
# the found library must exist
message(FATAL_ERROR "Package 'flexiv_hardware' found the library '${_lib}' which doesn't exist")
else()
list(APPEND flexiv_hardware_LIBRARIES ${_cfg} "${_lib}")
endif()
else()
if(NOT EXISTS "${_library}")
# the found library must exist
message(WARNING "Package 'flexiv_hardware' exports the library '${_library}' which doesn't exist")
else()
list(APPEND flexiv_hardware_LIBRARIES ${_cfg} "${_library}")
endif()
endif()
endwhile()
endif()
# find_library() library names with optional LIBRARY_DIRS
# and add the libraries to flexiv_hardware_LIBRARIES
if(NOT _exported_library_names STREQUAL "")
# loop over library names
# but remember related build configuration keyword if available
list(LENGTH _exported_library_names _length)
set(_i 0)
while(_i LESS _length)
list(GET _exported_library_names ${_i} _arg)
# pass linker flags along
if("${_arg}" MATCHES "^-" AND NOT "${_arg}" MATCHES "^-[l|framework]")
list(APPEND flexiv_hardware_LIBRARIES "${_arg}")
math(EXPR _i "${_i} + 1")
continue()
endif()
if("${_arg}" MATCHES "^(debug|optimized|general)$")
# remember build configuration keyword
# and get following library name
set(_cfg "${_arg}")
math(EXPR _i "${_i} + 1")
if(_i EQUAL _length)
message(FATAL_ERROR "Package 'flexiv_hardware' passes the build configuration keyword '${_cfg}' as the last exported target")
endif()
list(GET _exported_library_names ${_i} _library)
else()
# the value is a library target without a build configuration keyword
set(_cfg "")
set(_library "${_arg}")
endif()
math(EXPR _i "${_i} + 1")
# extract optional LIBRARY_DIRS from library name
string(REPLACE ":" ";" _library_dirs "${_library}")
list(GET _library_dirs 0 _library_name)
list(REMOVE_AT _library_dirs 0)
set(_lib "NOTFOUND")
if(NOT _library_dirs)
# search for library in the common locations
find_library(
_lib
NAMES "${_library_name}"
)
if(NOT _lib)
# warn about not existing library and later ignore it
message(WARNING "Package 'flexiv_hardware' exports library '${_library_name}' which couldn't be found")
endif()
else()
# search for library in the specified directories
find_library(
_lib
NAMES "${_library_name}"
PATHS ${_library_dirs}
NO_DEFAULT_PATH NO_CMAKE_FIND_ROOT_PATH
)
if(NOT _lib)
# warn about not existing library and later ignore it
message(WARNING
"Package 'flexiv_hardware' exports library '${_library_name}' with LIBRARY_DIRS '${_library_dirs}' which couldn't be found")
endif()
endif()
if(_lib)
list(APPEND flexiv_hardware_LIBRARIES ${_cfg} "${_lib}")
endif()
endwhile()
endif()
# TODO(dirk-thomas) deduplicate flexiv_hardware_LIBRARIES
# while maintaining library order
# as well as build configuration keywords
# as well as linker flags
install/flexiv_hardware/share/flexiv_hardware/cmake/ament_cmake_export_libraries-extras.cmake
Symlink
+1
View File
@@ -0,0 +1 @@
/home/cc/ros2/build/flexiv_hardware/ament_cmake_export_libraries/ament_cmake_export_libraries-extras.cmake
install/flexiv_hardware/share/flexiv_hardware/cmake/flexiv_hardwareConfig-version.cmake
-14
View File
@@ -1,14 +0,0 @@
# generated from ament/cmake/core/templates/nameConfig-version.cmake.in
set(PACKAGE_VERSION "1.9.1")
set(PACKAGE_VERSION_EXACT False)
set(PACKAGE_VERSION_COMPATIBLE False)
if("${PACKAGE_FIND_VERSION}" VERSION_EQUAL "${PACKAGE_VERSION}")
set(PACKAGE_VERSION_EXACT True)
set(PACKAGE_VERSION_COMPATIBLE True)
endif()
if("${PACKAGE_FIND_VERSION}" VERSION_LESS "${PACKAGE_VERSION}")
set(PACKAGE_VERSION_COMPATIBLE True)
endif()
install/flexiv_hardware/share/flexiv_hardware/cmake/flexiv_hardwareConfig-version.cmake
Symlink
+1
View File
@@ -0,0 +1 @@
/home/cc/ros2/build/flexiv_hardware/ament_cmake_core/flexiv_hardwareConfig-version.cmake
install/flexiv_hardware/share/flexiv_hardware/cmake/flexiv_hardwareConfig.cmake
-42
View File
@@ -1,42 +0,0 @@
# generated from ament/cmake/core/templates/nameConfig.cmake.in
# prevent multiple inclusion
if(_flexiv_hardware_CONFIG_INCLUDED)
# ensure to keep the found flag the same
if(NOT DEFINED flexiv_hardware_FOUND)
# explicitly set it to FALSE, otherwise CMake will set it to TRUE
set(flexiv_hardware_FOUND FALSE)
elseif(NOT flexiv_hardware_FOUND)
# use separate condition to avoid uninitialized variable warning
set(flexiv_hardware_FOUND FALSE)
endif()
return()
endif()
set(_flexiv_hardware_CONFIG_INCLUDED TRUE)
# output package information
if(NOT flexiv_hardware_FIND_QUIETLY)
message(STATUS "Found flexiv_hardware: 1.9.1 (${flexiv_hardware_DIR})")
endif()
# warn when using a deprecated package
if(NOT "" STREQUAL "")
set(_msg "Package 'flexiv_hardware' is deprecated")
# append custom deprecation text if available
if(NOT "" STREQUAL "TRUE")
set(_msg "${_msg} ()")
endif()
# optionally quiet the deprecation message
if(NOT ${flexiv_hardware_DEPRECATED_QUIET})
message(DEPRECATION "${_msg}")
endif()
endif()
# flag package as ament-based to distinguish it after being find_package()-ed
set(flexiv_hardware_FOUND_AMENT_PACKAGE TRUE)
# include all config extra files
set(_extras "ament_cmake_export_include_directories-extras.cmake;ament_cmake_export_libraries-extras.cmake;ament_cmake_export_dependencies-extras.cmake")
foreach(_extra ${_extras})
include("${flexiv_hardware_DIR}/${_extra}")
endforeach()
install/flexiv_hardware/share/flexiv_hardware/cmake/flexiv_hardwareConfig.cmake
Symlink
+1
View File
@@ -0,0 +1 @@
/home/cc/ros2/build/flexiv_hardware/ament_cmake_core/flexiv_hardwareConfig.cmake
install/flexiv_hardware/share/flexiv_hardware/environment/ament_prefix_path.dsv
-1
View File
@@ -1 +0,0 @@
prepend-non-duplicate;AMENT_PREFIX_PATH;
install/flexiv_hardware/share/flexiv_hardware/environment/ament_prefix_path.dsv
Symlink
+1
View File
@@ -0,0 +1 @@
/home/cc/ros2/build/flexiv_hardware/ament_cmake_environment_hooks/ament_prefix_path.dsv
install/flexiv_hardware/share/flexiv_hardware/environment/ament_prefix_path.sh
-4
View File
@@ -1,4 +0,0 @@
# copied from
# ament_cmake_core/cmake/environment_hooks/environment/ament_prefix_path.sh
ament_prepend_unique_value AMENT_PREFIX_PATH "$AMENT_CURRENT_PREFIX"
install/flexiv_hardware/share/flexiv_hardware/environment/ament_prefix_path.sh
Symlink
+1
View File
@@ -0,0 +1 @@
/home/cc/anaconda3/envs/ros_flexiv/share/ament_cmake_core/cmake/environment_hooks/environment/ament_prefix_path.sh
install/flexiv_hardware/share/flexiv_hardware/environment/path.dsv
-1
View File
@@ -1 +0,0 @@
prepend-non-duplicate-if-exists;PATH;bin
install/flexiv_hardware/share/flexiv_hardware/environment/path.dsv
Symlink
+1
View File
@@ -0,0 +1 @@
/home/cc/ros2/build/flexiv_hardware/ament_cmake_environment_hooks/path.dsv
install/flexiv_hardware/share/flexiv_hardware/environment/path.sh
-5
View File
@@ -1,5 +0,0 @@
# copied from ament_cmake_core/cmake/environment_hooks/environment/path.sh
if [ -d "$AMENT_CURRENT_PREFIX/bin" ]; then
ament_prepend_unique_value PATH "$AMENT_CURRENT_PREFIX/bin"
fi
install/flexiv_hardware/share/flexiv_hardware/environment/path.sh
Symlink
+1
View File
@@ -0,0 +1 @@
/home/cc/anaconda3/envs/ros_flexiv/share/ament_cmake_core/cmake/environment_hooks/environment/path.sh
install/flexiv_hardware/share/flexiv_hardware/flexiv_hardware_interface_plugin.xml
-20
View File
@@ -1,20 +0,0 @@
<library path="flexiv_hardware">
<class name="flexiv_hardware/FlexivHardwareInterface"
type="flexiv_hardware::FlexivHardwareInterface"
base_class_type="hardware_interface::SystemInterface">
<description>
ROS 2 Control Hardware interface for the Flexiv robots. This driver
uses Flexiv RDK. Position, Velocity and Effort interfaces are supported.
</description>
</class>
<class name="flexiv_hardware/FlexivDualHardwareInterface"
type="flexiv_hardware::FlexivDualHardwareInterface"
base_class_type="hardware_interface::SystemInterface">
<description>
ROS 2 Control Hardware interface for a pair of Flexiv robots. This driver
uses Flexiv DRDK. Position, Velocity and Effort interfaces are supported.
</description>
</class>
</library>
install/flexiv_hardware/share/flexiv_hardware/flexiv_hardware_interface_plugin.xml
Symlink
+1
View File
@@ -0,0 +1 @@
/home/cc/ros2/src/flexiv_hardware/flexiv_hardware_interface_plugin.xml
install/flexiv_hardware/share/flexiv_hardware/local_setup.bash
-46
View File
@@ -1,46 +0,0 @@
# generated from ament_package/template/package_level/local_setup.bash.in
# source local_setup.sh from same directory as this file
_this_path=$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" && pwd)
# provide AMENT_CURRENT_PREFIX to shell script
AMENT_CURRENT_PREFIX=$(builtin cd "`dirname "${BASH_SOURCE[0]}"`/../.." && pwd)
# store AMENT_CURRENT_PREFIX to restore it before each environment hook
_package_local_setup_AMENT_CURRENT_PREFIX=$AMENT_CURRENT_PREFIX
# trace output
if [ -n "$AMENT_TRACE_SETUP_FILES" ]; then
echo "# . \"$_this_path/local_setup.sh\""
fi
. "$_this_path/local_setup.sh"
unset _this_path
# unset AMENT_ENVIRONMENT_HOOKS
# if not appending to them for return
if [ -z "$AMENT_RETURN_ENVIRONMENT_HOOKS" ]; then
unset AMENT_ENVIRONMENT_HOOKS
fi
# restore AMENT_CURRENT_PREFIX before evaluating the environment hooks
AMENT_CURRENT_PREFIX=$_package_local_setup_AMENT_CURRENT_PREFIX
# list all environment hooks of this package
# source all shell-specific environment hooks of this package
# if not returning them
if [ -z "$AMENT_RETURN_ENVIRONMENT_HOOKS" ]; then
_package_local_setup_IFS=$IFS
IFS=":"
for _hook in $AMENT_ENVIRONMENT_HOOKS; do
# restore AMENT_CURRENT_PREFIX for each environment hook
AMENT_CURRENT_PREFIX=$_package_local_setup_AMENT_CURRENT_PREFIX
# restore IFS before sourcing other files
IFS=$_package_local_setup_IFS
. "$_hook"
done
unset _hook
IFS=$_package_local_setup_IFS
unset _package_local_setup_IFS
unset AMENT_ENVIRONMENT_HOOKS
fi
unset _package_local_setup_AMENT_CURRENT_PREFIX
unset AMENT_CURRENT_PREFIX
install/flexiv_hardware/share/flexiv_hardware/local_setup.bash
Symlink
+1
View File
@@ -0,0 +1 @@
/home/cc/ros2/build/flexiv_hardware/ament_cmake_environment_hooks/local_setup.bash
install/flexiv_hardware/share/flexiv_hardware/local_setup.dsv
-2
View File
@@ -1,2 +0,0 @@
source;share/flexiv_hardware/environment/ament_prefix_path.sh
source;share/flexiv_hardware/environment/path.sh
install/flexiv_hardware/share/flexiv_hardware/local_setup.dsv
Symlink
+1
View File
@@ -0,0 +1 @@
/home/cc/ros2/build/flexiv_hardware/ament_cmake_environment_hooks/local_setup.dsv
install/flexiv_hardware/share/flexiv_hardware/local_setup.sh
-184
View File
@@ -1,184 +0,0 @@
# generated from ament_package/template/package_level/local_setup.sh.in
# since this file is sourced use either the provided AMENT_CURRENT_PREFIX
# or fall back to the destination set at configure time
: ${AMENT_CURRENT_PREFIX:="/home/cc/ros2/install/flexiv_hardware"}
if [ ! -d "$AMENT_CURRENT_PREFIX" ]; then
if [ -z "$COLCON_CURRENT_PREFIX" ]; then
echo "The compile time prefix path '$AMENT_CURRENT_PREFIX' doesn't " \
"exist. Consider sourcing a different extension than '.sh'." 1>&2
else
AMENT_CURRENT_PREFIX="$COLCON_CURRENT_PREFIX"
fi
fi
# function to append values to environment variables
# using colons as separators and avoiding leading separators
ament_append_value() {
# arguments
_listname="$1"
_value="$2"
#echo "listname $_listname"
#eval echo "list value \$$_listname"
#echo "value $_value"
# avoid leading separator
eval _values=\"\$$_listname\"
if [ -z "$_values" ]; then
eval export $_listname=\"$_value\"
#eval echo "set list \$$_listname"
else
# field separator must not be a colon
_ament_append_value_IFS=$IFS
unset IFS
eval export $_listname=\"\$$_listname:$_value\"
#eval echo "append list \$$_listname"
IFS=$_ament_append_value_IFS
unset _ament_append_value_IFS
fi
unset _values
unset _value
unset _listname
}
# function to append non-duplicate values to environment variables
# using colons as separators and avoiding leading separators
ament_append_unique_value() {
# arguments
_listname=$1
_value=$2
#echo "listname $_listname"
#eval echo "list value \$$_listname"
#echo "value $_value"
# check if the list contains the value
eval _values=\$$_listname
_duplicate=
_ament_append_unique_value_IFS=$IFS
IFS=":"
if [ "$AMENT_SHELL" = "zsh" ]; then
ament_zsh_to_array _values
fi
for _item in $_values; do
# ignore empty strings
if [ -z "$_item" ]; then
continue
fi
if [ $_item = $_value ]; then
_duplicate=1
fi
done
unset _item
# append only non-duplicates
if [ -z "$_duplicate" ]; then
# avoid leading separator
if [ -z "$_values" ]; then
eval $_listname=\"$_value\"
#eval echo "set list \$$_listname"
else
# field separator must not be a colon
unset IFS
eval $_listname=\"\$$_listname:$_value\"
#eval echo "append list \$$_listname"
fi
fi
IFS=$_ament_append_unique_value_IFS
unset _ament_append_unique_value_IFS
unset _duplicate
unset _values
unset _value
unset _listname
}
# function to prepend non-duplicate values to environment variables
# using colons as separators and avoiding trailing separators
ament_prepend_unique_value() {
# arguments
_listname="$1"
_value="$2"
#echo "listname $_listname"
#eval echo "list value \$$_listname"
#echo "value $_value"
# check if the list contains the value
eval _values=\"\$$_listname\"
_duplicate=
_ament_prepend_unique_value_IFS=$IFS
IFS=":"
if [ "$AMENT_SHELL" = "zsh" ]; then
ament_zsh_to_array _values
fi
for _item in $_values; do
# ignore empty strings
if [ -z "$_item" ]; then
continue
fi
if [ "$_item" = "$_value" ]; then
_duplicate=1
fi
done
unset _item
# prepend only non-duplicates
if [ -z "$_duplicate" ]; then
# avoid trailing separator
if [ -z "$_values" ]; then
eval export $_listname=\"$_value\"
#eval echo "set list \$$_listname"
else
# field separator must not be a colon
unset IFS
eval export $_listname=\"$_value:\$$_listname\"
#eval echo "prepend list \$$_listname"
fi
fi
IFS=$_ament_prepend_unique_value_IFS
unset _ament_prepend_unique_value_IFS
unset _duplicate
unset _values
unset _value
unset _listname
}
# unset AMENT_ENVIRONMENT_HOOKS
# if not appending to them for return
if [ -z "$AMENT_RETURN_ENVIRONMENT_HOOKS" ]; then
unset AMENT_ENVIRONMENT_HOOKS
fi
# list all environment hooks of this package
ament_append_value AMENT_ENVIRONMENT_HOOKS "$AMENT_CURRENT_PREFIX/share/flexiv_hardware/environment/ament_prefix_path.sh"
ament_append_value AMENT_ENVIRONMENT_HOOKS "$AMENT_CURRENT_PREFIX/share/flexiv_hardware/environment/path.sh"
# source all shell-specific environment hooks of this package
# if not returning them
if [ -z "$AMENT_RETURN_ENVIRONMENT_HOOKS" ]; then
_package_local_setup_IFS=$IFS
IFS=":"
if [ "$AMENT_SHELL" = "zsh" ]; then
ament_zsh_to_array AMENT_ENVIRONMENT_HOOKS
fi
for _hook in $AMENT_ENVIRONMENT_HOOKS; do
if [ -f "$_hook" ]; then
# restore IFS before sourcing other files
IFS=$_package_local_setup_IFS
# trace output
if [ -n "$AMENT_TRACE_SETUP_FILES" ]; then
echo "# . \"$_hook\""
fi
. "$_hook"
fi
done
unset _hook
IFS=$_package_local_setup_IFS
unset _package_local_setup_IFS
unset AMENT_ENVIRONMENT_HOOKS
fi
# reset AMENT_CURRENT_PREFIX after each package
# allowing to source multiple package-level setup files
unset AMENT_CURRENT_PREFIX
install/flexiv_hardware/share/flexiv_hardware/local_setup.sh
Symlink
+1
View File
@@ -0,0 +1 @@
/home/cc/ros2/build/flexiv_hardware/ament_cmake_environment_hooks/local_setup.sh
install/flexiv_hardware/share/flexiv_hardware/local_setup.zsh
-59
View File
@@ -1,59 +0,0 @@
# generated from ament_package/template/package_level/local_setup.zsh.in
AMENT_SHELL=zsh
# source local_setup.sh from same directory as this file
_this_path=$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && pwd)
# provide AMENT_CURRENT_PREFIX to shell script
AMENT_CURRENT_PREFIX=$(builtin cd -q "`dirname "${(%):-%N}"`/../.." > /dev/null && pwd)
# store AMENT_CURRENT_PREFIX to restore it before each environment hook
_package_local_setup_AMENT_CURRENT_PREFIX=$AMENT_CURRENT_PREFIX
# function to convert array-like strings into arrays
# to wordaround SH_WORD_SPLIT not being set
ament_zsh_to_array() {
local _listname=$1
local _dollar="$"
local _split="{="
local _to_array="(\"$_dollar$_split$_listname}\")"
eval $_listname=$_to_array
}
# trace output
if [ -n "$AMENT_TRACE_SETUP_FILES" ]; then
echo "# . \"$_this_path/local_setup.sh\""
fi
# the package-level local_setup file unsets AMENT_CURRENT_PREFIX
. "$_this_path/local_setup.sh"
unset _this_path
# unset AMENT_ENVIRONMENT_HOOKS
# if not appending to them for return
if [ -z "$AMENT_RETURN_ENVIRONMENT_HOOKS" ]; then
unset AMENT_ENVIRONMENT_HOOKS
fi
# restore AMENT_CURRENT_PREFIX before evaluating the environment hooks
AMENT_CURRENT_PREFIX=$_package_local_setup_AMENT_CURRENT_PREFIX
# list all environment hooks of this package
# source all shell-specific environment hooks of this package
# if not returning them
if [ -z "$AMENT_RETURN_ENVIRONMENT_HOOKS" ]; then
_package_local_setup_IFS=$IFS
IFS=":"
for _hook in $AMENT_ENVIRONMENT_HOOKS; do
# restore AMENT_CURRENT_PREFIX for each environment hook
AMENT_CURRENT_PREFIX=$_package_local_setup_AMENT_CURRENT_PREFIX
# restore IFS before sourcing other files
IFS=$_package_local_setup_IFS
. "$_hook"
done
unset _hook
IFS=$_package_local_setup_IFS
unset _package_local_setup_IFS
unset AMENT_ENVIRONMENT_HOOKS
fi
unset _package_local_setup_AMENT_CURRENT_PREFIX
unset AMENT_CURRENT_PREFIX
install/flexiv_hardware/share/flexiv_hardware/local_setup.zsh
Symlink
+1
View File
@@ -0,0 +1 @@
/home/cc/ros2/build/flexiv_hardware/ament_cmake_environment_hooks/local_setup.zsh
install/flexiv_hardware/share/flexiv_hardware/package.xml
-25
View File
@@ -1,25 +0,0 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>flexiv_hardware</name>
<version>1.9.1</version>
<description>Hardware interfaces to Flexiv robots for ROS 2 control</description>
<maintainer email="munseng.phoon@flexiv.com">Mun Seng Phoon</maintainer>
<license>Apache 2.0</license>
<author email="munseng.phoon@flexiv.com">Mun Seng Phoon</author>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>rclcpp_lifecycle</depend>
<depend>hardware_interface</depend>
<depend>pluginlib</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<test_depend>ament_cmake_gtest</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
install/flexiv_hardware/share/flexiv_hardware/package.xml
Symlink
+1
View File
@@ -0,0 +1 @@
/home/cc/ros2/src/flexiv_hardware/package.xml
install/setup.bash
+2 -13
View File
@@ -19,19 +19,8 @@ _colcon_prefix_chain_bash_source_script() {
# source chained prefixes
# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script
# 不写死某一台机器的 conda。已激活的环境优先，否则找当前用户的 ros_flexiv。
if [ -n "${CONDA_PREFIX:-}" ] && [ -f "${CONDA_PREFIX}/local_setup.bash" ]; then
COLCON_CURRENT_PREFIX="${CONDA_PREFIX}"
elif [ -f "${HOME}/anaconda3/envs/ros_flexiv/local_setup.bash" ]; then
COLCON_CURRENT_PREFIX="${HOME}/anaconda3/envs/ros_flexiv"
elif [ -f "${HOME}/miniconda3/envs/ros_flexiv/local_setup.bash" ]; then
COLCON_CURRENT_PREFIX="${HOME}/miniconda3/envs/ros_flexiv"
else
COLCON_CURRENT_PREFIX=""
fi
if [ -n "${COLCON_CURRENT_PREFIX}" ]; then
_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash"
fi
COLCON_CURRENT_PREFIX="/home/cc/anaconda3/envs/ros_flexiv"
_colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash"
# source this prefix
# setting COLCON_CURRENT_PREFIX avoids determining the prefix in the sourced script

Some files were not shown because too many files have changed in this diff Show More