ROS 2 机械臂抓取与视觉感知实战

系列导航：

1. ROS 机器人仿真系统配置
2. ROS 2 入门与环境搭建
3. ROS 2 机器人仿真实战
4. ROS 2 自定义消息与服务实战
5. ROS 2 Nav2 自主导航实战
6. ROS 2 机械臂抓取与视觉感知实战（本文）
7. 外骨骼机器人技术概述
8. 基于ROS 2 的外骨骼控制系统实战
9. ROS 2 多机通信与系统部署实战

前言

把机械臂想象成一个力大无穷却闭着眼睛的壮汉：他能举起重物，却看不见东西在哪、会不会撞到桌子、该往哪儿伸手。前几篇教程里，我们已经教会他在仿真里动起来、能走能规划；但「动」和「会干活」之间，还差着一双眼睛和一套判断。

本文要做的，就是给这支壮汉装上「眼睛」（RealSense 深度相机）和「大脑」（MoveIt2 运动规划），教会他一句话就能概括的本事：看到物体 → 想清楚怎么伸手 → 稳稳抓起来放到该放的地方。

这条「看到→抓到」的主线会贯穿全文，我会按真实工程的搭建顺序带你走一遍，而不是把代码一甩了事：

1. 先搭身体：用 URDF 把机械臂和夹爪的「骨骼」描述出来，告诉仿真器每一节有多重、长啥样。
2. 再接大脑：用 SRDF 告诉 MoveIt2「哪些关节算手臂、哪些算夹爪」，并预设几个常用姿势，相当于给动作起好名字。
3. 然后装眼睛：用深度相机驱动 + Octomap 把真实世界的桌子、障碍物变成机械臂能「看见」的 3D 占据地图，避免它伸手撞桌。
4. 接着写感知：一段 Python 节点从相机画面里认出红色物体，并把它在相机坐标里的位置换算成机械臂坐标系里的 6DoF 位姿。
5. 最后串动作：另一段 Python 节点把「物体在哪」喂给 MoveIt2，走完 7 步 Pick & Place，并带上抓崩了就退回安全位的异常恢复。

读到最后，你手里的仿真机械臂就能自己完成「抓取—搬运—放置」这一整条动作链。一个实用的阅读约定：看不懂某个代码块时，先读它前面的「白话导读」三五行，知道这段在干什么，再回去看代码；代码里关键行我也都加了中文行内注释，讲「为什么这么写」。

环境：ROS 2 Jazzy + MoveIt2 + RealSense D435。全部代码在仿真（Gazebo）里跑通即可，没有真机也能照着学；真机只是把相机节点从仿真换成真实驱动。

零、开始之前：三个必须先懂的 ROS 2 概念

动手贴代码前，先把三个 ROS 2 的基础概念理顺。它们贯穿全文，不懂就会处处卡壳——不是语法卡，而是「为什么这里用 action 而不用 service」「为什么坐标老对不上」这种卡。花十分钟读这一节，后面每段代码你都能「知其所以然」。

1. 三种通信方式：话题、服务、动作

• 话题（Topic）是「广播」，一对多、单向、持续流式，适合传感器数据——相机的图、点云就是话题。
• 服务（Service）是「一问一答」，客户端发请求、服务器回一个响应，适合偶尔查询（如清 Octomap）。
• 动作（Action）是「交任务」，客户端发一个目标、服务器周期性回反馈、最后回结果，适合耗时且要盯进度的任务。机械臂规划+执行一次要好几秒，所以 MoveIt2 和夹爪都用 Action。你会在代码里看到 ActionClient、send_goal_async、spin_until_future_complete，这就是在「派活儿并等它干完」。

2. QoS：数据可靠性的开关
ROS 2 每个话题都有 QoS（服务质量）。图像、点云量大且允许丢帧，用 BEST_EFFORT（丢了就丢了，别积压）；相机内参小而关键、不能丢，用 RELIABLE（务必送达）。代码里 sensor_qos 和 reliable_qos 两套就是为此分设。配错 QoS，订阅端可能收不到任何数据却毫无报错，是最隐蔽的坑之一。

3. TF：坐标系之间的「翻译官」
相机说「物体在相机左边 30 cm」，机械臂说「我基座在原点」。两者语言不通，必须靠 TF（坐标变换树）把相机坐标系下的点换算成 base_link 坐标系下的点。本文检测节点末尾那段 lookup_transform('base_link', ...) 就是在问 TF：「相机测的点，在我基座坐标系里到底在哪？」TF 树断了（比如相机没发布自己的位姿），换算就会 fallback 到相机坐标、抓取全偏。

把这三件事记牢，后面每一段代码都不是照抄，而是理解后复用。

一、系统架构

动手写第一行代码之前，先用一张图把全局理清——否则后面每贴一段代码，你都会问「这段接在哪」。整条 Pick & Place 流水线其实就三层，和人的工作方式一模一样：

• 感知层（眼睛）：深度相机拍出彩色图和深度图，物体检测算法从中认出物体并算出它的 3D 位姿，Octomap 再把整个场景建成立体占据地图。
• 决策层（大脑）：MoveIt2 拿到「物体在哪」后，负责规划一条不撞桌子的关节轨迹，并做碰撞检测。
• 执行层（手脚）：ros2_control 把规划好的轨迹变成每个关节电机的实际转动，夹爪控制器负责开合。

下面这张图把三层各自的组件和它们之间的数据流画了出来。注意箭头方向：感知数据一路向上汇到决策层，决策层再把指令向下发给执行层。

┌─────────────────────────────────────────────────────────────┐
│                    Pick & Place 系统                          │
├──────────────┬──────────────┬───────────────────────────────┤
│   感知层      │   决策层      │   执行层                      │
├──────────────┼──────────────┼───────────────────────────────┤
│ RealSense    │              │                               │
│ D435 深度相机 │  MoveIt2     │  ros2_control                 │
│      ↓       │  运动规划器   │      ↓                        │
│ PointCloud2  │      ↓       │  关节轨迹执行                   │
│      ↓       │  Octomap     │      ↓                        │
│ 物体检测     │  3D 占据地图  │  夹爪控制器                    │
│ (YOLO/CNN)   │      ↓       │      ↓                        │
│      ↓       │  碰撞检测    │  关节位置反馈                   │
│ 位姿估计     │      ↓       │                               │
│ (6DoF)       │  抓取位姿生成 │                               │
│              │      ↓       │                               │
│              │  轨迹规划     │                               │
│              │  (RRT*/OMPL) │                               │
└──────────────┴──────────────┴───────────────────────────────┘

这张图你后面会反复回来对照：每当我们接好一层（比如写完感知节点），就想想它在图里处在哪一格、数据从哪进从哪出。带着这张地图读下面的代码，就不会迷路。

二、机械臂模型（带夹爪）

任何机械臂仿真的第一步，都是先把它「画」出来——不是画外观，而是用 URDF 描述它的运动学结构：有几节连杆、每个关节怎么转、每节多重。ROS 里这套描述格式叫 URDF（Unified Robot Description Format），本质就是一段 XML。

下面这个模型是个 5 自由度的机械臂（5 个旋转关节）外加一个平行夹爪。自由度少、结构规整，正好适合入门：你能一眼看懂每个 joint 的轴和限位，不会被几十个关节的工业臂劝退。

在贴 URDF 之前，先建立两个心智模型，否则你只会「照抄 XML」而不知道哪行能改、哪行不能动。

第一，URDF 描述的是「运动学骨架」，不是外观模型。它告诉系统「有几节、怎么连、多重、能转多大」，渲染出的圆柱方块只是为了让你在 RViz 里看得见，换成好看的 STL 网格不影响任何运动学。所以初学者常见的误区——「我把形状画漂亮点机械臂就更灵活」——是完全反的；灵活度只由关节类型和 limit 决定。

第二，xacro 是 URDF 的「预处理器」。纯 URDF 不能定义变量和宏，5 节臂就得把 <link> 写 5 遍。xacro 允许 <xacro:property> 定义常量（如 PI）、<xacro:macro> 定义可复用模板（如我们的夹爪）。真实工业臂几十个关节，没有 xacro 根本没法维护。理解这一点，你就能看懂为什么文件开头是 <robot xmlns:xacro=...> 而不是普通 URDF。

2.1 5-DOF 机械臂 + 平行夹爪 URDF

urdf/pick_place_robot.xacro：

下面这段是整个机械臂的「骨骼文件」。它很长，但结构高度重复——base_link、link1~link5 都是「一个连杆 + 一个旋转关节」的套路，只是尺寸、质量、轴方向不同。你真正该盯住的只有三件事：

1. 每个 <joint> 的 axis（绕哪个轴转）和 limit（能转多大、出多大劲），这决定机械臂的动作范围；
2. <inertial> 里的质量和惯性，仿真里物理引擎靠它算 dynamics，填错会「飘」或「抖」；
3. 文件末尾 <xacro:include> 把夹爪挂到 link5 上——机械臂本体和夹爪是分开两个文件写的，用 xacro 宏拼起来。

其余连杆外形（圆柱、方块）都是重复模板，第一遍读扫一眼即可，不用逐行抠。

<?xml version="1.0"?>
<robot xmlns:xacro="https://www.ros.org/wiki/xacro" name="pick_place_robot">

  <xacro:property name="PI" value="3.14159265"/>

  <!-- ====== 基座 ====== -->
  <!-- base_link 是整个机器人的根，所有连杆都相对它定义；
       质量给到 2.0kg 是为了让基座在仿真里「站得稳」，不会一推就倒 -->
  <link name="base_link">
    <visual>
      <geometry>
        <cylinder radius="0.06" length="0.05"/>
      </geometry>
      <material name="dark_gray">
        <color rgba="0.3 0.3 0.3 1.0"/>
      </material>
    </visual>
    <collision>
      <!-- collision 用和 visual 一样的简单形状即可，仿真碰撞检测不需要精细网格 -->
      <geometry>
        <cylinder radius="0.06" length="0.05"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="2.0"/>
      <origin xyz="0 0 0.025"/>
      <cylinder_inertia m="2.0" r="0.06" h="0.05"/>
    </inertial>
  </link>

  <!-- base_to_world 是个 fixed 关节：把基座焊死在世界坐标系上，
       没有它机械臂会「悬空」没有参照系 -->
  <joint name="base_to_world" type="fixed">
    <parent link="world"/>
    <child link="base_link"/>
    <origin xyz="0 0 0.0" rpy="0 0 0"/>
  </joint>

  <!-- ====== joint1: 底部旋转 ====== -->
  <!-- 下面 link1~link5 结构相同，区别只在尺寸/质量/旋转轴，先看一个就行 -->
  <link name="link1">
    <visual>
      <geometry>
        <cylinder radius="0.04" length="0.08"/>
      </geometry>
      <origin xyz="0 0 0.04"/>
      <material name="blue">
        <color rgba="0.2 0.4 0.8 1.0"/>
      </material>
    </visual>
    <collision>
      <geometry>
        <cylinder radius="0.04" length="0.08"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="1.5"/>
      <origin xyz="0 0 0.04"/>
      <cylinder_inertia m="1.5" r="0.04" h="0.08"/>
    </inertial>
  </link>

  <!-- type=revolute 表示绕固定轴旋转；axis xyz="0 0 1" 即绕 Z 轴(竖直)转，
       这就是机械臂「腰」的回转；limit 给出角度上下限和最大力矩/速度 -->
  <joint name="joint1" type="revolute">
    <parent link="base_link"/>
    <child link="link1"/>
    <origin xyz="0 0 0.05" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit lower="${-PI}" upper="${PI}" effort="50" velocity="2.0"/>
  </joint>

  <!-- ====== joint2: 肩部俯仰 ====== -->
  <!-- axis xyz="0 1 0" 绕 Y 轴，是「大臂抬起/放下」的肩俯仰；
       限位 ±90° 让它不会折到自己身上 -->
  <link name="link2">
    <visual>
      <geometry>
        <box size="0.04 0.04 0.20"/>
      </geometry>
      <origin xyz="0 0 0.10"/>
      <material name="blue"/>
    </visual>
    <collision>
      <geometry>
        <box size="0.04 0.04 0.20"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="1.0"/>
      <origin xyz="0 0 0.10"/>
      <box_inertia m="1.0" x="0.04" y="0.04" z="0.20"/>
    </inertial>
  </link>

  <joint name="joint2" type="revolute">
    <parent link="link1"/>
    <child link="link2"/>
    <origin xyz="0 0 0.08" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <limit lower="${-PI/2}" upper="${PI/2}" effort="40" velocity="1.5"/>
  </joint>

  <!-- ====== joint3~joint5: 肘/腕关节 ====== -->
  <!-- 结构与上面完全同构：link 是「形状+质量+惯性」，joint 是「轴向+限位」。
       三个关节的差异只在数字，列成一张表看得更清楚：

       | 关节 | 轴    | 限位            | 作用           |
       |------|-------|-----------------|----------------|
       | j3 肘| Y 轴  | ±135°           | 弯肘够远处物体 |
       | j4 腕| Y 轴  | ±180°           | 腕部俯仰微调   |
       | j5 腕| Z 轴  | ±180°           | 夹爪对正朝向   |

       每个 link 的 mass 从肘到腕递减（0.8→0.3→0.2 kg），
       越靠末端越轻——真实机械臂都这样设计，末端轻了惯性小、
       规划出来的轨迹才不「甩」。 -->
  <link name="link3">
    <!-- ...（link3~link5 的 visual/collision/inertial 同 link2 模板，
         尺寸 0.18/0.06/0.04 m，质量 0.8/0.3/0.2 kg，此处折叠）-->
  </link>
  <joint name="joint3" type="revolute">
    <parent link="link2"/>
    <child link="link3"/>
    <origin xyz="0 0 0.20" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <limit lower="${-PI*3/4}" upper="${PI*3/4}" effort="30" velocity="2.0"/>
  </joint>

  <!-- joint4/joint5 的 link 定义同上折叠，只保留关节本体 -->
  <joint name="joint4" type="revolute">
    <parent link="link3"/>
    <child link="link4"/>
    <origin xyz="0 0 0.18" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <limit lower="${-PI}" upper="${PI}" effort="15" velocity="3.0"/>
  </joint>

  <joint name="joint5" type="revolute">
    <parent link="link4"/>
    <child link="link5"/>
    <origin xyz="0 0 0.06" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit lower="${-PI}" upper="${PI}" effort="10" velocity="3.0"/>
  </joint>

  <!-- ====== 平行夹爪 ====== -->
  <!-- 夹爪不在本文件里画，而是用 xacro 宏从 gripper.xacro 引入，
       并挂到 link5(腕) 上；这样「臂」和「手」可以分开维护 -->
  <xacro:include filename="$(find pick_place_robot)/urdf/gripper.xacro"/>
  <xacro:parallel_gripper parent="link5"/>

  <!-- ====== Gazebo 仿真 ====== -->
  <!-- 下面几行只是给 Gazebo 仿真器指定每个连杆的显示材质颜色，
       不影响运动学，纯粹为了 RViz/Gazebo 里好看，可跳过 -->
  <!-- ...（6 个连杆的 Gazebo 材质标签：DarkGrey/Blue×3/Orange×2，
       纯外观设置，照抄即可）-->

</robot>

读到这里你可能会想：为什么不用一个文件写完，非要拆出 xacro 宏？答案是复用和参数化——xacro 允许你用变量（PI）和宏（parallel_gripper）生成重复结构，真实工业臂有几十个关节时，手写 XML 会疯掉。我们的夹爪就是靠 <xacro:parallel_gripper parent="link5"/> 这一行被「长」到腕关节上的。记住这个 parent="link5"：后面 SRDF 和抓取规划里说的「末端 link5」，指的就是这里。

2.2 平行夹爪

urdf/gripper.xacro：

上面用了一行 <xacro:parallel_gripper parent="link5"/> 把夹爪挂上去，这段就是那个宏的定义。它描述了一个「平行夹爪」：两根手指各自是一个小滑块（prismatic 关节，只能沿直线平移），右手指用 <mimic> 标签镜像跟随左手指——你只控制一根，另一根自动反向等距移动，保证两指平行开合。

这段真正的核心是三点：gripper_base 通过 fixed 关节固定在 link5；left_finger_joint 的 limit 给出开合行程 0~0.02 m（2 cm）；mimic 那行让右指 multiplier="-1" 反向联动。其余都是手指的方块外形和仿真摩擦系数，第一遍可略读。

<?xml version="1.0"?>
<robot xmlns:xacro="https://www.ros.org/wiki/xacro">

  <!-- 定义一个叫 parallel_gripper 的宏，参数 parent 表示挂到哪个连杆 -->
  <xacro:macro name="parallel_gripper" params="parent">

    <!-- 夹爪底座：fixed 关节把它焊死在 parent(也就是 link5) 上 -->
    <link name="gripper_base">
      <!-- ...（gripper_base 的 visual/collision/inertial 三件套省略：
           0.06×0.04×0.03 m 银色小方块，质量 0.1 kg，模板同上）-->
    </link>

    <joint name="gripper_joint" type="fixed">
      <parent link="${parent}"/>
      <child link="gripper_base"/>
      <origin xyz="0 0 0.04" rpy="0 0 0"/>
    </joint>

    <!-- 左夹爪手指 -->
    <!-- 手指是细长方块；prismatic 表示只能沿 axis 方向直线滑动，不是旋转 -->
    <link name="left_finger">
      <!-- ...（left_finger 的三件套省略：0.005×0.04×0.06 m 细长条，
           prismatic 关节 limit 0~0.02m 控制行程）-->
    </link>

    <!-- left_finger_joint 是真正被控制的关节：行程 0(闭合)~0.02(张开) 米 -->
    <joint name="left_finger_joint" type="prismatic">
      <parent link="gripper_base"/>
      <child link="left_finger"/>
      <origin xyz="0.0125 0 -0.01" rpy="0 0 0"/>
      <axis xyz="1 0 0"/>
      <limit lower="0.0" upper="0.02" effort="10" velocity="0.1"/>
    </joint>

    <!-- 右夹爪手指：形状与左指镜像对称，定义省略 -->
    <link name="right_finger">
      <!-- ...（right_finger 三件套省略：与 left_finger 镜像，x 取负）-->
    </link>

    <!-- 右指用 mimic 镜像左指：multiplier=-1 表示反向、offset=0 等距，
         于是控制一根手指，另一根自动对称开合 -->
    <joint name="right_finger_joint" type="prismatic">
      <parent link="gripper_base"/>
      <child link="right_finger"/>
      <origin xyz="-0.0125 0 -0.01" rpy="0 0 0"/>
      <axis xyz="1 0 0"/>
      <limit lower="-0.02" upper="0.0" effort="10" velocity="0.1"/>
      <mimic joint="left_finger_joint" multiplier="-1" offset="0.0"/>
    </joint>

    <!-- Gazebo 夹爪插件 -->
    <!-- mu1/mu2 是仿真里的摩擦系数，太滑会抓不住物体，0.5 是个稳妥起步值 -->
    <gazebo reference="left_finger">
      <mu1>0.5</mu1>
      <mu2>0.5</mu2>
      <material>Gazebo/Silver</material>
    </gazebo>
    <gazebo reference="right_finger">
      <mu1>0.5</mu1>
      <mu2>0.5</mu2>
      <material>Gazebo/Silver</material>
    </gazebo>

  </xacro:macro>

</robot>

这里有个容易踩的坑：夹爪开合行程只有 2 cm，意味着它只能抓 2 cm 以内的薄物体。后面检测节点里假设物体高度 4 cm、抓取宽度也得落在这个范围内，否则仿真里会「手指够不到」。真实项目里这个值要根据你买的夹爪规格改。

关于 prismatic（平移）关节再多说一句：它和 revolute（旋转）关节在 MoveIt2 里待遇一样，都能进规划组、都能设约束，区别在于自由度类型——平移关节改变的是位置而非角度。我们的夹爪只用一个平移自由度控制开合，简单可靠；复杂二指/多指手会用到更多关节和力控，但入门先用这个「一根手指带动另一根」的 mimic 方案最省心。

如果你后面想换真夹爪，重点改两处：本文件的 limit（行程）和 mimic 的 multiplier（若新夹爪不是对称开合，系数要改），以及 SRDF 里 open/close 两个状态的关节值要和新行程对应，否则「代码说张开、实际夹死」。

关于摩擦 mu1/mu2：仿真里夹爪靠摩擦力「夹住」物体，系数太低物体一抬就滑落。0.5 是稳妥起步值，真抓不住就往上加；但太高在仿真里又会出现「物体被粘在手指上甩不掉」，所以也是个要试的数。

2.3 SRDF 语义配置（MoveIt2 Setup Assistant 生成后手动修改）

URDF 只描述机械臂「长什么样、怎么动」，SRDF 则告诉 MoveIt2「怎么用它」。打个比方：URDF 是机械臂的户口本（姓名性别身高），SRDF 是使用说明书（哪些关节组成手臂、预设几个常用姿势、哪些相邻零件不用做碰撞检测）。它在这里干三件事：

1. 定义规划组（planning group）：把 5 个旋转关节归到 arm 组、把夹爪关节归到 gripper 组，MoveIt2 规划时按组操作；
2. 声明几个预设姿态（home/ready/pick_approach/place_approach/open/close），相当于给常见动作起好名字，后面 Python 里一句「去 ready 姿态」就能调用，不用每次重填 5 个关节角；
3. 关掉相邻连杆之间没意义的自碰撞检测（比如大臂和自己小臂永远贴着，没必要每帧检测），能大幅省算力。

小知识：MoveIt2 就是机械臂的「大脑」——你给它一个目标位姿，它负责算出每个关节该转到多少度、走哪条不撞桌子的轨迹。后面所有 Python 规划调用，最终都交给它。

config/pick_place_robot.srdf：

这段配置里，你第一遍只需关注三处：group（规划组定义）、group_state（预设姿态，记住 ready/pick_approach/place_approach 这几个名字，后面抓取流程直接点名用）、disable_collisions（自碰撞豁免，可整段跳过，它是 Setup Assistant 自动生成的）。其余按注释读即可。

<?xml version="1.0"?>
<robot name="pick_place_robot">

  <!-- 规划组 -->
  <!-- arm 组用 chain 从 base_link 链到 link5，意思是「这 5 节连杆算一条手臂」 -->
  <group name="arm">
    <chain base_link="base_link" tip_link="link5"/>
  </group>

  <!-- gripper 组只含左手指关节；因为右指是 mimic 镜像，控制左指就够了 -->
  <group name="gripper">
    <joint name="left_finger_joint"/>
  </group>

  <!-- 末端执行器：声明 link5 上的 gripper 组是「手」，MoveIt2 才把它当 EEF 处理 -->
  <end_effector name="gripper_eef" parent_link="link5" group="gripper"/>

  <!-- 自碰撞矩阵（禁用相邻连杆的碰撞检测） -->
  <!-- 下面 9 行是自动生成的「相邻连杆免检」清单，全部 Adjacent 理由，可整段跳过 -->
  <disable_collisions link1="base_link" link2="link1" reason="Adjacent"/>
  <!-- ...（其余 7 对相邻连杆的免检声明省略：link1~link5 与手指两两相邻对，
       Setup Assistant 自动生成，无需手写）-->

  <!-- 抓取位姿生成器（Pick 的预定义候选姿态） -->
  <!-- home：所有关节归零的中立站姿，开机默认停这 -->
  <group_state name="home" group="arm">
    <!-- ...（5 个关节全 0 的声明省略）-->
  </group_state>

  <!-- ready：手臂微微前倾待命，是抓取流程的「起点姿势」 -->
  <group_state name="ready" group="arm">
    <joint name="joint1" value="0"/>
    <joint name="joint2" value="-0.5"/>
    <joint name="joint3" value="1.0"/>
    <joint name="joint4" value="-0.5"/>
    <joint name="joint5" value="0"/>
  </group_state>

  <!-- pick_approach：手臂压低前伸，是「移到物体上方」时的典型肩肘角度 -->
  <group_state name="pick_approach" group="arm">
    <!-- ...（joint1=0, joint2=-1.0, joint3=1.8, joint4=-0.8, joint5=0 省略格式同上）-->
  </group_state>

  <!-- place_approach：关节1 转 90°，把物体转到另一侧放置区上方 -->
  <group_state name="place_approach" group="arm">
    <!-- ...（joint1=1.57 其余同 ready，省略格式同上）-->
  </group_state>

  <!-- 夹爪状态 -->
  <!-- open：左指行程拉满 0.02m = 张开；后面 Python 直接点这个状态名开爪 -->
  <group_state name="open" group="gripper">
    <joint name="left_finger_joint" value="0.02"/>
  </group_state>

  <!-- close：行程 0 = 闭合 -->
  <group_state name="close" group="gripper">
    <joint name="left_finger_joint" value="0.0"/>
  </group_state>

</robot>

到这里，机械臂的「身体」和「大脑接口」就备齐了。你可能会注意到：这些预设姿态的关节角度（ready 的 -0.5/1.0/-0.5 之类）是拍脑袋加仿真调出来的，不是算出来的。第一遍跑通后，你可以在 RViz 里手动拖关节、看机械臂姿态顺不顺眼，再回来改这几个数——这就是 MoveIt2 Setup Assistant 的常规工作流。

disable_collisions 这一长串千万别手删——它是 Setup Assistant 根据你模型算出来的「哪些连杆天生就挨着、不必每帧检测」。手删可能把不该免检的连杆也放进来（比如让大臂和小臂之间本该有的自碰撞被忽略），或者反过来保留大量无意义检测拖慢规划。正确做法是回到 Setup Assistant 里勾选/取消，让它重新生成这一段。

还有个实用技巧：预设姿态不一定只在 SRDF 里写死。本文为了清晰把 ready/pick_approach 等同时写进了 SRDF 和 Python 代码（两份），真实工程建议只信 SRDF 一份，Python 里用 moveit_msgs 去读 SRDF 的状态，避免两处不同步——改一个忘了另一个就会「代码说去 ready，实际去了别的地方」。本文写两份是为了让你肉眼看到姿态长什么样，知道数值从哪来。

三、MoveIt2 感知管线配置

身体有了，接下来给它接「眼睛」和「环境认知」。这一章全是配置文件（YAML），没有算法逻辑，但每一个参数都直接决定后面抓取稳不稳。我会在每个文件前告诉你「只需关注哪几行」，剩下的参数保持默认值通常就能跑，不必逐行死记。先讲清 YAML 在 ROS 2 里怎么生效：这些 .yaml 不是被 Python 读，而是被各自的节点在启动时通过 parameters=[...] 加载成节点的「参数服务器」值。所以你改了 YAML，必须重启对应 launch / 节点才生效，热改文件没用——这是新手最常犯的「我改了配置怎么没反应」。

我会在每个文件前告诉你「只需关注哪几行」，剩下的参数保持默认值通常就能跑，不必逐行死记。先建立这个「默认值能跑、按需再调」的心态，你就从「怕配置」变成「敢调配置」了。

3.1 深度相机驱动

RealSense D435 启动参数（config/realsense.yaml）：

这段是 Intel RealSense D435 深度相机的驱动参数。它告诉驱动节点：以多大分辨率、多少帧率输出彩色图和深度图，以及要不要把深度图对齐到彩色图坐标系。你只需关注三行：enable_pointcloud: true（必须开，后面 Octomap 和点云检测都靠它）、align_depth: true（把深度对齐到彩色，否则颜色和深度对不齐，测距会偏）、base_frame_id（相机坐标系名，后面 TF 变换要对上）。其余分辨率/帧率按机器性能调，默认即可。

realsense2_camera:
  ros__parameters:
    serial_no: ''                 # 空=自动连第一个相机；多相机时填序列号区分
    color_fps: 30
    depth_fps: 30
    color_width: 640
    color_height: 480
    depth_width: 640
    depth_height: 480
    enable_depth: true
    enable_color: true
    enable_pointcloud: true       # ★ 关键：开点云，Octomap 和检测节点都依赖它
    pointcloud_qos: "SENSOR_DATA"
    depth_qos: "SENSOR_DATA"
    color_qos: "SENSOR_DATA"
    align_depth: true             # ★ 关键：深度对齐到彩色坐标系，否则测距错位
    publish_tf: true
    tf_publish_rate: 30.0
    base_frame_id: "camera_link"  # ★ 关键：相机坐标系名，TF 树上要对得上
    depth_frame_id: "camera_depth_frame"
    color_frame_id: "camera_color_frame"

跑起来后，用 ros2 topic list | grep camera 应该能看到 /camera/color/image_raw、/camera/depth/image_raw、/camera/depth/color/points 等话题——这几个话题名后面检测节点要订阅，名字对不上就收不到图。

在讲 Octomap 配置前，先说清楚它到底解决什么问题。如果没有 Octomap，机械臂就是「盲抓」：它只知道目标物体的位姿，却不知道桌子、墙壁、其他障碍物在哪，规划出来的轨迹很可能擦着桌面过去甚至穿桌。Octomap 把相机持续扫到的点云累积成一张立体概率地图，标出哪儿「大概率有东西」，MoveIt2 规划时就绕着这些 occupied 格子走。

代价是算力：地图越精细（resolution 越小）、范围越大（pointcloud_max_z 越高），更新越慢。所以 Octomap 的参数本质是在「避障精度」和「实时性」之间找平衡，这也是下面配置里你最该理解 resolution 和 filter_ground 的原因。

3.2 Octomap 参数

config/octomap.yaml：

这段文件其实混了两件事：上半部分是控制器管理器的声明（告诉 MoveIt2 手臂和夹爪分别由哪个 action 接口控制），下半部分是 Octomap 3D 占据地图的参数。Octomap 是机械臂的「空间记忆」：它把相机扫到的点云一点点累积成一张立体概率地图，标出哪儿有桌子、哪儿有障碍物，MoveIt2 规划时就绕着这些 occupied 格子走。

你只需关注四行：resolution（体素大小，0.05 m = 5 cm，越小越精细也越慢）、filter_ground: true（过滤地面，否则地图会把整个地板当障碍）、pointcloud_max_z（只关心桌面高度以下，省算力）、以及控制器部分的 joints 列表（必须和 URDF 里的关节名完全一致）。

moveit_simple_controller_manager:
  controller_names:
    - arm_controller
    - gripper_controller

# arm_controller 用 FollowJointTrajectory：MoveIt2 规划出轨迹后，
# 通过 action 把整条关节轨迹下发给 ros2_control 执行
arm_controller:
  type: FollowJointTrajectory
  action_ns: follow_joint_trajectory
  default: true
  joints:
    - joint1
    - joint2
    - joint3
    - joint4
    - joint5

# gripper_controller 用 GripperCommand：夹爪只要「目标开度+最大力」两个量
gripper_controller:
  type: GripperCommand
  action_ns: gripper_action
  default: true
  joints:
    - left_finger_joint

# Octomap 配置（3D 占据地图，用于碰撞检测）
octomap_server:
  ros__parameters:
    resolution: 0.05                    # ★ 体素分辨率 5cm；抓桌面小物 0.05 够用
    range_max: 5.0                      # 最大检测距离
    probability_hit: 0.7                # 命中概率
    probability_miss: 0.4               # 未命中概率
    clamping_thres_min: 0.12            # 最小概率阈值
    clamping_thres_max: 0.97            # 最大概率阈值
    sensor_model_type: "ray"            # 射线模型
    max_ray_update_length: 0.0
    pointcloud_min_z: 0.05              # 过滤地面以下点云
    pointcloud_max_z: 2.0               # ★ 只建模 2m 以下，桌面以上不浪费算力
    occupied_mark_thresh: 0.7
    free_mark_thresh: 0.4
    filter_speckles: true
    filter_ground: true                 # ★ 关键：滤掉地面，否则整块地板都成障碍
    ground_filter_distance: 0.04
    ground_filter_angle: 0.15
    ground_filter_plane_distance: 0.07

一个常见翻车点：如果忘了 filter_ground: true，Octomap 会把整个地面当成实心障碍，机械臂一伸手就全程碰撞报警、规划失败。调试时打开 RViz 的 Octomap 显示，肉眼确认地图里只有桌子和物体、没有「地板墙」。

3.3 MoveIt2 启动配置

config/moveit_controllers.yaml：

这是 MoveIt2 的「运动规划器说明书」，也是本章最长的一段。它干了三件事：列出可用的规划管线（ompl 和 pilz）、给 OMPL 配几种规划算法（RRT*/RRTConnect/PRM*/TRRT，相当于不同的「抄近路策略」），最后设全局安全和重规划参数。

你第一遍只需抓住三行：default_planning_pipeline: "ompl"（默认用哪种规划器）、planning_time_limit: 10.0（最多想 10 秒，超时放弃）、以及 max_velocity_scaling_factor: 0.3（全局限速 30%，宁可慢点也别让臂甩出去）。中间那一大坨 planner_configs 是各算法的微调参数，默认值就能用，等真遇到「复杂环境规划不出来」再回来调。

# MoveIt2 运动规划器配置
planning_pipelines:
  pipeline_names: ["ompl", "pilz"]

# OMPL 规划器
ompl_planning:
  planning_plugin: "ompl_interface/OMPLPlanner"
  request_adapters: >-
    default_planner_request_adapters/AddTimeParameterization
    default_planner_request_adapters/ResolveConstraintFrames
  start_state_max_bounds_error: 0.1
  planning_time_limit: 10.0         # ★ 最多规划 10 秒，超时返回失败
  max_iterations: 200
  max_solution_segment_length: 0.1

  planner_configs:
    # RRT*（适合有障碍物的复杂环境，能优化出较短路）
    RRTstar:
      type: geometric::RRTstar
      range: 0.0           # 自动选择步长
      goal_bias: 0.05      # 朝目标偏置概率，太小随机游走、太大不探索
      prune_threshold: 0.05
      use_k_nearest: true

    # RRT Connect（速度快，适合无障碍简单场景，本文主用）
    RRTConnect:
      type: geometric::RRTConnect
      range: 0.0

    # PRM（适合多查询场景，先建路线图再反复查询）
    PRMstar:
      type: geometric::PRMstar
      max_nearest_neighbors: 10

    # TRRT（带温度退火，适合高维空间）
    TRRT:
      type: geometric::TRRT
      temp_change_factor: 0.1
      min_temperature: 1e-10
      init_temperature: 1e-6
      goal_bias: 0.95

# Pilz 规划器（工业风格，直线/圆弧运动，适合已知路径的精确定位）
pilz_industrial_motion_planner:
  planning_plugin: pilz_industrial_motion_planner/CommandPlanner
  request_adapters: >-
    default_planner_request_adapters/ResolveConstraintFrames
  capabilities: pilz_industrial_motion_planner/MoveGroupSequenceAction
    pilz_industrial_motion_planner/PlanningContextLoaderPTP
    pilz_industrial_motion_planner/PlanningContextLoaderLIN
    pilz_industrial_motion_planner/PlanningContextLoaderCIRC

  PTP:
    planning_limits:
      velocity_scaling_factor: 0.3
      acceleration_scaling_factor: 0.2
  LIN:
    planning_limits:
      velocity_scaling_factor: 0.2
      max_translational_velocity: 0.5
      max_translational_acceleration: 1.0

# 运动规划默认设置
move_group:
  move_group_capabilities: >-
    move_group/MoveGroupCartesianPathService
    move_group/ApplyPlanningSceneService
    move_group/ClearOctomapService

  default_planning_pipeline: "ompl"     # ★ 默认规划管线
  default_planning_attempts: 5          # 一次目标最多试 5 次规划，提高成功率
  max_velocity_scaling_factor: 0.3      # ★ 全局限速 30%，安全优先
  max_acceleration_scaling_factor: 0.2  # 加速度也限，避免急启急停抖晃

  # Cartesian 路径参数
  cartesian_path_interpolation_step: 0.005  # 笛卡尔路径插步 5mm，越小越平滑
  enable_lookahead: true
  enable_replanning: true              # 执行中若场景变了，自动重新规划
  replan_delay: 0.1

关于那几个规划算法，给个直观理解：RRTConnect 像「从起点和终点同时撒网、网碰上了就连线」，最快但有随机性；RRT* 在 RRT 基础上不断「优化已有路径」，更短更优雅但慢；Pilz 的 LIN/PTP 是工业机器人的「直线走 / 点对点走」，不找捷径、但走位精准可复现。入门阶段用默认 OMPL + RRTConnect 足矣。

default_planning_attempts: 5 和 replan_attempts 是「成功率保险」：MoveIt2 采样规划有随机性，一次失败不代表真不可达，多试几次往往就成功了。但别无限加——每次尝试都耗 planning_time_limit 秒，5 次失败就是 50 秒干等，机器人像死机。真遇到频繁失败，优先怀疑目标不可达或碰撞约束太紧，而不是狂加尝试次数。

enable_replanning: true 让执行中若场景突变（比如有人手伸进工作区）能自动重规划绕开。但它要求实时感知在更新 Octomap，否则重规划基于过期地图，照样撞。感知和规划是咬合的齿轮，单改一边没用。

cartesian_path_interpolation_step: 0.005 决定笛卡尔直线运动被切成多细的小段去规划。太小段数爆炸、规划慢；太大路径折线感强、可能穿模。5 mm 是平滑与速度的折中，做精细贴合（如插拔、擦拭）时调更小。

这些参数你先照默认值跑，等真机调试时再一个个动——记住改动前备份一份，参数文件改坏比代码改坏更难排查，因为它不报错、只是「行为诡异」。

3.4 规划器怎么选：一张实战决策树

配置里塞了 RRT*、RRTConnect、PRM*、TRRT 四种 OMPL 算法，新手常问「我该用哪个」。给一张按场景选的决策树，比背定义管用：

• 场景空旷、只要最快拿到一条能走的路 → RRTConnect。它是本文主用，双向撒网、连通即停，速度最快，代价是路径不一定最优、可能抖。
• 场景有障碍、且希望路径短而优雅 → RRT*。在 RRT 基础上持续「剪枝优化」，越算越短，但耗时更长，适合离线或在乎能耗/时间的场合。
• 同一环境要反复规划很多次（如分拣线） → PRM*。先花时间建一张「路线图」，之后每次查询极快，建图贵、查询便宜。
• 高维/狭窄通道（如臂要钻进货架缝隙） → TRRT。带模拟退火，擅长逃出局部，但调参麻烦。

实操建议：先用 RRTConnect 把全流程跑通，确认逻辑对；再遇到「复杂环境规划慢/失败」才换 RRT* 或调 goal_bias/range。别一上来就纠结算法——规划器只是「怎么走」，前提是你给的目标位姿、碰撞约束、工作空间都合理，否则换哪个算法都救不了。

另外 pipeline_names: ["ompl", "pilz"] 里 Pilz 是另一类思路：它不「找路径」而是「按指令走」（PTP 点对点、LIN 直线、CIRC 圆弧），适合路径已知、要求可复现的工业动作（如每次都走同一条直线插装）。本文抓取用 OMPL 的自由规划更合适，Pilz 留作你做「固定轨迹精确定位」时启用。

四、物体检测与位姿估计

配置都就位了，现在写第一段真正干活的 Python——感知节点。它要解决的核心问题是：相机看见一张图，怎么变成「物体在机械臂坐标系里的 6DoF 位姿」？这条链路是：彩色图做颜色分割找出红物体 → 深度图读出该像素的距离 → 用相机内参把「像素+深度」反算成相机坐标系下的 3D 点 → 用 TF 把这个点变换到 base_link（机械臂基座）坐标系 → 打包成位姿发出去。

4.1 基于深度相机的物体检测节点

pick_place_perception/detection_node.py：

下面这段是全文最长的代码之一，但逻辑是一条直线，别被行数吓到。我把它拆成几段讲：初始化（建好发布/订阅和参数）、回调存图（把最新的彩色/深度/点云存起来）、detect_objects（核心：分割→测距→换算→发结果）、定时器入口。detect_objects 里你重点看三处：HSV 颜色分割（怎么从图里圈出红物体）、深度反算 3D（像素+深度→相机坐标）、TF 变换（相机坐标→基座坐标）。其余订阅、发布都是 ROS 2 的标准套路。

#!/usr/bin/env python3
"""基于深度相机的物体检测与位姿估计节点"""

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
from sensor_msgs.msg import Image, CameraInfo, PointCloud2
from geometry_msgs.msg import PoseStamped, PoseArray
from vision_msgs.msg import Detection3DArray, Detection3D, BoundingBox3D
from std_msgs.msg import Header
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListener
from tf2_ros import TransformException
import numpy as np
import cv2
from cv_bridge import CvBridge
import struct
import threading


class ObjectDetectionNode(Node):
    def __init__(self):
        super().__init__('object_detection_node')

        # CvBridge 负责 ROS 图像消息 <-> OpenCV 矩阵互转，是视觉节点的标配
        self.bridge = CvBridge()
        # tf_buffer + listener 用来查询「相机坐标系 -> 基座坐标系」的实时变换
        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)

        # 检测结果
        # 两个发布器：Detection3DArray 给别的模块用，PoseArray 给后面的抓取节点用
        self.detected_objects_pub = self.create_publisher(
            Detection3DArray, '/detected_objects', 10)
        self.pose_array_pub = self.create_publisher(
            PoseArray, '/object_poses', 10)

        # 可视化
        self.debug_image_pub = self.create_publisher(
            Image, '/detection_debug_image', 10)

        # QoS - 深度相机使用 Best Effort
        # 图像/点云量大且允许丢帧，用 BEST_EFFORT 而非 RELIABLE，避免积压卡死
        sensor_qos = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,
            history=HistoryPolicy.KEEP_LAST,
            depth=1,
        )

        # 相机内参(CameraInfo)小而关键、不能丢，用 RELIABLE 保送达
        reliable_qos = QoSProfile(
            reliability=ReliabilityPolicy.RELIABLE,
            history=HistoryPolicy.KEEP_LAST,
            depth=1,
        )

        # 订阅
        # 彩色图：用来做颜色分割找物体轮廓
        self.color_sub = self.create_subscription(
            Image, '/camera/color/image_raw',
            self.color_callback, sensor_qos)

        # 深度图：用来读物体到相机的距离
        self.depth_sub = self.create_subscription(
            Image, '/camera/depth/image_raw',
            self.depth_callback, sensor_qos)

        # 相机内参：含焦距 fx/fy 和光心 cx/cy，深度反算 3D 必须用它
        self.camera_info_sub = self.create_subscription(
            CameraInfo, '/camera/color/camera_info',
            self.camera_info_callback, reliable_qos)

        # 点云：备用，本文主要用「深度图单点」而非整云，订阅但不强依赖
        self.pointcloud_sub = self.create_subscription(
            PointCloud2, '/camera/depth/color/points',
            self.pointcloud_callback, sensor_qos)

        # 状态
        # 用最新一帧的图；frame_lock 防止回调线程和检测线程同时改这些变量
        self.latest_color = None
        self.latest_depth = None
        self.camera_info = None
        self.latest_pointcloud = None
        self.frame_lock = threading.Lock()

        # 检测参数
        # 面积过滤：太小是噪点、太大超出抓取范围，都跳过
        self.min_object_area = 500         # 最小像素面积
        self.max_object_area = 50000       # 最大像素面积
        # 工作空间：只认机械臂够得着的 XYZ 范围，范围外的一律当背景
        self.workspace_x = [0.1, 0.5]     # 工作空间 X 范围（米）
        self.workspace_y = [-0.3, 0.3]    # 工作空间 Y 范围
        self.workspace_z = [0.0, 0.3]     # 工作空间 Z 范围
        self.grasp_height_offset = 0.02   # 抓取高度偏移（爪尖略高于物体顶面）

        self.get_logger().info('物体检测节点已启动')

上面 __init__ 一次性把「接口」都建好了，三个细节值得停一下：

• 两个发布器（/detected_objects 和 /object_poses）内容几乎一样、格式不同——前者是标准检测消息给通用工具链用，后者是轻量位姿数组给我们自己的抓取节点用。发布两种格式是 ROS 2 社区的常见做法：标准格式换生态兼容，轻量格式换性能。
• QoS 双轨制是刻意的：图像用 BEST_EFFORT（丢帧无所谓、延迟要低），内参用 RELIABLE（一帧都不能丢）。如果你以后遇到「订阅端死活收不到图」，九成是 QoS 不匹配——发布端 RELIABLE、订阅端 BEST_EFFORT 能通，反过来就静默失败。
• 工作空间过滤（workspace_x/y/z）在检测阶段就把「够不着的物体」筛掉，比抓取时才发现规划失败快得多。这是「把失败提前」的典型工程思维。

接着看核心的检测循环。先是颜色分割——把「红色」从画面里抠出来：

def camera_info_callback(self, msg: CameraInfo):
    self.camera_info = msg

def color_callback(self, msg: Image):
    # 只存引用、不在此处处理，处理集中到 detect_objects 里做
    with self.frame_lock:
        self.latest_color = msg

def depth_callback(self, msg: Image):
    with self.frame_lock:
        self.latest_depth = msg

def pointcloud_callback(self, msg: PointCloud2):
    self.latest_pointcloud = msg

def detect_objects(self):
    """同步检测：取最新一帧彩色+深度，算出物体位姿并发布"""
    with self.frame_lock:
        color = self.latest_color
        depth = self.latest_depth

    if color is None or depth is None:
        return

    # 把 ROS 图像消息转成 OpenCV 矩阵；深度用 passthrough 保留原始数值
    color_img = self.bridge.imgmsg_to_cv2(color, 'bgr8')
    depth_img = self.bridge.imgmsg_to_cv2(depth, 'passthrough')

    # ====== 颜色分割检测 ======
    # 转 HSV 是因为颜色在 HSV 里比 RGB 更不受光照影响，阈值更好定
    hsv = cv2.cvtColor(color_img, cv2.COLOR_BGR2HSV)

    # 红色在 HSV 色环上跨 0 度，被切成两段，所以要用两个区间再取并集
    lower_red1 = np.array([0, 100, 100])
    upper_red1 = np.array([10, 255, 255])
    lower_red2 = np.array([160, 100, 100])
    upper_red2 = np.array([180, 255, 255])

    mask1 = cv2.inRange(hsv, lower_red1, upper_red1)
    mask2 = cv2.inRange(hsv, lower_red2, upper_red2)
    mask = cv2.bitwise_or(mask1, mask2)

    # 形态学开+闭运算：先去小噪点(开)、再补物体内部空洞(闭)
    kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (5, 5))
    mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, kernel, iterations=2)
    mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, kernel, iterations=2)

颜色分割拿到的是「哪些像素是红的」，接下来这个循环把每个红块变成一个 3D 目标。这段是全节点的心脏，四个步骤环环相扣：面积过滤 → 深度读值 → 内参反算 → 坐标变换。每一步的 continue 都是在主动放弃坏数据——噪点、深度丢失、超出工作空间，宁可少抓不可抓错。深度反算那句 (cx - cx_cam) * z / fx 是针孔相机模型的逆投影，值得盯着看一分钟：像素离光心越远、或物体离相机越远，算出来的 x/y 就越大。

        # 找轮廓：每个连通白区就是一个候选物体
        contours, _ = cv2.findContours(
            mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)

        detections = Detection3DArray()
        detections.header = color.header
        poses = PoseArray()
        poses.header = color.header

        debug_img = color_img.copy()

        for contour in contours:
            area = cv2.contourArea(contour)
            # 面积过滤：太小是噪点、太大超出抓取范围
            if area < self.min_object_area or area > self.max_object_area:
                continue

            # 取最小外接矩形：得到中心点像素和旋转角，后面算姿态用
            rect = cv2.minAreaRect(contour)
            center = rect[0]  # (x, y) 像素坐标
            size = rect[1]    # (w, h)

            # 从深度图获取 3D 位置
            cx, cy = int(center[0]), int(center[1])
            # 边界保护：中心点越界就跳过，否则索引报错
            if cx < 0 or cx >= depth_img.shape[1]:
                continue
            if cy < 0 or cy >= depth_img.shape[0]:
                continue

            depth_value = depth_img[cy, cx]
            # 深度为 0 或 NaN 通常是红外没返回（反光/超出量程），必须跳过
            if depth_value <= 0 or np.isnan(depth_value):
                continue

            # 像素坐标转 3D（需要相机内参）
            if self.camera_info is None:
                continue

            # 相机内参矩阵 K 的 6 个关键值：fx,fy 焦距，cx,cy 光心
            fx = self.camera_info.k[0]  # fx
            fy = self.camera_info.k[4]  # fy
            cx_cam = self.camera_info.k[2]
            cy_cam = self.camera_info.k[5]

            # 3D 坐标（相机坐标系）
            # 针孔相机模型反算：z 是深度，x/y 由「像素偏离光心 * 深度 / 焦距」
            z = depth_value / 1000.0  # mm → m（RealSense 深度单位是毫米）
            x_3d = (cx - cx_cam) * z / fx
            y_3d = (cy - cy_cam) * z / fy

            # 工作空间过滤
            # 只认机械臂够得着的 XYZ，范围外的当背景，避免去抓够不到的东西
            if not (self.workspace_x[0] <= x_3d <= self.workspace_x[1] and
                    self.workspace_y[0] <= y_3d <= self.workspace_y[1] and
                    self.workspace_z[0] <= z <= self.workspace_z[1]):
                continue

            # 计算物体角度（绕 Z 轴的偏航，让夹爪对正物体长边）
            angle = rect[2]
            if size[0] < size[1]:
                angle += 90

            # 创建 3D 检测结果
            det = Detection3D()
            det.header = color.header

            pose = PoseStamped()
            pose.header = color.header

            try:
                # 变换到 base_link 坐标系
                # 查「相机帧 -> 基座帧」的 TF；超时 0.1s 拿不到就走 except 兜底
                transform = self.tf_buffer.lookup_transform(
                    'base_link', color.header.frame_id,
                    rclpy.time.Time(),
                    timeout=rclpy.duration.Duration(seconds=0.1))

                # 应用变换（简化：仅平移）
                # 严谨做法是 4x4 矩阵乘，但这里近似只加平移量，够入门用
                pose.pose.position.x = x_3d + transform.transform.translation.x
                pose.pose.position.y = y_3d + transform.transform.translation.y
                pose.pose.position.z = z + transform.transform.translation.z
            except TransformException:
                # TF 还没准备好时的兜底：直接用相机坐标，至少不崩
                pose.pose.position.x = x_3d
                pose.pose.position.y = y_3d
                pose.pose.position.z = z

            # 旋转：仅绕 Z 轴（平面抓取不需要俯仰/翻滚）
            # 四元数表达绕 Z 转 angle：z=sin(a/2), w=cos(a/2)
            import math
            yaw = math.radians(angle)
            pose.pose.orientation.z = math.sin(yaw / 2)
            pose.pose.orientation.w = math.cos(yaw / 2)

            # Bounding Box
            bbox = BoundingBox3D()
            bbox.center = pose.pose
            # 物体实际宽高 ≈ 像素尺寸 * 深度 / 焦距（小孔模型同上的投影反算）
            bbox.size.x = size[0] * z / fx   # 近似实际尺寸
            bbox.size.y = size[1] * z / fy
            bbox.size.z = 0.04                 # 假设物体高度 4cm，按实物体改

            det.bbox = bbox
            det.results.append(
                type='red_object',  # 标签：本文只认红色物体
                id=0,
                score=float(area) / self.max_object_area
            )

            detections.detections.append(det)
            poses.poses.append(pose.pose)

            # 可视化绘制：在调试图上画框+中心点+坐标，方便肉眼确认抓哪
            box = cv2.boxPoints(rect)
            box = np.int0(box)
            cv2.drawContours(debug_img, [box], 0, (0, 255, 0), 2)
            cv2.circle(debug_img, (cx, cy), 5, (0, 0, 255), -1)
            cv2.putText(debug_img,
                f'({x_3d:.2f}, {y_3d:.2f}, {z:.2f})',
                (cx + 10, cy - 10),
                cv2.FONT_HERSHEY_SIMPLEX, 0.4, (0, 255, 0), 1)

        # 发布检测结果
        self.detected_objects_pub.publish(detections)
        self.pose_array_pub.publish(poses)

        # 发布调试图像（在 RViz 或 rqt_image_view 看，确认识别对不对）
        debug_msg = self.bridge.cv2_to_imgmsg(debug_img, 'bgr8')
        self.debug_image_pub.publish(debug_msg)

        # ...（日志打印部分省略：循环 detections 打印每个物体的 pos/size，
        #      纯样板代码，照抄即可）


def main():
    rclpy.init()
    node = ObjectDetectionNode()

    # 检测定时器（5Hz）：每 0.2 秒跑一次 detect_objects，不必等话题触发
    timer = node.create_timer(0.2, node.detect_objects)

    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()

把这段跑起来后，你应该能在终端看到 检测到 N 个物体 并打印每个物体的 (x, y, z) 坐标；打开 /detection_debug_image 话题，能看到红物体在画面上被绿框圈出、中心标着红点。如果识别不到，优先检查三件事：场景里有没有足够红的物体、相机话题名是否和订阅一致、camera_info 是否到位（没有它无法反算 3D）。

需要点破的一个工程现实：这里用的是最朴素的颜色分割，不是深度学习检测。它胜在零训练、立即可跑，但只认纯色物体、怕光照变化。真要做「认任意物体」，把 detect_objects 里的颜色分割换成 YOLO 推理、保留后面「深度反算 3D + TF 变换」即可，主干不变——这也是为什么我把测距和坐标变换单独写清楚。

五、Pick & Place 工作流

感知给了「物体在哪」，大脑（MoveIt2）会算「手该怎么伸过去」，这一章就是把两者接起来、串成完整动作。这里会碰到一个核心概念：逆运动学（IK）——你知道手要伸到某个 3D 位姿，但臂有 5 个关节，反推每个关节该转多少度才能把手送到那？MoveIt2 帮我们解了这道题。本文为了稳，目标姿态多用「预定义命名姿态 + 笛卡尔约束」，省去实时解算的麻烦；等你熟了，可以把 pick_approach 这类固定姿态也换成实时 IK 求解。

关于逆运动学再多讲一点，因为它是机械臂的「灵魂难题」：一个 5 自由度臂要把末端送到空间某点并带某个朝向，符合条件的关节组合往往不止一个（冗余），甚至可能无解（目标太远够不到）。MoveIt2 背后的 IK 求解器（如 KDL、TRAC-IK）就是在解这道非线性方程。本文故意绕开实时 IK，改用「预定义姿态 + 笛卡尔约束」：预定义姿态直接给了关节角（不用解），笛卡尔约束只约束末端位置（自由度少、好解）。这是「先用起来、再谈优雅」的务实路线，等你信得过规划器了再上实时 IK 也不迟。

还有个关键认知：规划（plan）和执行（execute）是两件事。MoveIt2 先算出一条无碰撞轨迹（plan），再通过 ExecuteTrajectory action 把轨迹下发给 ros2_control 真正驱动电机（execute）。代码里 MoveGroup action 同时干了这两步（plan_only=False），你也可以在调试时先 plan_only=True 只看规划成不成功、不下发，安全试错。这个区分在排查「规划成功但臂不动」时尤其重要——那是执行环节掉链子，不是规划。

5.1 核心抓取节点

pick_place_robot/pick_place_node.py：

这是整篇的「总指挥」节点，也是最长的一段。它订阅感知节点发来的物体位姿，收到触发信号后，按 7 步把活干完：移到准备位 → 开爪 → 移到物体上方 → 下降 → 闭爪抓取 → 提起 → 移到放置位放下。

读这段代码，盯着 execute_pick_and_place() 里的 Step 1~7 注释走一遍即可，其余都是封装好的工具函数（开爪/闭爪、移动到命名姿态、移动到笛卡尔目标）。末尾的 try/except 是异常恢复：任何一步规划失败就抛 RuntimeError，被捕获后开爪并退回 ready 安全姿态，不会让臂卡在半空。下面每个函数我也都标了「为什么这么写」。

#!/usr/bin/env python3
"""Pick & Place 主控节点 — MoveIt2 完整工作流"""

import rclpy
from rclpy.node import Node
from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.executors import MultiThreadedExecutor
from rclpy.action import ActionClient
from geometry_msgs.msg import PoseStamped, PoseArray
from std_msgs.msg import Bool
from moveit_msgs.action import MoveGroup, ExecuteTrajectory
from moveit_msgs.msg import (
    MotionPlanRequest, PlanningOptions,
    JointConstraint, Constraints, RobotState
)
from control_msgs.action import GripperCommand
from builtin_interfaces.msg import Duration
import math
import time


class PickPlaceNode(Node):
    def __init__(self):
        super().__init__('pick_place_node')

        # ReentrantCallbackGroup 允许回调并发：触发回调和动作回调能同时跑，
        # 否则默认单线程会让一个未完成的 action 阻塞新消息
        self.callback_group = ReentrantCallbackGroup()

        # ====== MoveIt2 客户端 ======
        # MoveGroup action：提交一个运动规划请求，MoveIt2 算完轨迹
        self.arm_move_client = ActionClient(
            self, MoveGroup, '/move_action',
            callback_group=self.callback_group)

        # ExecuteTrajectory action：把算好的轨迹真正下发执行
        self.arm_execute_client = ActionClient(
            self, ExecuteTrajectory, '/execute_trajectory',
            callback_group=self.callback_group)

        # 夹爪用 GripperCommand action：只给「目标位置+最大力」两个量
        self.gripper_client = ActionClient(
            self, GripperCommand, '/gripper_controller/gripper_cmd',
            callback_group=self.callback_group)

        # ====== 订阅检测 ======
        # 接收感知节点发来的物体位姿数组，存最新一份
        self.object_poses_sub = self.create_subscription(
            PoseArray, '/object_poses',
            self.object_poses_callback, 10)

        # ====== 服务/状态 ======
        # 外部发一个 True 来触发一次抓取流程（比如按钮、上位机）
        self.pick_trigger_sub = self.create_subscription(
            Bool, '/trigger_pick_and_place',
            self.trigger_callback, 10)

        self.busy = False          # 防重入：流程进行中忽略新触发
        self.latest_poses = None   # 最新物体位姿
        self.pick_count = 0        # 已完成次数统计

        self.get_logger().info('Pick & Place 节点已启动，等待触发...')

    # ...（object_poses_callback / trigger_callback 两个小回调省略：
    #      前者存最新位姿，后者「空闲时才启动流程」的防重入门闸，
    #      各 5 行样板，逻辑见注释）

    # ====== 夹爪控制 ======

    def open_gripper(self):
        """打开夹爪"""
        goal = GripperCommand.Goal()
        goal.command.position = 0.02   # 最大开度（对应 SRDF 里 open 状态）
        goal.command.max_effort = 10.0 # 最大力，防夹力过大捏坏物体
        future = self.gripper_client.send_goal_async(goal)
        rclpy.spin_until_future_complete(self, future, timeout_sec=5.0)
        self.get_logger().info('夹爪已打开')

    def close_gripper(self):
        """关闭夹爪"""
        goal = GripperCommand.Goal()
        goal.command.position = 0.0    # 完全闭合
        goal.command.max_effort = 15.0 # 抓取时力略大，确保夹稳不滑落
        future = self.gripper_client.send_goal_async(goal)
        rclpy.spin_until_future_complete(self, future, timeout_sec=5.0)
        self.get_logger().info('夹爪已闭合')

    # ====== 运动规划与执行 ======

    def move_to_named_pose(self, pose_name: str):
        """移动到预定义姿态（如 home/ready/pick_approach）"""
        self.get_logger().info(f'移动到姿态: {pose_name}')

        goal = MoveGroup.Goal()
        goal.request.group_name = 'arm'
        goal.request.num_planning_attempts = 5    # 失败重试 5 次提高成功率
        goal.request.allowed_planning_time = 5.0  # 单次最多想 5 秒
        goal.request.max_velocity_scaling_factor = 0.3  # 限速 30% 保安全
        goal.request.max_acceleration_scaling_factor = 0.2

        # 目标约束来自 SRDF 里预存的命名姿态，不用实时解 IK，最稳
        goal.request.goal_constraints.append(
            self._build_named_constraint(pose_name))

        goal.planning_options = PlanningOptions()
        goal.planning_options.plan_only = False   # False=规划完直接执行
        goal.planning_options.replan = True       # 执行中若碰撞则重规划
        goal.planning_options.replan_attempts = 3

        future = self.arm_move_client.send_goal_async(goal)
        rclpy.spin_until_future_complete(future, timeout_sec=15.0)

        if future.result() is not None:
            result = future.result()
            if result.error_code.val == 1:  # MoveIt2 的 SUCCESS 错误码就是 1
                self.get_logger().info(f'到达姿态: {pose_name}')
                return True
            else:
                self.get_logger().warn(
                    f'规划失败: error_code={result.error_code.val}')
        return False

    def move_to_pose_target(self, target_pose: PoseStamped):
        """移动到笛卡尔目标位姿（给具体 3D 位姿而非命名姿态）"""
        self.get_logger().info(
            f'移动到目标: ({target_pose.pose.position.x:.3f}, '
            f'{target_pose.pose.position.y:.3f}, '
            f'{target_pose.pose.position.z:.3f})')

        goal = MoveGroup.Goal()
        goal.request.group_name = 'arm'
        goal.request.num_planning_attempts = 10   # 笛卡尔目标更难，多试几次
        goal.request.allowed_planning_time = 10.0
        goal.request.max_velocity_scaling_factor = 0.2  # 精确运动用更低速度
        goal.request.max_acceleration_scaling_factor = 0.15

        goal.request.goal_constraints.append(
            self._build_pose_constraint(target_pose))

        goal.planning_options = PlanningOptions()
        goal.planning_options.plan_only = False
        goal.planning_options.replan = True
        goal.planning_options.replan_attempts = 5

        future = self.arm_move_client.send_goal_async(goal)
        rclpy.spin_until_future_complete(future, timeout_sec=20.0)

        if future.result() is not None:
            result = future.result()
            if result.error_code.val == 1:
                self.get_logger().info('到达目标位姿')
                return True
            else:
                self.get_logger().warn('笛卡尔规划失败')
        return False

    def _build_named_constraint(self, pose_name: str) -> Constraints:
        """构建预定义姿态约束：把 SRDF 里存的关节角转成关节约束"""
        # 这里把 SRDF 的命名姿态「搬」一份到代码里，方便按名取用；
        # 实际工程更推荐直接读 SRDF，此处为可读起见写死
        poses = {
            'home': [0, 0, 0, 0, 0],
            'ready': [0, -0.5, 1.0, -0.5, 0],
            'pick_approach': [0, -1.0, 1.8, -0.8, 0],
            'place_approach': [1.57, -0.5, 1.0, -0.5, 0],
        }
        # ...（把每个关节角包装成 JointConstraint 的循环：样板代码，
        #      容差 0.01 rad，照抄即可）

    def _build_pose_constraint(self, pose_stamped: PoseStamped) -> Constraints:
        """构建位姿约束：要求 link5 末端到达指定 3D 点"""
        from moveit_msgs.msg import PositionConstraint, OrientationConstraint
        from shape_msgs.msg import SolidPrimitive

        constraints = Constraints()

        # 位置约束
        pos_c = PositionConstraint()
        pos_c.header = pose_stamped.header
        pos_c.link_name = 'link5'   # 约束的是「腕关节(末端)」到达目标点
        pos_c.target_point_offset.x = 0.0
        pos_c.target_point_offset.y = 0.0
        pos_c.target_point_offset.z = 0.04  # 爪尖比 link5 再往外 4cm
        # 用一个 1cm 见方的小盒子作为允许误差区域，太严会规划失败
        pos_c.constraint_region.primitives.append(
            SolidPrimitive(type=SolidPrimitive.BOX))
        pos_c.constraint_region.primitives[0].dimensions = [
            0.01, 0.01, 0.01  # 1cm 精度
        ]
        pos_c.constraint_region.primitive_poses.append(pose_stamped.pose)
        pos_c.weight = 1.0
        constraints.position_constraints.append(pos_c)

        # ...（朝向约束同理省略：平面抓取只锁 Z 轴偏航，
        #      完整 6DoF 约束模板见 MoveIt2 官方 examples）

        return constraints

    # ====== Pick & Place 主流程 ======

工具函数都齐了，现在看真正的「总指挥」——execute_pick_and_place。读它的诀窍是只盯 Step 1~7 的注释主线，每步都是「构造目标位姿 → 调 move_to → 失败就 raise」的同一个节奏。两个数值直接影响成败，值得单独记：approach 的 +0.10（先悬停物体上方 10 cm）和 grasp 的 +0.02（爪尖停在物体顶面上方 2 cm）——前者太大浪费时间、太小容易撞，后者取决于你的夹爪指长，换夹爪必改。

另外注意 raise RuntimeError 的连锁反应：任何一步失败，异常一路抛到最外层 except，统一执行「开爪 + 回 ready」。这是集中式错误处理——不在每步重复写恢复代码，恢复策略只写一处，改起来也不会漏。

    def execute_pick_and_place(self):
        """执行完整的 Pick & Place 流程（7 步）"""
        if self.busy:
            self.get_logger().warn('系统忙，请等待当前任务完成')
            return

        # 检查是否有物体
        if not self.latest_poses or len(self.latest_poses) == 0:
            self.get_logger().warn('未检测到物体，请检查相机')
            return

        self.busy = True
        self.get_logger().info('=' * 50)
        self.get_logger().info('开始 Pick & Place 流程')
        self.get_logger().info('=' * 50)

        try:
            # 取第一个检测到的物体
            object_pose = self.latest_poses[0]

            # === Step 1: 移动到准备位姿 ===
            # 先回 ready，保证每次抓取都从同一个干净起点出发，避免累积误差
            self.get_logger().info('[Step 1/7] 移动到准备位姿')
            if not self.move_to_named_pose('ready'):
                raise RuntimeError('移动到准备位姿失败')

            # === Step 2: 打开夹爪 ===
            # 伸手前先张开，免得手指撞到物体
            self.get_logger().info('[Step 2/7] 打开夹爪')
            self.open_gripper()
            time.sleep(0.5)

            # === Step 3: 移动到物体上方（Approach） ===
            # 先悬停在物体正上方 10cm，再下降，避免水平位移时蹭到物体
            self.get_logger().info('[Step 3/7] 移动到物体上方')
            approach_pose = PoseStamped()
            approach_pose.header.frame_id = 'base_link'
            approach_pose.pose.position.x = object_pose.position.x
            approach_pose.pose.position.y = object_pose.position.y
            approach_pose.pose.position.z = object_pose.position.z + 0.10  # 上方 10cm
            approach_pose.pose.orientation = object_pose.orientation

            if not self.move_to_pose_target(approach_pose):
                raise RuntimeError('移动到物体上方失败')

            # === Step 4: 下降到抓取位置 ===
            # 降到物体顶面之上 2cm，给爪尖留接触空间
            self.get_logger().info('[Step 4/7] 下降到抓取位置')
            grasp_pose = PoseStamped()
            grasp_pose.header.frame_id = 'base_link'
            grasp_pose.pose.position.x = object_pose.position.x
            grasp_pose.pose.position.y = object_pose.position.y
            grasp_pose.pose.position.z = object_pose.position.z + 0.02  # 物体上方 2cm
            grasp_pose.pose.orientation = object_pose.orientation

            if not self.move_to_pose_target(grasp_pose):
                raise RuntimeError('下降到抓取位置失败')

            # === Step 5: 闭合夹爪 ===
            self.get_logger().info('[Step 5/7] 闭合夹爪（抓取）')
            self.close_gripper()
            time.sleep(0.8)  # 等夹爪闭合稳定，别刚夹上就抬

            # === Step 6: 提起物体 ===
            # 先抬到安全高度再横移，防止物体拖过桌面
            self.get_logger().info('[Step 6/7] 提起物体')
            lift_pose = PoseStamped()
            lift_pose.header.frame_id = 'base_link'
            lift_pose.pose.position.x = approach_pose.pose.position.x
            lift_pose.pose.position.y = approach_pose.pose.position.y
            lift_pose.pose.position.z = object_pose.position.z + 0.15  # 提高到安全高度
            lift_pose.pose.orientation = object_pose.orientation

            if not self.move_to_pose_target(lift_pose):
                raise RuntimeError('提起物体失败')

            # === Step 7: 移动到放置位置 ===
            # 先转到 place_approach 把物体带到放置区上方，再精确下放
            self.get_logger().info('[Step 7/7] 移动到放置位置')
            if not self.move_to_named_pose('place_approach'):
                raise RuntimeError('移动到放置位置失败')

            # 放下物体：固定放置点 (0.35, 0, 0.08)，姿态竖直(w=1)
            self.get_logger().info('放置物体')
            place_pose = PoseStamped()
            place_pose.header.frame_id = 'base_link'
            place_pose.pose.position.x = 0.35
            place_pose.pose.position.y = 0.0
            place_pose.pose.position.z = 0.08
            place_pose.pose.orientation.w = 1.0

            if not self.move_to_pose_target(place_pose):
                raise RuntimeError('移动到放置位置失败')

            self.open_gripper()
            time.sleep(0.5)

            # 回到准备位姿，准备下一轮
            self.move_to_named_pose('ready')

            # ...（成功日志与计数省略：pick_count 自增 + 打印横幅，样板）

        except RuntimeError as e:
            # 异常恢复：任意一步失败都开爪并退回 ready，绝不把臂留在半空
            self.get_logger().error(f'流程异常: {e}')
            self.get_logger().info('执行异常恢复...')
            self.open_gripper()
            self.move_to_named_pose('ready')
        finally:
            self.busy = False


def main():
    # ...（main 样板省略：MultiThreadedExecutor + spin，
    #      注意必须多线程——MoveIt2 的 action 回调和主回调要并发，单线程会死锁）


if __name__ == '__main__':
    main()

这个 launch 文件的关键不在代码，在**启动时序**。四个节点的依赖是单向链：Gazebo（世界）→ MoveIt2（规划器）→ 检测节点（要相机）→ 主控节点（要检测结果）。用  的 3 秒/5 秒错峰启动是最朴素的解法——**用时间换依赖**。它有个已知短板：如果机器慢、MoveIt2 五秒还没就绪，主控节点会连不上 action 服务直接报错退出。生产级做法是用事件机制（ 监听进程启动完成事件）替代裸延时，入门阶段裸延时够用，知道这个坑在哪就行。

把这 7 步走一遍，你就能在 RViz 里看到机械臂：先从 ready 抬起 → 张开 → 平移到物体上方 → 缓缓下降 → 合爪 → 提起 → 转到放置区上方 → 下放张开 → 回 ready。每一步终端都有 [Step X/7] 日志，卡在哪一步一眼可知。

这里有个设计取舍值得记住：Step 3/4 的「先悬停再下降」和 Step 6 的「先提起再横移」，是为了避免夹爪在水平移动时蹭到桌面或物体。真实抓取里这类「抬升—平移—下降」的分段轨迹，比直接斜插要稳得多，是工业机器人通用的安全范式。

把 Step 3–7 的设计逻辑再提炼一下，方便你改成自己的流程：

• 「接近→作业」两段式（先悬停再下降、先提再横移）是防蹭防撞的通用范式，任何抓取都该有「靠近段」和「操作段」分开，别让夹爪在水平移动时蹭到桌面或物体。
• 每一步都检查返回值：move_to_* 返回 False 就 raise，不让后续步骤在一个失败的姿态上继续叠加错误。这是「fail fast」原则，比含糊继续执行安全得多。
• 异常恢复永远回安全姿态：except 里先开爪（防止带着物体卡住）再回 ready，保证无论哪步崩，臂都停在已知安全位，不会悬在半空等下次开机。
• busy 防重入：流程进行中忽略新触发，避免两个抓取流程交错把臂搞乱。多机/多任务场景这个锁更重要。

如果你要改成「抓多个物体」，核心改动在 execute_pick_and_place：把 object_pose = self.latest_poses[0] 换成 for object_pose in self.latest_poses: 循环，并在每轮结束把已抓物体从检测列表剔除（或让检测节点标记 consumed）。其余 7 步逻辑完全复用——这就是把主流程写成独立函数的价值。

还有一个值得注意的细节：time.sleep(0.5) 这类硬编码等待。仿真里关节/夹爪动作是瞬时的，「睡半秒」是为了等 RViz 显示和仿真状态跟上；真机上应该改成「等 action 返回结果」而非盲目睡，否则要么等太久拖慢节拍、要么睡不够动作没完成就进下一步。入门用 sleep 图省事，真部署要换成状态等待。

5.3 MoveIt2 error_code 速查：规划失败看这表

代码里反复判断 result.error_code.val == 1，这个 1 是 MoveIt2 的 SUCCESS。失败时它会返回别的数，对应不同病因。把最常见的几个列出来，终端一打印你就能对号入座，不用每次翻源码：

经验法则：带 COLLISION 的（7/10）几乎都是 Octomap 或 TF 问题，先清地图、查坐标；PLANNING_FAILED（3）是目标不可达，先确认目标在关节限位和工作空间内；其余多为约束配置错误。把这张表存进书签，能省下大量瞎调时间。

5.2 一键启动

单个节点再强，也得有人按正确顺序把它们全拉起来。下面这段 Launch 文件就是「开机脚本」：先起 Gazebo 仿真和 MoveIt2，等几秒再用 TimerAction 延迟启动检测节点和主控节点——延迟是为了等依赖（相机驱动、MoveIt2 动作服务器）就绪，否则节点一启动就去连还没起来的服务会报错。

launch/pick_place.launch.py：

你只需关注两点：TimerAction(period=3.0/5.0) 的延迟秒数（机器慢就调大）、以及 DeclareLaunchArgument 里 use_camera 默认 false（仿真用假相机，true 才接真实 D435）。其余 IncludeLaunchDescription 是把子 launch 文件拼进来，按项目路径改包名即可。

#!/usr/bin/env python3
"""Pick & Place 系统一键启动"""

import os
from launch import LaunchDescription
from launch.actions import (
    IncludeLaunchDescription, DeclareLaunchArgument,
    TimerAction, LogInfo
)
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory


def generate_launch_description():
    pkg_robot = get_package_share_directory('pick_place_robot')
    pkg_perception = get_package_share_directory('pick_place_perception')

    use_sim = LaunchConfiguration('use_sim')

    # === 参数声明 ===
    # 这三个参数可在命令行覆盖，例如 ros2 launch ... use_camera:=true
    declare_sim = DeclareLaunchArgument('use_sim', default_value='true')
    declare_rviz = DeclareLaunchArgument('use_rviz', default_value='true')
    declare_camera = DeclareLaunchArgument(
        'use_camera', default_value='false',
        description='使用真实相机（非仿真）')

    # === Gazebo 仿真 ===
    # 仿真时才起 Gazebo；真机部署把 use_sim 设为 false 跳过
    gazebo_sim = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(pkg_robot, 'launch', 'gazebo_sim.launch.py')),
        condition=IfCondition(use_sim),
    )

    # === MoveIt2 启动 ===
    # MoveIt2 无论仿真真机都要起，提供规划服务
    moveit_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(pkg_robot, 'launch', 'moveit.launch.py')))

    # === 深度相机（真实或仿真）===
    # use_camera=true 才起真实 D435 驱动；仿真由 gazebo 插件提供假相机
    real_camera = Node(
        package='realsense2_camera',
        executable='realsense2_camera_node',
        name='realsense2_camera',
        parameters=[os.path.join(pkg_perception, 'config', 'realsense.yaml')],
        condition=IfCondition(LaunchConfiguration('use_camera')),
    )

    # === 物体检测节点 ===
    # 延迟 3 秒：等相机驱动和 MoveIt2 起来后再启动，避免连不上服务
    detection = TimerAction(
        period=3.0,
        actions=[
            Node(
                package='pick_place_perception',
                executable='detection_node',
                name='object_detection',
                output='screen',
            )
        ]
    )

    # === Pick & Place 主控节点 ===
    # 再延迟到 5 秒：等检测节点也就绪，确保一触发就有物体位姿可用
    pick_place = TimerAction(
        period=5.0,
        actions=[
            Node(
                package='pick_place_robot',
                executable='pick_place_node',
                name='pick_place_controller',
                output='screen',
            )
        ]
    )

    return LaunchDescription([
        declare_sim, declare_rviz, declare_camera,
        LogInfo(msg='=== Pick & Place 系统启动 ==='),
        LogInfo(msg='1. Gazebo 仿真环境'),
        LogInfo(msg='2. MoveIt2 运动规划'),
        LogInfo(msg='3. 深度相机 + 物体检测'),
        LogInfo(msg='4. Pick & Place 控制器'),
        gazebo_sim,
        moveit_launch,
        real_camera,
        detection,
        pick_place,
    ])

跑 ros2 launch pick_place_robot pick_place.launch.py 后，终端会依次打印那几行 LogInfo，RViz 里出现机械臂和相机坐标系；等约 5 秒主控节点起来，你再往场景里放个红色小物块，发布一个 Bool: true 到 /trigger_pick_and_place，就能看到完整抓取动作。

六、调试与优化

能跑通 7 步只是开始，真正干活时总会遇到「规划超时」「夹爪滑落」「位姿偏移」。这一章给你一套命令行工具和一张排查表，把常见毛病对号入座。

调试的本质是「分层隔离」：机械臂不动，到底是感知没给位姿、还是规划失败、还是执行没收到轨迹？不要一上来就改代码，先用 6.1 的命令一层层剥：

• 先 ros2 topic echo /object_poses 看感知有没有输出——没有就回第四章查相机和分割；
• 有输出但臂不动，看 ros2 action list 里 MoveIt2 的 action 在不在——不在就回第三章查 launch 有没有起 MoveIt2；
• action 在但规划失败，看终端 error_code——非 1 的错误码对照 MoveIt2 文档，多半是目标不可达或碰撞约束太紧。

把「感知→规划→执行」当成三段独立管道分别验证，比在几百行代码里大海捞针快十倍。这也解释了为什么本文把链路拆成独立节点：坏哪段修哪段，互不拖累。

6.1 常见调试命令

下面几条是现场最常用的「听诊器」。前两条确认 MoveIt2 和规划场景在线；view_frames 能导出 TF 树 PDF，用来肉眼检查「相机→基座」的变换链有没有断；最后一条用你的 rviz 配置直接打开可视化。

# 查看 MoveIt2 状态：确认 /move_action 等 action 已就绪
ros2 action list | grep move_group

# 查看规划场景中的碰撞物体：确认 Octomap/物体有没有进规划场景
ros2 topic echo /planning_scene

# 手动更新 Octomap：地图脏了(物体被移走还在)就清一下重扫
ros2 service call /clear_octomap std_srvs/srv/Empty

# 查看 TF 树（相机→基座变换）：没有这条链，位姿换算会全错
ros2 run tf2_tools view_frames

# 在 RViz 中可视化：直接载入配套配置，省去手动加显示项
rviz2 -d ~/pick_place_ws/src/pick_place_robot/config/moveit.rviz

6.2 抓取失败排查清单

把 7 步流程里最容易出的问题列成表。注意每一行都是「现象→根因→对策」的闭环：比如「规划超时」往往不是算法慢，而是目标位姿落在了工作空间外或关节限位里，调 pick_approach 姿态或放宽 workspace_z 即可。

6.3 性能优化

调通之后如果想让它「更跟手」，可以在开发后期逐步放开限速、缩短规划时间。下面这组值是把安全系数往下调一档的示例——前提是你已经在仿真里反复验证不会撞，否则别轻易动 max_velocity_scaling_factor。

# moveit_controllers.yaml 优化
move_group:
  max_velocity_scaling_factor: 0.5    # 开发时 0.3，熟练后提升（前提：已验证不撞）
  max_acceleration_scaling_factor: 0.3

# OMPL 优化
ompl_planning:
  planning_time_limit: 5.0            # 降低规划时间，响应更快（代价：复杂场景易失败）
  max_iterations: 100                 # 减少迭代次数，提速

# Octomap 优化
octomap_server:
  resolution: 0.08                    # 降低分辨率提升性能（代价：地图变粗糙）
  pointcloud_max_z: 1.5               # 减小检测范围，少处理无用点云

性能优化本质上是「速度 vs 安全/精度」的权衡：限速放开了动作更利落，但撞桌风险上升；Octomap 分辨率调粗了算力省，但小物体可能漏检。建议始终保留一个「安全档」配置和一个「性能档」配置，按需切换，别一把梭把参数全推到极限。

七、从仿真迁移到真机：到底要改什么

仿真跑通≠真机能用，但好消息是改动集中在一小块，不至于推倒重来。本文架构从一开始就为「换相机、换真臂」留了缝：感知节点只认 /camera/* 话题，执行节点只认 MoveIt2 的 action，中间不绑定任何仿真专有逻辑。迁移时你要动的是下面几处：

1. 相机：把 use_camera 设为 true，让 Launch 起真实 D435 驱动代替 Gazebo 假相机。话题名（/camera/color/image_raw 等）必须和仿真一致，检测节点才不用改一行。
2. TF 外参：真机上 camera_link → base_link 的变换要靠你实测标定得到（眼在手/眼在外两种安装方式算法不同），写进 URDF 的 <joint> 或用一个 static_transform_publisher 发布。TF 不准，抓取就整体偏移——这是真机第一大坑。
3. 控制器：ros2_control 的硬件接口从 Gazebo 插件换成真实驱动（如 ros2_control 的实机硬件接口或厂商 SDK 桥接）。只要 action 名字（/move_action、/execute_trajectory、/gripper_controller/gripper_cmd）不变，抓取节点就原样可用。
4. 参数微调：真机惯性、摩擦、限位都和仿真不同，夹爪 max_effort、各 scaling_factor 要重新调，否则要么夹不稳要么动作太猛。

一句话总结迁移心法：保持话题名和 action 名不变，只换数据源（相机/控制器）和坐标外参（TF）。这也是为什么本文反复强调「名字要对得上」——它不只是跑通仿真的技巧，更是真机迁移的命脉。

八、FAQ：新手最常卡住的 8 个问题

把社区和实操里最高频的疑问集中答一遍，省得你逐个搜：

Q1：终端报 action server not available？ 多数情况是 Launch 里 TimerAction 延迟不够，节点去连还没起来的 MoveIt2。把 period 调大（如 8 秒）再试。

Q2：检测节点没输出、调试图全黑？ 先 ros2 topic list | grep camera 确认相机话题存在；再查 camera_info 是否到位（深度反算 3D 依赖它）；最后确认场景里真有够红的物体、且 HSV 阈值覆盖该红色。

Q3：机械臂朝物体反方向伸？ TF 链 base_link → camera_link 断了或外参错。用 view_frames 导 TF 树确认链完整；仿真里检查相机是否挂在了正确的 link 上。

Q4：一伸手就碰撞报警、规划失败？ 八成 Octomap 没 filter_ground（把地板当障碍）或地图过期。清 Octomap 重扫，并确认 filter_ground: true。

Q5：抓起来一抬就滑落？ 夹爪摩擦 mu 太低或 max_effort 太小。调大这两个值；也检查物体是否在 2 cm 开合行程内。

Q6：规划要好几秒甚至超时？ 目标可能落在关节限位或工作空间外；或 planning_time_limit 太小。先放宽时间，再检查目标姿态是否合理。

Q7：改了 YAML 没反应？ YAML 是启动时加载的，必须重启节点/launch 才生效，热改文件无效。

Q8：真机和仿真行为不一样？ 优先怀疑 TF 外参和控制器接口，而不是算法。仿真里「睡半秒」的等待在真机上要换成「等 action 结果」。

附录：参数速查与名词解释

把全文散落的关键参数和名词收一张表，调试时直接查，不用翻回上文。

必调参数速查（按出错频率排序）

名词解释

• URDF / xacro：机器人描述格式 / 其宏预处理器，定义运动学骨架。
• SRDF：MoveIt2 语义配置，定义规划组、预设姿态、自碰撞豁免。
• Octomap：概率占据地图，把点云累积成 3D 障碍图用于避障。
• MoveIt2：ROS 2 运动规划框架，负责 IK、轨迹规划、碰撞检测。
• TF：坐标变换树，把不同坐标系下的位姿互相换算。
• Action：ROS 2 的「派任务+等结果」通信，用于耗时操作（规划、夹爪）。
• QoS：话题的服务质量策略，BEST_EFFORT 允许丢、RELIABLE 必达。
• IK（逆运动学）：由末端目标位姿反解各关节角度，机械臂的核心难题。
• 笛卡尔约束：直接约束末端的位置/姿态，而非关节角。
• mimic 关节：一个关节镜像跟随另一个，本文用于对称开合的夹爪。

收尾：从「看得见」到「抓得稳」还差什么

回到开头那个比喻——这篇我们给闭眼壮汉装上了眼睛（深度相机 + 颜色分割检测）和大脑（MoveIt2），并教会他一句话的本事：看到红色物体就抓起来放到指定位置。整条链路可以浓缩成三句话：感知把像素变成位姿，规划把位姿变成关节轨迹，异常恢复保证任何一步崩了都退回到安全姿态。

但真要把它从「仿真跑通」推进到「产线可用」，还有几道坎这篇故意没碰，给你指个方向：

• 检测升级：颜色分割只认纯色，换成 YOLO/Segment Anything 才能认任意物体，且对光照鲁棒——前面说过，只需替换 detect_objects 前半段。
• 抓取姿态生成：本文用固定命名姿态 + 竖直抓取，真物体有各种朝向和形状，需要 6DoF 抓取采样（如 GPD、GraspNet）生成候选并挑最优。
• 力控与滑移：现在夹爪只给个开度，真抓易碎或异形件要靠力反馈闭环，夹到了才知道夹稳没。
• 多物体排序：现在只抓 latest_poses[0] 第一个，实际要按距离/优先级排个抓取序列。

这些都不是推翻重来，而是在本文骨架上「换零件」。你把这篇的 7 步主流程、TF 坐标变换、Octomap 碰撞避障吃透，上面每一个扩展都是顺理成章的下一步。

下一篇我们将进入 ROS 2 多机通信与系统部署实战，解决多机器人协作和 Docker 容器化部署问题——当你不再只有「一只手臂」，而是好几台机器要协同干活时，通信与部署才是真正的工程门槛。

学完这篇，下一步往哪走

这篇跑通的是「仿真里的平面抓取」。往真机走，还差三级台阶，每级都有对应的进阶方向：

1. 标定：仿真里相机和机械臂的相对位置是「声明出来的」，真机上是「标定出来的」。手眼标定（Eye-in-Hand / Eye-to-Hand）决定你抓得准不准，是仿真到真机的第一道坎——站内进阶篇手眼标定与 3D 抓取：眼手协同实战专门拆这个。
2. 动态场景：本文物体是静止的。物体在动（传送带）就要引入视觉伺服或预测；环境在变就要靠 Octomap 持续更新 + 重规划，这正是 enable_replanning 存在的意义。
3. 力控：平行夹爪「夹到就完事」，但插拔、装配、柔性物体需要力反馈——那是阻抗/导纳控制的世界，站内外骨骼柔顺控制的思想完全通用。

一个建议的练手路径：先把本文的红色物体换成两种颜色（改 HSV 阈值 + 检测结果带 class），再按颜色分拣到不同放置点——这个「双色分拣」小项目会把检测、TF、规划、执行全链路再踩一遍，比看十篇教程都扎实。

许可协议

本文采用 署名-非商业性使用-相同方式共享 4.0 国际 许可协议，转载请注明出处。

分享文章