第六章：完整任务功能联调¶

6.1 功能联调的目标¶

  前面章节我们已经实现了各个独立的功能模块，本章节我们将学习将它们进行调用并编排任务流程。功能联调是机器人系统开发的核心里程碑，其目标不是重新验证单个模块的正确性，而是验证模块间的协同能力，确保数据在全链路中正确流转、指令在时序上准确执行、系统在异常情况下具备容错能力。

6.1.1 三大核心功能的拼接逻辑¶

  有限状态机（FSM）是刻画离散系统行为的计算模型，由有限状态集合、输入事件集合、状态转移函数与输出逻辑构成，核心特征是系统任一时刻仅处于单一状态，接收到输入事件后依据预定义规则切换状态并输出对应动作；在 Bobac 机器人的搬运任务中，grasp_control 状态机作为中央调度中枢，是典型的事件驱动型有限状态机，它以导航等待、视觉检测等待、抓取执行等待等为离散状态集合，以各子系统的完成反馈（到达通知、抓取点输出、抓取完成）为输入事件，按照预设的调度转移逻辑，依次向导航、视觉感知、抓取执行子系统输出触发指令，通过有序的状态切换串联起导航 - 感知 - 执行的闭环控制链路，实现多子系统的协同管控与搬运任务的流程化推进。

如下图所示

graph LR
    subgraph 导航子系统
        A[Nav2 导航栈] --> A1[底盘控制]
        A1 --> A2[到达目标位置]
    end

    subgraph 视觉感知子系统
        B[YOLOE 检测] --> B1[深度反投影]
        B1 --> B2[抓取点计算]
        B2 --> B3["坐标转换 (base_link_arm)"]
    end

    subgraph 抓取执行子系统
        C[MoveIt2 运动规划] --> C1[轨迹执行]
        C1 --> C2[夹爪控制]
    end

    D[grasp_control 状态机] -->|触发导航| A
    A2 -->|到达通知| D
    D -->|触发检测| B
    B3 -->|抓取点输出| D
    D -->|触发抓取| C
    C2 -->|抓取完成| D

    style D fill:#fff9c4,stroke:#d48806

6.1.2 联调整体架构解析¶

  我们的系统分为 “Isaac Sim 仿真层” 和 “ROS2 计算层” ，通过“任务管理器”模式串联：

  1. 模块分工：
层级 模块 职责
Isaac Sim 层 Bobac USD 模型 以及场地USD 提供机器人模型、仿真环境（地图、物体）
导航 cmd / 相机 cmd / 机械臂 cmd 接收 ROS2 指令，驱动仿真中机器人运动、拍照、控制机械臂
导航模块 / 独立化脚本 实现仿真内的运动控制（如底盘速度解析）
ROS2 层 Nav2 导航功能包 路径规划、发布底盘线速度 / 角速度
深度相机识别算法 处理仿真图像，输出目标位姿
MoveIt算法 规划机械臂轨迹，输出关节控制指令
核心中枢 任务管理器 触发各模块（导航→相机→机械臂），监听状态反馈

整体架构图：

graph TD
    %% 核心调度节点
    TM[任务管理器]

    %% 导航阶段子图
    subgraph 导航阶段
        Nav2[Nav2]
        NavCmd[导航 cmd]
    end

    %% 视觉阶段子图
    subgraph 视觉阶段
        CamCmd[相机 cmd]
        Det[相机识别]
        CuRobo[cuRobo]
    end

    %% 抓取阶段子图
    subgraph 抓取阶段
        ArmCmd[双臂 cmd]
    end

    %% 导航数据流
    TM -->|话题| Nav2
    Nav2 -->|速度指令| NavCmd
    NavCmd -->|状态| TM

    %% 视觉数据流
    TM -->|拍照指令| CamCmd
    CamCmd -->|图像| Det
    Det -->|位姿| CuRobo

    %% 抓取数据流
    CuRobo -->|轨迹指令| ArmCmd
    ArmCmd -->|状态| TM

graph TD
    %% 核心调度节点
    TM[任务管理器]

    %% 导航阶段子图
    subgraph 导航阶段
        Nav2[Nav2]
        NavCmd[导航 cmd]
    end

    %% 视觉阶段子图
    subgraph 视觉阶段
        CamCmd[相机 cmd]
        Det[相机识别]
        CuRobo[cuRobo]
    end

    %% 抓取阶段子图
    subgraph 抓取阶段
        ArmCmd[双臂 cmd]
    end

    %% 导航数据流
    TM -->|话题| Nav2
    Nav2 -->|速度指令| NavCmd
    NavCmd -->|状态| TM

    %% 视觉数据流
    TM -->|拍照指令| CamCmd
    CamCmd -->|图像| Det
    Det -->|位姿| CuRobo

    %% 抓取数据流
    CuRobo -->|轨迹指令| ArmCmd
    ArmCmd -->|状态| TM

6.2 联调前置条件：模块独立验证¶

能够顺利完成之前的demo，就证明模块独立运行没问题。

联调前必须确保 单个模块能稳定运行，避免联调时定位问题困难：

模块 验证方式
导航 在 Isaac Sim 中，通过 RViz 发布目标点，机器人能自主规划路径并移动（监听/cmd_vel话题）。
相机 触发仿真相机拍照（调用相机 cmd 节点），能在 ROS2 中接收图像话题（如/left_camera/rgb）。
机械臂 手动发送目标位姿到 MoveIt2，机械臂能规划轨迹并执行（观察仿真中夹爪运动）。

6.2.1 联调核心流程¶

  以 ROS2 话题为核心，构建 IsaacSim 仿真环境与 ROS2 多层功能节点间的数据传递闭环：IsaacSim 仿真环境输出关节状态（/joint_states）、RGB 图像与深度数据（/arm_camera/rgb、arm_camera/depth），分别作为 MoveIt2 规划核心的状态基准与视觉检测模块的感知输入.
  视觉检测层输出目标检测结果，为抓取状态机的任务编排提供决策触发依据；抓取状态机分别下发抓取请求与夹爪控制指令，驱动运动规划节点完成轨迹解算、夹爪控制节点执行开合动作；
运动规划节点与夹爪控制节点共同向仿真环境下发控制指令（/hand_command），同时 MoveIt2 核心回传 TF 变换与运动状态形成规划侧内部闭环，仿真端执行指令后持续回传实时数据，完成全链路端到端流转闭环。

6.3 多阶段联调复合案例¶

6.3.1 相机与机械臂、底盘联调¶

本案例以 Isaac Sim 仿真平台中的bobac机器人为对象，演示通过深度相机与底盘、机械臂协同，实现对粉笔的目标定位、导航与抓取准备动作，核心流程可通过动图直观查看，具体分为 “准备识别姿态” 与 “抓取准备姿态” 两步执行。

一、案例核心组件与目标

核心组件：bobac机器人（含机械臂、左手夹爪、底盘）、深度相机、粉笔、Nav2导航系统、MoveIt2运动规划库

核心目标：通过相机识别定位并导航到目标，结合运动规划计算，控制机械臂完成从 “识别准备” 到 “抓取准备” 的连贯动作，验证视觉与机械臂的协同控制逻辑。

二、联调流程（结合动图分步解析）

第一步：机械臂到达 “准备识别姿态”

动图中初始阶段，bobac机器人底盘先执行姿态调整，然后通过Nav2导航系统到达目标位置，随后机械臂整体抬升并展开，使深度相机的视野完全覆盖前方，避免机械臂自身结构遮挡相机视线。这一步的核心目的是为相机识别创造稳定视野—— 深度相机需无遮挡地拍摄粉笔，才能准确提取粉笔的三维坐标（位置与姿态信息），并将识别结果以数据形式发送至 MoveIt2运动规划库。

第二步：机械臂到达 “抓取准备姿态”

数据处理与规划：MoveIt2接收相机发送的粉笔后，结合bobac机器人机械臂的关节限制、运动范围等参数，自动计算出从 “准备识别姿态” 到 “抓取准备姿态” 的关节运动轨迹（包括每个关节的转动角度、速度）。 关节控制与执行：MoveIt2将规划好的轨迹结果以指令形式发布给机械臂的各个关节，动图中可观察到bobac机器人左手夹爪逐步靠近粉笔，最终停留在粉笔正上方（夹爪开口对准方块，距离适中），完成抓取前的姿态定位。

三、关键协同逻辑

整个过程中，深度相机负责 “感知定位”，MoveIt2负责 “运动规划”，机械臂关节负责 “执行动作”，三者通过数据传递形成闭环：相机识别结果是规划的基础，规划结果直接决定关节运动，最终通过姿态调整实现 “先看清、再靠近” 的联调目标，为后续实际抓取动作奠定基础。

exported_image

6.3.2抓取案例¶

一、案例背景与场景升级

本案例基于上一章节“相机与机械臂联调案例” 的定位基础，在 Isaac Sim 仿真环境中优化场景并扩展抓取流程：新增桌子（粉笔放置的地方），核心新增 “靠近 - 抓取 - 重新放置” 动作，通过MoveIt2完成三次关键运动规划（前两次定位规划见上一章节），实现从 “定位” 到 “抓取回收” 的闭环。

二、核心组件与场景参数

核心组件：bobac机器人（含机械臂、夹爪）、深度相机、粉笔（适配抓取参数）、桌子、MoveIt2运动规划库 场景布局：桌子位于机器人正前方，粉笔固定于桌面，确保相机可稳定识别

三、抓取核心流程（三次关键运动规划 + 动图解析）

前两次运动规划与上一章节类似，这里不再阐述。

（一） 第三次运动规划：靠近目标物MoveIt2接收相机的粉笔精准坐标后，结合桌面高度与机械臂关节限制，规划短距离运动轨迹。动图中可见，机械臂从 “抓取准备位置”（粉笔正前方几厘米）缓慢前移，直至夹爪开口完全包裹方块两侧，到达 “可抓取” 近距离姿态。

（二） 第四次运动规划：夹爪闭合抓取MoveIt2将夹爪关节纳入规划，根据粉笔尺寸计算闭合角度（避免过松脱落或过紧损坏）。动图中，夹爪关节向内转动、开口缩小，最终完全闭合并夹紧粉笔，完成核心抓取动作。

（三） 第五次运动规划：抓取成功后，MoveIt2规划放置轨迹轨迹（避开桌子障碍）。机械臂带着粉笔抬升、旋转，然后放置到恢指定位置。

（四） 第六次运动规划：返回准备识别姿态 抓取成功后，MoveIt2规划安全复位轨迹。动图中，机械臂抬升、后退，最终恢复到初始 “准备识别姿态”，实现抓取流程闭环。

exported_image

6.4 本章小结¶

本章作为课程的成果汇总章，重点讲解了从 "单个功能能跑" 到 "整套流程能跑" 的系统联调方法。

关键要点：

  1. 联调核心：验证模块间的协同能力，而不是重新验证单个模块
  2. 任务编排：采用分层状态机设计，将复杂任务拆解为独立状态，逻辑清晰、易于调试
  3. 常见问题：联调中 80% 的问题出在坐标、时序、控制和执行一致性上
  4. 验证方法：按照 "接口→单步→全程→稳定性→异常" 的顺序逐步验证

通过本章的学习，你应该建立起系统级的工程思维，理解机器人系统开发不是简单的模块堆砌，而是需要精心设计的协同工作。