视觉动态抓取

01问题引入

EXP-06 的坐标是尺子量的——物体一挪就抓空。要让机械臂"看到"物体，需要把一帧彩色图像变成 base_link 下的 3D 坐标，再交给你已经熟悉的 IK+抓取链路。这中间横着三座桥：

• 颜色桥（HSV）：图像成千上万像素，哪些属于目标？光照一变 RGB 全乱，HSV 才稳。
• 深度桥（反投影）：像素 (u,v) 只有二维，加上深度图的 d 才能还原相机坐标系里的 (X,Y,Z)。
• 坐标桥（TF）：相机装在夹爪上随臂晃动，相机系的点必须实时换到 base_link 才能抓。

本实验 8 学时：一条 launch 拉起全栈，rqt 三栏盯着它看，四个人机服务管着节奏，最后拿红色物体（30~50mm）做成功率验收。

02原理通俗讲解

HSV 分色像用"色相环"筛豆子。RGB 是红绿蓝三盏灯的混合亮度，灯一暗全都变样；HSV 把"什么颜色（H）"、"多纯（S）"、"多亮（V）"拆开——只圈定 H 的范围，屋内灯稍亮稍暗都不影响筛选。inRange 就是拿一个区间筛子过一遍像素，命中的涂白成掩膜。

深度反投影像"由近及远钉钉子"。已知像素在画面偏右 100 像素、深度 0.5m：乘内参（fx≈614）一除，就知道它在相机右侧约 8 厘米、正前方半米——(u,v,d)→(X,Y,Z) 就是这一除。深度图单位是 uint16 的毫米，用前必须 /1000，这是最经典的翻车点。

TF 像"随军翻译"。相机说"在我右前方 30cm"（camera_depth_optical_frame 系，X右/Y下/Z前，REP-103），底座听不懂；tf_buffer.transform 一换算，底座就知道"在base_link 前方多少"。相机装在手上，臂一动翻译表就更新——这就是 TF 的实时性。

look-then-move 是"看准了再伸手"。观察（OBSERVE）→ 有人按确认（CONFIRM）→ 猛抓（PICK）→ 回怠速（IDLE）。光看不确认就动是鲁莽，确认了快动是果断——五态循环把二者拆开。

03关联知识点

基线指标：OBSERVE 检测 <1s；PICK <8s；单周期 <15s；HSV 检测成功率 >95%；1m 处反投影误差 ±2-3cm；TF 换算误差 <1cm；整体抓取成功率 >85%；误抓率 <5%（面积<500 剔除 + 重复抓取自动 skip）。相机按课程规格书为 HP60C（讲义环境表记 realsense2_camera 驱动，如设备不同仅影响驱动参数，五节点流水线不变）。

04实验目标

• 理解视觉抓取端到端原理：HSV 检测 → 深度反投影 → TF 变换 → IK → 状态机，五模块各司其职。
• 独立完成一站式启动 visual_grasp.launch.py 并跑通"start → 观察 → confirm → 抓取"动态闭环。
• 能调 HSV 阈值适配新颜色物体（hsv_tuner.py），检测成功率 >95%。
• 能调 gripper_depth 适配 30~50mm 物体，并解释 look-then-move 与眼在手下压失视野问题的对策。
• 掌握四服务人机节奏与重复抓取处理（目标已抓走时自动 skip，避免夹空气）。

05实验环境与准备

06动画讲解：观察—确认—抓取三乐章

动画 9 秒一循环：左框里绿色虚线框住红色目标并提取质心，经 TF 换算到 base_link；右上状态机光点在 OBSERVE(绿)→CONFIRM(黄)→PICK(红)→IDLE(蓝) 间移动；下方机械臂在 PICK 段伸出、合爪、带物回位——"看准了再伸手"的节奏一目了然。

07互动讲解：HSV 掩膜调参台 + 反投影验算

先点"判掩膜窗口"：H 区间与预设完全一致报 ok，部分重叠报 warn（会漏检边缘色阶），完全无关报 bad。特别注意：如果你只在主段 [0,15] 里调，而手里红块的实际 H 在 167~178，那么无论怎么调都是 bad——这正是"两段并集"存在的理由。再点"反投影"：输入质心像素与深度毫米数，按 X=(u−cx)·d/fx, Y=(v−cy)·d/fy, Z=d 算相机系坐标——和你在 rqt Mask 栏看到的白色团块互为印证。

08实操步骤

# 步骤 1 · 一站式启动（分时拉 4 组东西：bringup → arm_controller → 相机 → 节点 → rqt）
ros2 launch car1_app visual_grasp.launch.py
# [INFO] [visual_grasp_node]: State IDLE, waiting for /visual_grasp/start
# 启动时序 t=0/5/8/12/14s：先串口与 TF → 控制器 → 相机出流 → 检测节点 → rqt 窗口
# 别在 t<12s 就急着 call start：节点还没起，服务不存在

# 步骤 2 · 三栏可视化（一个话题就够，launch 已自动开 rqt_image_view）
ros2 run rqt_image_view rqt_image_view /visual_grasp/visualization
# 合成图：左 RGB（绿轮廓+黄框+红十字质心）｜中 Depth 伪彩（标 mm 读数）｜右 HSV Mask 二值真相
# 想看纯掩膜：ros2 run rqt_image_view rqt_image_view /visual_grasp/mask
# hsv_tuner.py 是讲义提供的离线调参脚本；线上调参直接改 launch 参数里的两段 H/S/V

# 步骤 3 · 反投影自测（内参请从 camera_info 读，别写死）
ros2 topic echo /ascamera_hp60c/camera_publisher/depth0/camera_info --once
# 看 k[0]=fx、k[4]=fy、k[2]=cx、k[5]=cy；实测 ≈614/614/320/240
# 手算：u,v=320,240 → X=Y=0（正好光轴中心），Z=d/1000；偏 200px、d=0.5m → X=(200)*0.5/614≈0.163m
# 标定物实测：1m 距离误差预期 ±2-3cm；超差先查 camera_info 是否已更新

# 步骤 4 · TF 链与时序
ros2 run tf2_tools view_frames      # … → arm_link4 → … → tcp_link → camera_depth_optical_frame
ros2 run tf2_ros tf2_echo base_link camera_depth_optical_frame

# 步骤 5 · 人机协同四服务（Trigger 类，命令体留空）
ros2 service call /visual_grasp/start   std_srvs/srv/Trigger "{}"
# 日志：State IDLE -> ... -> DETECT → Contour: area=…, centroid=(u,v) → WAIT_CONFIRMATION
ros2 service call /visual_grasp/confirm std_srvs/srv/Trigger "{}"   # 确认后进 PICK
ros2 service call /visual_grasp/skip    std_srvs/srv/Trigger "{}"   # 放弃本目标
ros2 service call /visual_grasp/abort   std_srvs/srv/Trigger "{}"   # 中止回 IDLE

# 步骤 6 · 换色验收（以抓 30~50mm 红色物体为例）
# 1) 改 HSV 两段阈值：优先确保"副段 160~180"覆盖住你手里的红块（实测 H 落在 167~178）
# 2) 桌面摆放物体，start → DETECT 出轮廓 → confirm → 看 PICK 是否 <8s 完成
# 3) 拿走物体再 start：轮廓面积 <100 应被熔断（"Largest red contour too small"）→ 不抓空气
# 4) 免确认直连（首次务必先过一遍带确认的）
ros2 launch car1_app visual_grasp.launch.py wait_for_confirmation:=false
# 安全预演：只定位不抓，机械臂只升到观察位
ros2 launch car1_app visual_grasp.launch.py dry_run:=true
# 验收指标：HSV 检测成功率>95% · 单周期<15s · 抓取成功率>85% · 误抓<5%

09重点与易错点

• 交付四件套：换色检测成功率 >95%、单周期 <15s、整体成功率 >85%、误抓 <5%。
• 三栏图像必须都看过才收工：rgb_annotated（框）、depth_overlay（深度着色）、mask（二值真相）。
• 留档一组"失败样本"截图 + 当时 HSV/深度/TF 日志——调试能力比一次成功更值钱。

10总结与思考