大家好，我是专注于机器人技术分享的博主。在探索具身智能和机器人自动化的过程中，机械臂的运动规划与控制是绕不开的核心环节。很多开发者，尤其是刚接触ROS2的朋友，面对MoveIt2这个强大的框架时，常常感到无从下手——环境配置复杂、概念繁多、代码不知从何写起。本文将带你从零开始，手把手搭建一个基于ROS2 Humble和MoveIt2的机械臂控制项目。无论你是机器人方向的在校学生，还是希望将机械臂集成到项目中的工程师，都能通过这篇教程，掌握从URDF建模、MoveIt2配置到C++代码控制的全流程，最终实现一个包含夹爪控制的完整示例。

1. 背景与核心概念

在深入实操之前，我们有必要厘清几个关键概念，这能帮助你更好地理解整个项目架构。

ROS2 (Robot Operating System 2) ： 机器人操作系统第二代，是一个用于编写机器人软件的模块化框架。它提供了硬件抽象、底层设备控制、进程间消息传递、包管理等服务，其核心改进在于分布式架构、实时性支持和更完善的安全机制。ROS2采用DDS作为底层通信中间件，使得系统更加可靠和适用于工业场景。

MoveIt2 ： 是ROS2中用于移动操作的核心软件包。它集成了运动规划、操作控制、3D感知、运动学、碰撞检测等功能，是让机械臂“动起来”的大脑。MoveIt2是MoveIt在ROS2生态中的移植和升级版，支持ROS2的所有新特性。

URDF (Unified Robot Description Format) ： 统一机器人描述格式。它是一种XML格式的文件，用于描述机器人的物理结构，包括连杆(links)和关节(joints)的尺寸、形状、质量、惯性矩阵以及它们之间的连接关系。简单说，URDF定义了机器人长什么样。

SRDF (Semantic Robot Description Format) ： 语义机器人描述格式。它是URDF的补充，定义了用于运动规划的语义信息，例如机器人的规划组、末端执行器、虚拟关节、禁用碰撞矩阵等。MoveIt2的配置主要围绕SRDF展开。

具身智能 (Embodied AI) ： 这是当前机器人学和人工智能交叉领域的热点。它强调智能体需要通过与其所处环境进行物理交互来学习和完成任务。我们的机械臂控制项目，正是具身智能在“执行层”的一个具体体现——为智能算法提供一个可靠、可控的物理执行载体。

本教程项目流程 ： 我们将创建一个简单的6自由度机械臂模型，为其配置MoveIt2，并编写C++节点，通过MoveIt2的API实现运动规划、轨迹执行以及夹爪的开合控制。

2. 环境准备与版本说明

工欲善其事，必先利其器。一个稳定、版本匹配的开发环境是成功的第一步。

操作系统 ： Ubuntu 22.04 LTS (Jammy Jellyfish)。这是ROS2 Humble长期支持版本的官方推荐系统。

ROS2 发行版 ： Humble Hawksbill。这是当前的长期支持版本，社区支持完善，与MoveIt2的兼容性最好。

其他关键工具 ：

• 编译器 ： GCC 11+ 或 Clang 14+
• 构建工具 ： Colcon (ROS2标准的构建工具)
• Python 版本 ： 3.8+
• Git ： 用于克隆必要的软件包

2.1 安装ROS2 Humble

如果你已经安装好ROS2 Humble，可以跳过此步。以下是官方推荐的最小化安装步骤：

# 1. 设置语言环境
sudo apt update && sudo apt install locales
sudo locale-gen en_US en_US.UTF-8
sudo update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8
export LANG=en_US.UTF-8

# 2. 添加ROS2软件源
sudo apt install software-properties-common
sudo add-apt-repository universe
sudo apt update && sudo apt install curl -y
sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg
echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null

# 3. 安装ROS2基础包和开发工具
sudo apt update
sudo apt upgrade -y
sudo apt install ros-humble-desktop python3-colcon-common-extensions python3-rosdep2 -y

# 4. 初始化rosdep并更新
sudo rosdep init
rosdep update

# 5. 设置环境变量（建议写入~/.bashrc）
echo "source /opt/ros/humble/setup.bash" >> ~/.bashrc
source ~/.bashrc

2.2 安装MoveIt2

我们将从源码构建MoveIt2，以获得最大的灵活性和最新的功能。

# 1. 创建工作空间
mkdir -p ~/moveit2_ws/src
cd ~/moveit2_ws/src

# 2. 克隆MoveIt2主仓库及其依赖
git clone https://github.com/ros-planning/moveit2.git -b humble
for repo in moveit2/moveit2.repos $(find moveit2 -name "*.repos"); do vcs import < "$repo"; done

# 3. 安装系统依赖
rosdep install -r --from-paths . --ignore-src --rosdistro humble -y

# 4. 构建工作空间
cd ~/moveit2_ws
colcon build --event-handlers desktop_notification- status- --cmake-args -DCMAKE_BUILD_TYPE=Release

构建过程可能需要较长时间（30分钟以上，取决于硬件）。构建成功后，记得source工作空间：

echo "source ~/moveit2_ws/install/setup.bash" >> ~/.bashrc
source ~/.bashrc

3. 创建自定义机械臂URDF模型

我们将创建一个简单的6自由度旋转关节机械臂模型，并附带一个二指夹爪作为末端执行器。

3.1 项目结构规划

首先，创建一个独立的工作空间用于我们的项目：

mkdir -p ~/my_robot_arm_ws/src
cd ~/my_robot_arm_ws/src

创建一个ROS2功能包：

ros2 pkg create --build-type ament_cmake my_robot_arm --dependencies rclcpp std_msgs sensor_msgs geometry_msgs moveit_core moveit_ros_planning_interface tf2_ros tf2_geometry_msgs

3.2 编写URDF文件

在 my_robot_arm 包内创建 urdf 目录，并新建 my_robot_arm.urdf.xacro 文件。我们使用 xacro （XML宏）来让URDF更模块化、更易维护。

<?xml version="1.0"?>
<!-- 文件路径：~/my_robot_arm_ws/src/my_robot_arm/urdf/my_robot_arm.urdf.xacro -->
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="my_robot_arm">

  <!-- 定义材料颜色 -->
  <material name="blue">
    <color rgba="0.0 0.0 0.8 1.0"/>
  </material>
  <material name="gray">
    <color rgba="0.7 0.7 0.7 1.0"/>
  </material>
  <material name="black">
    <color rgba="0.1 0.1 0.1 1.0"/>
  </material>

  <!-- 定义基础连杆，作为世界坐标系 -->
  <link name="world"/>

  <!-- 基座连杆 -->
  <link name="base_link">
    <visual>
      <geometry>
        <cylinder radius="0.1" length="0.05"/>
      </geometry>
      <material name="gray"/>
    </visual>
    <collision>
      <geometry>
        <cylinder radius="0.1" length="0.05"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="1.0"/>
      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
    </inertial>
  </link>

  <!-- 基座与世界之间的固定关节 -->
  <joint name="world_to_base" type="fixed">
    <parent link="world"/>
    <child link="base_link"/>
    <origin xyz="0 0 0" rpy="0 0 0"/>
  </joint>

  <!-- 宏定义：一个旋转关节连杆单元 -->
  <xacro:macro name="rotating_joint_link" params="link_name parent_link_name radius length mass ixx iyy izz joint_axis:= '0 0 1'">
    <link name="${link_name}">
      <visual>
        <geometry>
          <cylinder radius="${radius}" length="${length}"/>
        </geometry>
        <material name="blue"/>
      </visual>
      <collision>
        <geometry>
          <cylinder radius="${radius}" length="${length}"/>
        </geometry>
      </collision>
      <inertial>
        <mass value="${mass}"/>
        <inertia ixx="${ixx}" ixy="0" ixz="0" iyy="${iyy}" iyz="0" izz="${izz}"/>
      </inertial>
    </link>
    <joint name="${parent_link_name}_to_${link_name}" type="revolute">
      <parent link="${parent_link_name}"/>
      <child link="${link_name}"/>
      <origin xyz="0 0 ${length/2}" rpy="0 0 0"/>
      <axis xyz="${joint_axis}"/>
      <limit lower="-3.14" upper="3.14" effort="10.0" velocity="1.0"/>
      <dynamics damping="0.7" friction="0.0"/>
    </joint>
  </xacro:macro>

  <!-- 使用宏定义6个关节连杆 -->
  <xacro:rotating_joint_link link_name="link1" parent_link_name="base_link" radius="0.05" length="0.3" mass="0.5" ixx="0.001" iyy="0.001" izz="0.001"/>
  <xacro:rotating_joint_link link_name="link2" parent_link_name="link1" radius="0.04" length="0.25" mass="0.4" ixx="0.0008" iyy="0.0008" izz="0.0008" joint_axis="0 1 0"/>
  <xacro:rotating_joint_link link_name="link3" parent_link_name="link2" radius="0.03" length="0.2" mass="0.3" ixx="0.0006" iyy="0.0006" izz="0.0006"/>
  <xacro:rotating_joint_link link_name="link4" parent_link_name="link3" radius="0.02" length="0.15" mass="0.2" ixx="0.0004" iyy="0.0004" izz="0.0004" joint_axis="0 1 0"/>
  <xacro:rotating_joint_link link_name="link5" parent_link_name="link4" radius="0.02" length="0.1" mass="0.15" ixx="0.0003" iyy="0.0003" izz="0.0003"/>
  <xacro:rotating_joint_link link_name="link6" parent_link_name="link5" radius="0.01" length="0.05" mass="0.1" ixx="0.0002" iyy="0.0002" izz="0.0002" joint_axis="0 1 0"/>

  <!-- 末端法兰 -->
  <link name="flange">
    <visual>
      <geometry>
        <cylinder radius="0.03" length="0.02"/>
      </geometry>
      <material name="black"/>
    </visual>
    <collision>
      <geometry>
        <cylinder radius="0.03" length="0.02"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.05"/>
      <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
    </inertial>
  </link>
  <joint name="joint6_to_flange" type="fixed">
    <parent link="link6"/>
    <child link="flange"/>
    <origin xyz="0 0 0.035" rpy="0 0 0"/>
  </joint>

  <!-- 二指夹爪模型（简化版） -->
  <!-- 左手指 -->
  <link name="left_finger">
    <visual>
      <geometry>
        <box size="0.02 0.005 0.05"/>
      </geometry>
      <material name="black"/>
    </visual>
    <collision>
      <geometry>
        <box size="0.02 0.005 0.05"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.01"/>
      <inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
    </inertial>
  </link>
  <joint name="flange_to_left_finger" type="prismatic">
    <parent link="flange"/>
    <child link="left_finger"/>
    <origin xyz="0.015 0 0.025" rpy="0 0 0"/>
    <axis xyz="1 0 0"/>
    <limit lower="0.0" upper="0.02" effort="5.0" velocity="0.1"/>
  </joint>

  <!-- 右手指 -->
  <link name="right_finger">
    <visual>
      <geometry>
        <box size="0.02 0.005 0.05"/>
      </geometry>
      <material name="black"/>
    </visual>
    <collision>
      <geometry>
        <box size="0.02 0.005 0.05"/>
      </geometry>
    </collision>
    <inertial>
      <mass value="0.01"/>
      <inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
    </inertial>
  </link>
  <joint name="flange_to_right_finger" type="prismatic">
    <parent link="flange"/>
    <child link="right_finger"/>
    <origin xyz="-0.015 0 0.025" rpy="0 0 0"/>
    <axis xyz="-1 0 0"/>
    <limit lower="0.0" upper="0.02" effort="5.0" velocity="0.1"/>
  </joint>

</robot>

这个URDF定义了一个从基座到末端法兰的6自由度旋转关节机械臂，并在末端添加了一个简单的二指平移关节夹爪。

4. 配置MoveIt2

MoveIt2的配置主要通过 MoveIt Setup Assistant 工具完成，它是一个图形化工具，能帮我们生成SRDF和大量的配置文件。

4.1 安装并启动MoveIt Setup Assistant

# 安装（如果之前从源码构建了MoveIt2，应该已包含）
sudo apt install ros-humble-moveit-setup-assistant

# 启动
ros2 launch moveit_setup_assistant setup_assistant.launch.py

4.2 使用Setup Assistant配置机器人

1. 创建新配置包 ：点击“Create New MoveIt Configuration Package”，选择我们刚才创建的URDF文件 ( my_robot_arm.urdf.xacro )。注意，需要先将xacro文件转换为纯URDF供其读取，或者直接提供一个转换后的临时URDF文件。

   cd ~/my_robot_arm_ws/src/my_robot_arm/urdf
   ros2 run xacro xacro my_robot_arm.urdf.xacro > my_robot_arm.urdf
   

   然后在Setup Assistant中选择这个 my_robot_arm.urdf 文件。

2. 生成自碰撞矩阵 ：在“Self-Collisions”标签页，点击“Regenerate Default Collision Matrix”。MoveIt2会计算机器人各部件之间默认应忽略的碰撞，这能显著提升规划速度。

3. 定义规划组 ：这是最关键的一步。在“Planning Groups”标签页：

   • 点击“Add Group”。
   • 组名 ： arm_group 。
   • 类型 ： Chain 。
   • 基座连杆 ： base_link 。
   • 末端提示连杆 ： flange 。这样会自动将 base_link 到 flange 之间的所有关节纳入该规划组。
   • 运动学求解器 ：选择 KDLKinematicsPlugin （默认）。
   • 点击“Save”。

4. 定义末端执行器 ：

   • 点击“Add End Effector”。
   • 名称 ： gripper 。
   • 规划组 ： arm_group 。
   • 父连杆 ： flange 。
   • 子连杆组 ： 我们需要为夹爪创建一个新的规划组。
     • 回到“Planning Groups”标签页，点击“Add Group”。
     • 组名 ： gripper_group 。
     • 类型 ： Joint Model 。
     • 在关节列表中，手动选择 flange_to_left_finger 和 flange_to_right_finger 这两个关节。
     • 点击“Save”。
   • 回到“End Effectors”标签页，在“End Effector”配置中， 子连杆组 选择 gripper_group 。
   • 点击“Save”。

5. 定义位姿 ：在“Robot Poses”标签页，可以定义一些预设位姿，如“home”。为 arm_group 设置各个关节的角度（例如全为0），并保存为“home”。

6. 生成配置文件 ：

   • 在“Configuration Files”标签页，选择输出路径。建议放在我们的功能包内，例如 ~/my_robot_arm_ws/src/my_robot_arm/config 。
   • 点击“Generate Package”。它会生成一个名为 my_robot_arm_moveit_config 的新包，里面包含了所有MoveIt2运行时需要的配置文件（如SRDF、kinematics.yaml、ompl_planning.yaml等）。

4.3 整合配置到我们的工作空间

将生成的配置包移动到我们的工作空间 src 目录下，并修改其 package.xml 和 CMakeLists.txt ，确保它依赖于我们的 my_robot_arm 包（描述URDF的包）。

# 假设Setup Assistant将包生成在了 ~/moveit_config 目录
mv ~/moveit_config/my_robot_arm_moveit_config ~/my_robot_arm_ws/src/

然后，我们需要编辑 my_robot_arm_moveit_config/package.xml ，添加对 my_robot_arm 的依赖：

<!-- 在 package.xml 的 <depend> 标签列表中添加 -->
<depend>my_robot_arm</depend>

5. 编写C++控制节点

现在，我们将编写一个C++节点，使用MoveIt2的C++接口来控制机械臂和夹爪。

5.1 创建节点源文件

在 my_robot_arm 包的 src 目录下创建文件 arm_controller.cpp 。

// 文件路径：~/my_robot_arm_ws/src/my_robot_arm/src/arm_controller.cpp
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/msg/display_robot_state.hpp>
#include <moveit_msgs/msg/display_trajectory.hpp>
#include <moveit_msgs/msg/attached_collision_object.hpp>
#include <moveit_msgs/msg/collision_object.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>

static const rclcpp::Logger LOGGER = rclcpp::get_logger("arm_controller");

int main(int argc, char** argv) {
  // 初始化ROS2
  rclcpp::init(argc, argv);
  rclcpp::NodeOptions node_options;
  node_options.automatically_declare_parameters_from_overrides(true);
  auto node = rclcpp::Node::make_shared("arm_controller_node", node_options);

  // 创建一个用于执行异步任务的单线程执行器
  rclcpp::executors::SingleThreadedExecutor executor;
  executor.add_node(node);
  std::thread([&executor]() { executor.spin(); }).detach();

  // 1. 初始化MoveGroupInterface
  // 用于控制机械臂规划组
  static const std::string PLANNING_GROUP_ARM = "arm_group";
  static const std::string PLANNING_GROUP_GRIPPER = "gripper_group";
  
  moveit::planning_interface::MoveGroupInterface move_group_arm(node, PLANNING_GROUP_ARM);
  moveit::planning_interface::MoveGroupInterface move_group_gripper(node, PLANNING_GROUP_GRIPPER);

  // 获取规划组名称和末端执行器链接
  RCLCPP_INFO(LOGGER, "Planning frame: %s", move_group_arm.getPlanningFrame().c_str());
  RCLCPP_INFO(LOGGER, "End effector link: %s", move_group_arm.getEndEffectorLink().c_str());
  RCLCPP_INFO(LOGGER, "Available Planning Groups:");
  for (const std::string& name : move_group_arm.getJointModelGroupNames()) {
    RCLCPP_INFO(LOGGER, "  %s", name.c_str());
  }

  // 2. 规划并移动到“home”位姿
  // 首先，设置一个目标位姿（这里使用之前定义的命名位姿“home”）
  move_group_arm.setNamedTarget("home");
  
  // 创建规划结果对象
  moveit::planning_interface::MoveGroupInterface::Plan my_plan_arm;
  
  // 进行运动规划
  bool success_arm = (move_group_arm.plan(my_plan_arm) == moveit::core::MoveItErrorCode::SUCCESS);
  RCLCPP_INFO(LOGGER, "Move to HOME pose %s", success_arm ? "SUCCESS" : "FAILED");
  
  // 如果规划成功，则执行该轨迹
  if (success_arm) {
    move_group_arm.execute(my_plan_arm);
  } else {
    RCLCPP_ERROR(LOGGER, "Failed to plan to HOME pose. Exiting.");
    rclcpp::shutdown();
    return 1;
  }

  // 等待2秒
  rclcpp::sleep_for(std::chrono::seconds(2));

  // 3. 规划并移动到目标位置（使用位姿目标）
  geometry_msgs::msg::Pose target_pose;
  target_pose.orientation.w = 1.0; // 四元数，表示无旋转
  target_pose.position.x = 0.3;
  target_pose.position.y = 0.1;
  target_pose.position.z = 0.4;
  move_group_arm.setPoseTarget(target_pose);

  success_arm = (move_group_arm.plan(my_plan_arm) == moveit::core::MoveItErrorCode::SUCCESS);
  RCLCPP_INFO(LOGGER, "Move to target pose %s", success_arm ? "SUCCESS" : "FAILED");
  
  if (success_arm) {
    move_group_arm.execute(my_plan_arm);
  } else {
    RCLCPP_WARN(LOGGER, "Planning to target pose failed, trying joint space goal instead.");
    // 如果位姿规划失败，可以尝试关节空间目标
    std::vector<double> joint_group_positions;
    move_group_arm.getCurrentState()->copyJointGroupPositions(
        move_group_arm.getCurrentState()->getJointModelGroup(PLANNING_GROUP_ARM),
        joint_group_positions);
    // 微调关节角度
    joint_group_positions[0] = 0.5; // 第一个关节旋转0.5弧度
    move_group_arm.setJointValueTarget(joint_group_positions);
    success_arm = (move_group_arm.plan(my_plan_arm) == moveit::core::MoveItErrorCode::SUCCESS);
    if (success_arm) {
        move_group_arm.execute(my_plan_arm);
    }
  }

  rclcpp::sleep_for(std::chrono::seconds(2));

  // 4. 控制夹爪闭合
  // 夹爪的两个关节是平移关节，设置其目标位置（单位：米）
  std::vector<double> gripper_close_position = {0.015, 0.015}; // 两个手指都向内移动1.5cm
  move_group_gripper.setJointValueTarget(gripper_close_position);
  
  moveit::planning_interface::MoveGroupInterface::Plan my_plan_gripper;
  bool success_gripper = (move_group_gripper.plan(my_plan_gripper) == moveit::core::MoveItErrorCode::SUCCESS);
  RCLCPP_INFO(LOGGER, "Close gripper %s", success_gripper ? "SUCCESS" : "FAILED");
  if (success_gripper) {
    move_group_gripper.execute(my_plan_gripper);
  }

  rclcpp::sleep_for(std::chrono::seconds(1));

  // 5. 控制夹爪打开
  std::vector<double> gripper_open_position = {0.0, 0.0}; // 回到初始位置
  move_group_gripper.setJointValueTarget(gripper_open_position);
  success_gripper = (move_group_gripper.plan(my_plan_gripper) == moveit::core::MoveItErrorCode::SUCCESS);
  RCLCPP_INFO(LOGGER, "Open gripper %s", success_gripper ? "SUCCESS" : "FAILED");
  if (success_gripper) {
    move_group_gripper.execute(my_plan_gripper);
  }

  rclcpp::sleep_for(std::chrono::seconds(1));

  // 6. 再次移动机械臂并闭合夹爪（模拟抓取-放置循环）
  target_pose.position.z = 0.3;
  move_group_arm.setPoseTarget(target_pose);
  success_arm = (move_group_arm.plan(my_plan_arm) == moveit::core::MoveItErrorCode::SUCCESS);
  if (success_arm) {
    move_group_arm.execute(my_plan_arm);
    rclcpp::sleep_for(std::chrono::seconds(1));
    // 闭合夹爪
    move_group_gripper.setJointValueTarget(gripper_close_position);
    move_group_gripper.plan(my_plan_gripper);
    move_group_gripper.execute(my_plan_gripper);
  }

  RCLCPP_INFO(LOGGER, "Demo completed. Shutting down.");
  rclcpp::shutdown();
  return 0;
}

5.2 修改CMakeLists.txt

编辑 my_robot_arm 包下的 CMakeLists.txt ，添加可执行文件的构建规则。

# 在 CMakeLists.txt 的 find_package 部分，确保包含以下包
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)
find_package(moveit_core REQUIRED)
find_package(moveit_ros_planning_interface REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)

# 添加可执行文件并链接库
add_executable(arm_controller src/arm_controller.cpp)
target_include_directories(arm_controller PRIVATE
  ${moveit_core_INCLUDE_DIRS}
  ${moveit_ros_planning_interface_INCLUDE_DIRS}
)
ament_target_dependencies(arm_controller
  rclcpp
  moveit_core
  moveit_ros_planning_interface
  tf2_ros
  tf2_geometry_msgs
)

# 安装目标（可选，便于部署）
install(TARGETS arm_controller
  DESTINATION lib/${PROJECT_NAME}
)

5.3 修改package.xml

确保 package.xml 包含了所有必要的依赖。

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>my_robot_arm</name>
  <version>0.0.0</version>
  <description>My custom robot arm with MoveIt2 control</description>
  <maintainer email="you@example.com">Your Name</maintainer>
  <license>Apache License 2.0</license>

  <buildtool_depend>ament_cmake</buildtool_depend>

  <depend>rclcpp</depend>
  <depend>std_msgs</depend>
  <depend>sensor_msgs</depend>
  <depend>geometry_msgs</depend>
  <depend>moveit_core</depend>
  <depend>moveit_ros_planning_interface</depend>
  <depend>tf2_ros</depend>
  <depend>tf2_geometry_msgs</depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

6. 运行与验证

现在，让我们构建并运行整个系统，看看机械臂是否能在RViz中动起来。

6.1 构建工作空间

cd ~/my_robot_arm_ws
colcon build --symlink-install
source install/setup.bash

6.2 启动MoveIt2和RViz

首先，启动MoveIt2配置包提供的演示启动文件，它会加载机器人模型、MoveIt2核心节点并打开RViz可视化界面。

ros2 launch my_robot_arm_moveit_config demo.launch.py

保持这个终端运行。你应该能看到RViz窗口打开，里面显示着我们的机械臂模型。

6.3 运行控制节点

打开一个新的终端，source工作空间后运行我们写的C++节点：

cd ~/my_robot_arm_ws
source install/setup.bash
ros2 run my_robot_arm arm_controller

6.4 观察结果

在RViz中，你应该能看到：

1. 机械臂首先移动到“home”位姿（各关节为0）。
2. 然后规划并运动到目标位置 (0.3, 0.1, 0.4) 。
3. 夹爪执行闭合动作。
4. 夹爪打开。
5. 机械臂再次运动到 (0.3, 0.1, 0.3) 并闭合夹爪。

同时，在运行控制节点的终端里，会打印出每一步规划成功或失败的信息。

7. 常见问题与排查思路

在实际操作中，你可能会遇到一些问题。以下是常见问题的排查指南。

问题现象	可能原因	解决思路
启动 demo.launch.py 时报错，找不到包或节点	1. 工作空间未构建成功。 2. 环境变量未正确source。 3. 配置包未正确移动到工作空间或依赖缺失。	1. 检查 colcon build 是否有错误。 2. 确保在每个新终端都执行 source ~/my_robot_arm_ws/install/setup.bash 。 3. 检查 my_robot_arm_moveit_config 包的 package.xml 是否包含 <depend>my_robot_arm</depend> 。
RViz中看不到机器人模型	1. URDF文件路径错误或格式有误。 2. robot_description 参数未正确加载。	1. 检查URDF文件是否能被 xacro 正确解析： ros2 run xacro xacro my_robot_arm.urdf.xacro 。 2. 在终端运行 `ros2 param list
规划失败 (Plan Failed)	1. 目标位姿超出工作空间或处于自碰撞状态。 2. 规划时间太短。 3. 规划算法参数不合适。	1. 在RViz中使用“Interactive Markers”手动拖拽末端到一个可达位置，再尝试规划。 2. 在代码中增加规划时间： move_group_arm.setPlanningTime(10.0); 。 3. 检查 ompl_planning.yaml 中的规划算法配置。
执行轨迹时机器人不动	1. move_group.execute() 被调用，但轨迹控制器未运行。 2. 仿真的 joint_state_publisher 或 robot_state_publisher 未启动。	1. 确保 demo.launch.py 正确启动了 move_group 和 fake_controller （对于仿真）。 2. 检查 /joint_states 话题是否有数据发布。
夹爪控制不生效	1. 夹爪规划组 gripper_group 定义不正确。 2. 关节限位设置过小或目标位置超出限位。	1. 在Setup Assistant中重新检查 gripper_group 的关节列表。 2. 检查URDF中夹爪关节的 limit 标签，确保目标位置在 lower 和 upper 之间。
C++节点编译错误	1. 缺少头文件。 2. 找不到MoveIt2库。	1. 检查 CMakeLists.txt 中的 find_package 和 target_include_directories 。 2. 确保MoveIt2工作空间 ( ~/moveit2_ws ) 已被source。可以尝试 source ~/moveit2_ws/install/setup.bash 。
运行时出现TF转换错误	坐标系树不完整或发布频率过低。	1. 检查URDF中所有关节的父子连接关系是否正确。 2. 确保 robot_state_publisher 节点正在运行。

8. 最佳实践与工程建议

将MoveIt2集成到实际项目中时，遵循以下最佳实践可以避免很多坑。

1. URDF/Xacro 建模规范

• 模块化 ： 使用Xacro宏和文件包含来管理复杂的机器人模型，提高复用性。
• 质量与惯性 ： 务必为每个 <link> 定义合理的 <inertial> 属性，尤其是质量。不准确的动力学参数会导致运动规划和控制不准确。
• 碰撞几何体 ： <collision> 几何体应尽可能简化（如使用长方体、圆柱体、球体组合），以提升碰撞检测效率。它可以比 <visual> 几何体更简单。
• 关节限位 ： 在 <joint> 的 <limit> 中设置真实、安全的物理限位，这是运动规划安全的基石。

2. MoveIt2 配置优化

• 规划组划分 ： 合理规划组。例如，将移动底盘和机械臂分开定义，便于独立或协调控制。
• 自碰撞矩阵 ： 务必使用Setup Assistant生成并仔细检查自碰撞矩阵。对于永远不会接触的部件（如底座和末端），可以禁用碰撞检查以提升规划速度。
• 规划器参数调优 ： 默认的OMPL规划器参数可能不适合你的机器人。在 ompl_planning.yaml 中，针对你的规划组调整 planning_time 、 num_planning_attempts 等参数。
• 使用位姿目标缓存 ： 对于常用的抓取、放置等位姿，在SRDF中定义为“位姿”，便于代码中通过 setNamedTarget 调用，提高可读性和可靠性。

3. C++ 代码健壮性

• 错误检查 ： 始终检查 plan() 和 execute() 的返回值。 MoveItErrorCode 提供了丰富的错误信息。
• 异步执行 ： 对于长时间运行的任务，考虑使用 asyncExecute() 并配合回调函数，避免阻塞主线程。
• 状态查询 ： 在执行动作前，通过 getCurrentState() 获取当前机器人状态，作为规划的起点或进行条件判断。
• 轨迹监控 ： 可以订阅 /execute_trajectory/feedback 等话题来监控轨迹的执行状态，实现更精细的控制。

4. 仿真与实物部署

• 控制器配置 ： demo.launch.py 使用的是 fake_controller ，它只发布关节状态用于RViz显示。连接真实机器人时，需要在 controllers.yaml 中配置真实的硬件控制器（如 position_controllers/JointTrajectoryController ），并确保其与机器人硬件接口（如 ros2_control ）正确对接。
• 启动文件管理 ： 为仿真和实物创建不同的启动文件。仿真文件加载 fake_controller 和 joint_state_publisher_gui ；实物文件则加载硬件控制器和驱动节点。
• 网络配置 ： 在多机或分布式系统中，确保ROS_DOMAIN_ID设置一致，并且网络 multicast 正常。

5. 安全与监控

• 关节限位保护 ： 在硬件驱动层和MoveIt2规划层设置双重关节限位保护。
• 碰撞检测 ： 启用并配置好环境碰撞物体。使用 PlanningSceneInterface 动态添加/移除障碍物。
• 超时与重试 ： 为规划和执行操作设置合理的超时时间，并实现重试逻辑。
• 日志记录 ： 充分利用ROS2的日志系统，对不同级别的信息（DEBUG, INFO, WARN, ERROR）进行记录，便于后期排查问题。

通过本教程，你完成了从零搭建一个基于ROS2和MoveIt2的机械臂仿真控制系统的全过程。你掌握了URDF建模、MoveIt Setup Assistant配置、以及使用C++ API进行运动规划和夹爪控制的核心技能。这套流程是连接机器人算法（如视觉识别、路径规划）与物理执行器的关键桥梁，也是具身智能研究中实现“手眼协调”等复杂任务的基础。建议你以此项目为起点，尝试集成摄像头进行视觉伺服抓取，或者添加力传感器实现柔顺控制，不断探索机器人技术的更多可能性。如果在实践中遇到问题，多查阅ROS2和MoveIt2的官方文档，以及社区论坛，大部分难题都能找到解决方案。