arm_dev
dfc945b7e0
新增
----
- arm_application/gauge_assembly_node.py：主流程。每个阶段一个 _step_*，
返回值就是状态机的事件；纯逻辑（状态机/配置/限位/夹爪后端/自检）全部
在 gauge_*.py 里，节点只做"发指令 + 读反馈 + 报错"。
* S00 自检报告带"缺了会怎样"，必检拦住、选检告警；
* S01 回 home → 切工具系 → 逆时针预绕，**事后用 TF 实测绕工具 Z 转了多少度**
（转轴偏差 >5° 直接中止：以"度"为单位的动作全建立在这条假设上）；
* S03 手外粗定位：没有 eye-to-hand 标定文件时明确报"缺哪个文件、怎么生成"，
回 S04 兜底（不做假装成功的反投影）；
* S05 稳定判据用"到均值的最远距离"，不用相邻帧之差；
* S07 悬停处测基线 → 步进下压 → 两路切向任一突变即停，法向超安全上限抬起中止；
* S08 棘轮四步（慢转→松爪→快回转→夹紧），停止判据「切向 >阈值 且 法向突增」，
掉件走 E1 重放；S09 反向拧出（比拧入多一轮）；
* E1 重放等 /gauge/reload_ok 人工确认（超时按中止），E2 任何中止都先退安全高度。
- launch/gauge_assembly.launch.py：只做一件事 —— 发布手眼静态 TF（S05 的前提），
其余判据全交给 S00（判断标准只有一处）。
- scripts/run_gauge_assembly.sh：--arm/--sim/--dry-run/--holes/--backend。
- gauge_assembly 入口（setup.py）+ 站点参数 reload_timeout_s。
- test/test_gauge_step_events.py：**读节点源码 AST**，把每个 _step_* 可能返回的
事件逐个问状态机"合法吗" —— 节点与转移表是两份数据，漏登记要在 pytest 里炸，
而不是在真机上跑到那一步才炸。
顺手修掉的两个真问题
--------------------
1. 四份 launch 里的四元数换算**已经互相抄歪了**（退化分支的公式各不相同）。
统一收口到 handeye_lib.read_transform_yaml，并加回环测试（真实标定文件 +
180°/270° 等分支切换角度）；实测 4 个 launch 输出与库完全一致。
2. pipeline_node.AssemblyPipeline 支持 node_name 复用（本流程继承它的一阶段
跟踪/二阶段定位链，但在图上必须是另一个节点名）。
验证（都在本机真跑过）
----------------------
pytest（仿真环境） → 349 passed, 14 skipped（缺 onnx 的用例跳过）
干跑全流程 → S00→S01→…→S11→DONE，exit 0
仿真里不干跑（缺 /home 等） → S00 拦住，exit 1，未发任何运动指令
另：仿真环境没有 onnx（只有真机 conda 有），相关用例改为 skip —— 让测试结果
"红就是真红"。
Co-authored-by: Cursor <cursoragent@cursor.com>
88 lines
3.7 KiB
Python
88 lines
3.7 KiB
Python
from setuptools import find_packages, setup
|
||
import os
|
||
from glob import glob
|
||
|
||
package_name = 'arm_application'
|
||
|
||
|
||
def _existing(paths):
|
||
"""只保留真实存在的常规文件。
|
||
|
||
必需：colcon 的 ``build/<pkg>/`` 里放着指向源文件的符号链接，
|
||
源文件被挪走或删掉之后这些链接就断了。``glob`` / ``os.walk`` 仍会把它们
|
||
列出来，随后 copy 会报 "doesn't exist or not a regular file"，
|
||
**整个包构建失败**，而报错信息指向 build/ 下的一个幽灵文件，很难查。
|
||
这里统一过滤掉。
|
||
"""
|
||
out = []
|
||
for p in paths:
|
||
if os.path.isfile(p): # 会跟随符号链接，断链返回 False
|
||
out.append(p)
|
||
return out
|
||
|
||
|
||
def _data_files():
|
||
entries = [
|
||
('share/ament_index/resource_index/packages', ['resource/' + package_name]),
|
||
('share/' + package_name, ['package.xml']),
|
||
(os.path.join('share', package_name, 'launch'), _existing(glob('launch/*.py'))),
|
||
(os.path.join('share', package_name, 'trajectories'), _existing(glob('trajectories/*'))),
|
||
]
|
||
for root, _dirs, files in os.walk('config'):
|
||
srcs = _existing([os.path.join(root, name) for name in files])
|
||
if not srcs:
|
||
continue
|
||
dest = os.path.join('share', package_name, root)
|
||
entries.append((dest, srcs))
|
||
return entries
|
||
|
||
|
||
setup(
|
||
name=package_name,
|
||
version='0.1.0',
|
||
packages=find_packages(exclude=['test']),
|
||
data_files=_data_files(),
|
||
install_requires=['setuptools'],
|
||
zip_safe=True,
|
||
maintainer='arm_dev',
|
||
maintainer_email='dev@example.com',
|
||
description='High-level arm application nodes.',
|
||
license='Apache-2.0',
|
||
entry_points={
|
||
'console_scripts': [
|
||
# Bringup helpers
|
||
'demo_named_poses = arm_application.demo_named_poses:main',
|
||
'move_to_pose_client = arm_application.move_to_pose_client:main',
|
||
'run_joint_trajectory = arm_application.run_joint_trajectory:main',
|
||
'goto_ee_plane = arm_application.goto_ee_plane:main',
|
||
# 相机
|
||
'realsense_color_pub = arm_application.realsense_color_pub:main',
|
||
'realsense_view = arm_application.realsense_view:main',
|
||
# 手眼标定
|
||
'handeye_teach = arm_application.handeye_teach:main',
|
||
'handeye_waypoints = arm_application.handeye_waypoints:main',
|
||
'handeye_calibrate = arm_application.handeye_calibrate:main',
|
||
# 视觉伺服（当前主链路）
|
||
'perception_node = arm_application.perception_node:main',
|
||
'servo_node = arm_application.servo_node:main',
|
||
# 夹爪
|
||
'dh_pgea = arm_application.dh_pgea_node:main',
|
||
'dh_pgea_cli = arm_application.dh_pgea:main',
|
||
'dual_gripper = arm_application.dual_gripper_node:main',
|
||
'dual_gripper_cli = arm_application.dual_gripper_cli:main',
|
||
# 入孔检测（只检测，不做插入动作）
|
||
'hole_detect = arm_application.hole_detect_node:main',
|
||
# 第二阶段：一次性孔定位（k1/k2）+ 对规
|
||
'stage2_locate = arm_application.stage2_locate_node:main',
|
||
# 第三阶段：柔顺找孔（Orisys 触觉螺旋 + 插入）
|
||
'stage3_search = arm_application.stage3_search_node:main',
|
||
# 三段式装配流水线（跟踪 → 定位 → 找孔，单进程）
|
||
'assembly_pipeline = arm_application.pipeline_node:main',
|
||
# 通止规装配（看板 S00–S11，含自检/换孔/掉件重放）
|
||
'gauge_assembly = arm_application.gauge_assembly_node:main',
|
||
'tactile = arm_application.tactile_node:main',
|
||
'tactile_cli = arm_application.tactile_cli:main',
|
||
],
|
||
},
|
||
)
|