机械臂控制程序

机械臂抓取系统

场景：机械臂从车上抓取 杆件/钩子，放到货架上。

核心业务流程

抓取任务 (catch_kfs, task_id=2)：
1. 移动到 catch_gan_ready 准备位
2. 视觉伺服逼近目标（RealSense 检测红色杆 → TF → 视觉伺服收敛）
3. 气泵吸附 → 夹爪抓取
4. 沿脱离路径 detach_gan_ready → detach_gan_1 → detach_gan_2 移动
5. 向下戳杆（Z - 0.27m 直线运动）
6. 通知 driver 抓取完成

放置任务 (place_kfs, task_id=3)：
1. 移动到 kfs{N}_touch_pos 触摸位
2. 打开气泵吸附
3. 退到 place_interim_pos_0
4. 笛卡尔空间移动到货架前方（x - 0.40m）
5. 直线插入货架位
6. 关闭气泵 → 放杆
7. 退到 ready 位

移动任务 (move_kfs, task_id=1)：
- 支持关节空间（6轴+duration）和笛卡尔空间（xyz+rpy+duration）两种模式
- 可选气泵开关

视觉伺服

vision 节点检测红色杆 → 发布 camera_link → target_camera TF → arm_task 实时转换到 base_link → 持续发布 visual_target_pose 给 arm_calc 做闭环控制。

距离 < 0.35m 锁定目标位姿，位置误差 < 0.05m 判定收敛。

架构

ROS2 多节点系统，8个包，C++20。

节点数据流

vision_node ──TF──→ arm_task ──visual_target_pose──→ arm_calc ──myjoints_target──→ dog_controller
│ │ │
│ └──joint_states←───────────────┘
└──joint_space_target(命名位姿)

任务调度

Robot 类维护 task_table_（map），porcess_task() 线程循环执行当前任务的 process()，返回下一个任务名实现状态机切换。

idel ──(grasp_it=1 或 task_id=2)──→ catch_kfs ──→ idel
idel ──(task_id=1)────────────────→ move_kfs ──→ idel
idel ──(task_id=3)────────────────→ place_kfs ──→ idel

运动控制

arm_calc 节点提供4种模式（参数 motion_mode）：
- mode 1：关节空间轨迹
- mode 2：笛卡尔空间轨迹
- mode 3：视觉伺服（arm_task 持续发布目标位姿）

arm_task 通过 AsyncParametersClient 远程设置 arm_calc 参数，通过 execute_trajectory 布尔量触发执行并等待完成。

视觉伺服

独立线程 100Hz 运行：
1. TF 查找 camera_link → target_camera
2. 坐标变换到 base_link + 偏置（grasp_x_run, grasp_y_run）
3. Z 强制为 grasp_height，姿态强制 RPY(0, 0.3, 0)
4. 距离 < 0.35m 锁定位姿
5. 位置误差 < 0.05m 收敛

硬件接口

robot_driver 通过 USB CDC 与 STM32 通信，发布 myjoints_state，接收 myjoints_target。气泵和抓取状态通过参数服务控制。

TF 坐标系

world → base_link → Link1~7（机械臂）
Link4 → c