ROS/MoveIt 的使用以及源码核心解读

下面详细说明 ROS/MoveIt 的使用以及源码核心解读，解析一下 ROS MoveIt 的源码结构和核心组件并提供一些代码片段。

一、MoveIt 核心概念和功能

MoveIt 是 ROS 中一个非常强大的软件包，用于机器人运动规划、操作、3D 感知、运动学、控制和导航。它提供了一个易于使用的平台，可以处理复杂的机器人操作任务。

• 运动规划 (Motion Planning): MoveIt 的核心功能是找到从起点到目标点的无碰撞路径。它使用各种运动规划算法（如 RRT、PRM）来搜索机器人的配置空间。
• 运动学 (Kinematics): MoveIt 包含正向和逆向运动学求解器。正向运动学计算给定关节角度的末端执行器位置，而逆向运动学计算给定末端执行器姿态的关节角度。
• 碰撞检测 (Collision Detection): MoveIt 使用灵活碰撞库 (FCL) 来检测机器人与环境之间的碰撞。
• 约束 (Constraints): 可以在运动规划过程中添加约束，例如路径约束（保持末端执行器在特定方向上）或姿态约束（保持末端执行器接近特定姿态）。
• 轨迹执行 (Trajectory Execution): MoveIt 可以将规划的轨迹发送到机器人控制器执行。
• 感知 (Perception): MoveIt 可以与感知软件包（如 OctoMap）集成，以构建环境的 3D 地图，并在规划过程中考虑障碍物。
• 用户界面 (GUI): MoveIt 提供了 RViz 插件，可以可视化机器人模型、规划场景、规划结果和执行轨迹。

核心目标：

1. 易用性： 提供简单的接口（C++, Python, GUI）来执行运动规划任务。
2. 安全性： 集成碰撞检测，确保规划出的路径不会与机器人自身或环境发生碰撞。
3. 灵活性： 通过插件机制支持不同的运动规划算法、运动学求解器、传感器等。
4. 集成性： 与 ROS 生态系统紧密集成（TF, ros_control, 传感器驱动等）。

源码仓库:

MoveIt 的核心代码主要分布在 GitHub 的 ros-planning 组织下：

• moveit: 主要的核心包，包含 move_group、运动规划器接口、运动学插件接口、碰撞检测、场景表示等。 (https://github.com/ros-planning/moveit)
• moveit_msgs: 定义 MoveIt 使用的 ROS 消息、服务和动作类型。 (https://github.com/ros-planning/moveit_msgs)
• moveit_resources: 包含用于测试和示例的机器人模型（URDF/SRDF）和配置文件。 (https://github.com/ros-planning/moveit_resources)
• geometric_shapes: 用于表示和处理基本几何形状（用于碰撞检测）。 (https://github.com/ros-planning/geometric_shapes)
• srdfdom: 用于解析 SRDF (Semantic Robot Description Format) 文件。 (https://github.com/ros-planning/srdfdom)
• moveit_visual_tools: 提供在 RViz 中进行可视化调试的便捷工具。 (https://github.com/ros-planning/moveit_visual_tools)
• 特定规划器/求解器仓库: 如 moveit_planners (OMPL, STOMP, CHOMP), bio_ik, trac_ik 等。

二、MoveIt 架构

MoveIt 的主要组件包括：

• MoveGroupInterface: 这是与 MoveIt 交互的主要 C++ 接口。它提供了用于设置规划场景、目标、约束和执行规划的函数。
• Planning Scene: 规划场景表示机器人和环境的当前状态。它包括机器人模型、碰撞对象和附加的传感器数据。
• Motion Planners: MoveIt 支持多种运动规划算法（OMPL 库提供了许多实现）。
• Kinematics Solvers: MoveIt 使用运动学插件（如 KDL、IKFast）来求解正向和逆向运动学。
• Collision Checking: 使用 FCL 进行碰撞检测。

核心架构与关键组件详解

下面是 MoveIt 的关键组件及其在源码中的大致位置（以 moveit 主仓库为例）：

1. move_group 节点 (核心)

   • 位置: moveit/moveit_ros/move_group/src/move_group.cpp
   • 功能: 这是用户与 MoveIt 交互的主要入口点。它作为一个 ROS 节点运行，提供 Action 和 Service 接口来处理运动规划请求、执行轨迹、获取机器人状态、更新规划场景等。
   • 内部机制:
     • 集成者: move_group 整合了 MoveIt 的各个核心库（规划、场景、运动学、执行）。
     • Action/Service 服务器: 实现了 MoveGroupAction, GetPlanningScene, ExecuteTrajectoryAction 等核心接口。
     • 状态管理: 维护当前的机器人状态和规划场景。
     • 能力（Capabilities）: 通过插件加载不同的功能模块（如运动规划、运动学计算、轨迹执行）。常见的 Capabilities 源码位于 moveit/moveit_ros/move_group/src/default_capabilities/ 目录下，例如 move_action_capability.cpp（处理规划和执行请求）、kinematics_service_capability.cpp（提供运动学服务）。
   • 交互: 用户通常通过 MoveGroupInterface (C++) 或 MoveGroupCommander (Python) 与 move_group 节点交互。

2. 规划场景 (Planning Scene)

   • 位置: moveit/moveit_core/planning_scene/src/
   • 功能: 维护机器人当前状态以及其所处环境的表示。这是进行碰撞检测和约束检查的基础。
   • 核心类:
     • planning_scene::PlanningScene：表示一个特定的世界状态，包括机器人模型、机器人当前状态（关节角度）、附加到机器人的物体、以及世界中的碰撞物体。它提供了碰撞检测、距离查询、约束检查等核心功能。
     • planning_scene_monitor::PlanningSceneMonitor：(位于 moveit/moveit_ros/planning_scene_monitor/src/) 这是一个更高级别的类，通常在 move_group 内部使用。它负责：
       • 监听 /joint_states 或其他关节状态来源，更新机器人的当前状态。
       • 监听 TF 变换，更新机器人模型的位置。
       • 监听传感器输入（如 /point_cloud, /depth_image），并使用 OctoMap 或其他方式更新环境表示（通过 moveit/moveit_ros/perception 中的插件）。
       • 提供对当前 PlanningScene 的线程安全访问。
       • 管理场景的发布（例如发布到 /planning_scene topic 供 RViz 显示）。
   • 碰撞检测: PlanningScene 内部通常使用 FCL (Flexible Collision Library) 或 Bullet 进行底层的碰撞检测计算。相关的封装代码在 moveit/moveit_core/collision_detection_fcl/ 或 moveit/moveit_core/collision_detection_bullet/。

3. 运动规划器接口与插件 (Motion Planners)

   • 位置:
     • 接口: moveit/moveit_core/planning_interface/include/moveit/planning_interface/
     • OMPL 插件: moveit_planners/ompl_interface/
     • STOMP 插件: moveit_planners/stomp_ros/
     • CHOMP 插件: moveit_planners/chomp_motion_planner/
     • Pilz Industrial Motion Planner: moveit/moveit_planners/pilz_industrial_motion_planner/
   • 功能: 负责根据起始状态、目标状态和约束条件，在无碰撞的空间中计算出一条可行的运动轨迹。
   • 核心概念:
     • planning_interface::PlannerManager：规划器插件的基类接口。每个具体的规划库（如 OMPL）都需要实现这个接口。
     • planning_interface::PlanningContext：表示一次具体的规划请求的上下文，包含规划场景、规划请求等信息。
   • 插件机制: move_group 通过 ROS 插件库 (pluginlib) 加载配置文件中指定的运动规划器插件。这使得用户可以轻松切换或添加不同的规划算法。最常用的是 OMPL (Open Motion Planning Library) 插件。
   • 交互: move_group 中的 PlanningPipeline (位于 moveit/moveit_ros/planning/planning_pipeline/src/) 负责接收规划请求，选择合适的规划器插件，调用其 solve() 方法，并处理返回的轨迹（例如进行后处理、时间参数化）。

4. 运动学求解器接口与插件 (Kinematics Solvers)

   • 位置:
     • 接口: moveit/moveit_core/kinematics_base/include/moveit/kinematics_base/
     • KDL 插件: moveit/moveit_kinematics/kdl_kinematics_plugin/
     • TRAC-IK 插件: (独立仓库) trac_ik/trac_ik_kinematics_plugin
     • IKFast 插件: 需要用户自行生成并集成。
   • 功能:
     • 正向运动学 (FK): 根据给定的关节角度计算末端执行器的位姿。
     • 逆向运动学 (IK): 根据给定的末端执行器位姿计算对应的（一组）关节角度。
   • 核心概念:
     • kinematics::KinematicsBase：运动学求解器插件的基类接口。定义了 getPositionIK, getPositionFK, searchPositionIK 等核心虚函数。
   • 插件机制: 同样使用 pluginlib 加载。通常在 SRDF 文件中为每个规划组（planning group）指定一个运动学求解器。
   • 交互: RobotState 类使用运动学求解器来更新状态；move_group 可能在规划前调用 IK 来确定目标关节状态，或者在规划过程中用于满足姿态约束。IK 服务 (GetPositionIK) 也由 move_group 提供。

5. 机器人模型与状态 (Robot Model & State)

   • 位置: moveit/moveit_core/robot_model/src/ 和 moveit/moveit_core/robot_state/src/
   • 功能: 在内存中表示机器人的结构和当前状态。
   • 核心类:
     • robot_model::RobotModel：加载 URDF 和 SRDF 文件，构建机器人的运动学结构、关节限制、碰撞几何体、规划组（JointModelGroup）等静态信息。这是一个只读的数据结构。
     • robot_state::RobotState：表示机器人在某个特定时刻的状态，包括所有关节的位置、速度、加速度，以及附着在机器人上的物体。它提供了设置/获取关节角度、计算 FK/IK（通过关联的 RobotModel 和运动学插件）、检查碰撞（通过关联的 PlanningScene）、检查约束等方法。RobotState 是可变的。
     • robot_state::JointModelGroup：表示 RobotModel 中的一个规划组（例如 “manipulator”, “endeffector”），包含该组的关节、链接、运动学求解器、默认状态等信息。

6. 轨迹执行管理 (Trajectory Execution Manager)

   • 位置: moveit/moveit_ros/trajectory_execution_manager/src/
   • 功能: 负责接收规划好的轨迹 (RobotTrajectory)，并将其发送给实际的机器人控制器执行。
   • 核心类: trajectory_execution_manager::TrajectoryExecutionManager
   • 内部机制:
     • 控制器接口: 通过插件机制与不同的控制器接口交互。最常见的是 FollowJointTrajectory Action 接口 (用于 ros_control)。相关代码在 moveit_plugins/moveit_ros_control_interface。
     • 轨迹验证: 在执行前可以再次验证轨迹的合法性。
     • 执行监控: 监控轨迹执行过程，处理成功、失败或中止的情况。
   • 交互: move_group 在规划成功后，会将生成的轨迹传递给 TrajectoryExecutionManager 来执行。

7. 配置与描述文件

   • URDF (Unified Robot Description Format): 定义机器人的物理结构（连杆、关节、惯性、视觉、碰撞几何体）。MoveIt 直接使用 ROS 标准的 URDF。
   • SRDF (Semantic Robot Description Format):
     • 位置: 由 srdfdom 包解析。
     • 功能: 为 URDF 添加语义信息，供 MoveIt 使用。包括：
       • 定义规划组 (Planning Groups)。
       • 定义末端执行器 (End-Effectors)。
       • 定义默认或常用的机器人姿态 (Named Poses)。
       • 指定关节组的运动学求解器。
       • 禁用特定的连杆对之间的碰撞检测（例如，相邻连杆）。
   • YAML 文件:
     • kinematics.yaml: 配置每个规划组使用的运动学求解器插件及其参数。
     • ompl_planning.yaml (或其他规划器配置): 配置运动规划器插件、可用的规划算法及其参数。
     • joint_limits.yaml: (可选) 覆盖 URDF 中的关节限制，或添加速度/加速度限制。
     • controllers.yaml: 配置 MoveIt 如何与 ros_control 或其他控制器接口交互。
     • moveit_controllers.yaml: (较新版本) 更精细地配置控制器。

8. 可视化 (RViz Plugin)

   • 位置: moveit/moveit_ros/visualization/rviz_plugin_render_tools/ 和 moveit/moveit_ros/visualization/motion_planning_rviz_plugin/
   • 功能: 在 RViz 中提供强大的交互式界面和可视化工具。
   • 组件:
     • MotionPlanning Display: 主要的 RViz 显示插件。
     • 机器人状态显示: 显示当前、起始、目标状态。
     • 规划场景显示: 显示环境碰撞物体、附加物体。
     • 规划路径可视化: 显示规划出的轨迹。
     • 交互式标记: 允许用户拖动末端执行器来设定目标姿态。
     • 规划请求: 可以直接在 RViz 中发起规划请求。
   • 交互: RViz 插件通过 ROS Topic, Service, Action 与 move_group 和 PlanningSceneMonitor 通信。

典型工作流程 (以 C++ MoveGroupInterface 为例):

1. 初始化: 创建 MoveGroupInterface 实例，指定规划组名称。它会连接到 move_group 节点的 Action 和 Service。
2. 设置目标: 调用 setPoseTarget(), setJointValueTarget(), setNamedTarget() 等方法设置规划目标。
3. 规划: 调用 plan() 方法。
   • MoveGroupInterface 向 move_group 发送 MoveGroupAction 的 Goal (只包含规划请求)。
   • move_group 接收 Goal，在其 MoveActionCapability 中处理。
   • move_group 获取当前的 PlanningScene。
   • 调用 PlanningPipeline。
   • PlanningPipeline 选择规划器插件 (如 OMPL)。
   • OMPL 插件使用 PlanningScene 进行碰撞检测，计算出 RobotTrajectory。
   • PlanningPipeline (可选) 对轨迹进行后处理（如时间参数化）。
   • move_group 将规划结果 (MotionPlanResponse) 返回给 MoveGroupInterface。
4. 执行 (可选): 调用 execute() 方法，传入上一步得到的 MotionPlanResponse。
   • MoveGroupInterface 向 move_group 发送 MoveGroupAction 的 Goal (包含要执行的轨迹)。
   • move_group 接收 Goal。
   • move_group 将轨迹传递给 TrajectoryExecutionManager。
   • TrajectoryExecutionManager 选择合适的控制器接口 (如 FollowJointTrajectory Action Client)。
   • 将 RobotTrajectory 转换为控制器期望的格式 (如 trajectory_msgs/JointTrajectory)。
   • 发送给控制器执行。
   • 监控执行状态，并将结果返回给 MoveGroupInterface。
5. 组合规划与执行: 调用 move() 方法，内部会先调用 plan() 再调用 execute()。

如何探索源码:

1. 从接口入手: 查看 MoveGroupInterface (C++) 或 MoveGroupCommander (Python) 的代码，了解它们如何调用 move_group 的 Action 和 Service。
2. 研究 move_group: 这是核心节点，理解它的 Capabilities 机制和如何协调其他组件至关重要。
3. 跟踪核心流程: 选择一个简单的操作（如 plan()），使用 IDE 的代码跳转功能或 grep/ctags/cscope 跟踪其调用链，从接口层 -> move_group -> PlanningPipeline -> 规划器插件。
4. 理解核心数据结构: 阅读 RobotModel, RobotState, PlanningScene 的头文件和注释，了解它们的作用和提供的 API。
5. 查看插件接口: 阅读 planning_interface, kinematics_base 等基类的头文件，了解插件需要实现哪些功能。
6. 关注配置文件: 理解 URDF, SRDF 和 YAML 文件如何影响 MoveIt 的行为。
7. 使用调试工具: GDB、日志 (ROS_INFO, ROS_DEBUG 等)、RViz 可视化都是理解运行时行为的重要手段。

三、MoveIt 使用步骤

1. 配置 MoveIt:

   • 使用 MoveIt Setup Assistant 为你的机器人创建一个 MoveIt 配置包。这个包包含机器人的 URDF 模型、SRDF（语义机器人描述格式）文件、配置文件和启动文件。
   • SRDF 文件定义了机器人的规划组、末端执行器、虚拟关节以及禁用碰撞检查的关节对。

2. 创建 MoveGroupInterface 对象:

   #include <moveit/move_group_interface/move_group_interface.h>
   
   moveit::planning_interface::MoveGroupInterface move_group("manipulator"); // "manipulator" 是你在SRDF中定义的规划组的名称
   

3. 设置规划场景 (可选):

   • 添加/移除碰撞对象。
   • 附加/分离对象到机器人。
   • 更新传感器数据（例如，使用 OctoMap）。

   #include <moveit/planning_scene_interface/planning_scene_interface.h>
   
   moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
   
   // 添加碰撞对象
   moveit_msgs::CollisionObject collision_object;
   collision_object.header.frame_id = move_group.getPlanningFrame();
   collision_object.id = "box1";
   // ... 设置 box1 的形状、姿态等 ...
   
   std::vector<moveit_msgs::CollisionObject> collision_objects;
   collision_objects.push_back(collision_object);
   planning_scene_interface.addCollisionObjects(collision_objects);
   

4. 设置目标:

   • 关节空间目标: 设置每个关节的目标角度。
   • 姿态目标: 设置末端执行器的目标位置和姿态（四元数或欧拉角）。
   • 多个目标: 可以使用 move_group.setJointValueTarget() 或 move_group.setPoseTarget()。

   // 关节空间目标
   std::vector<double> joint_group_positions;
   // ... 将 joint_group_positions 设置为目标关节角度 ...
   move_group.setJointValueTarget(joint_group_positions);
   
   // 姿态目标
   geometry_msgs::Pose target_pose1;
   target_pose1.orientation.w = 1.0;
   target_pose1.position.x = 0.28;
   target_pose1.position.y = -0.2;
   target_pose1.position.z = 0.5;
   move_group.setPoseTarget(target_pose1);
   
   //设置允许的规划时间
   move_group.setPlanningTime(10.0);
   

5. 规划:

   moveit::planning_interface::MoveGroupInterface::Plan my_plan;
   bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
   
   if (success) {
     ROS_INFO("Plan found!");
   } else {
     ROS_INFO("No plan found.");
     return; // 如果没有找到规划，则退出
   }
   

6. 执行 (可选):

   move_group.execute(my_plan); 
   

   或者，可以使用 move_group.move() 一步完成规划和执行：

    bool success = (move_group.move() == moveit::planning_interface::MoveItErrorCode::SUCCESS);
   

示例 (完整代码片段 - C++)

#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <geometry_msgs/Pose.h>

int main(int argc, char** argv) {
  ros::init(argc, argv, "moveit_example");
  ros::NodeHandle node_handle;
  ros::AsyncSpinner spinner(1);
  spinner.start();

  // 创建 MoveGroupInterface 对象
  moveit::planning_interface::MoveGroupInterface move_group("manipulator"); // 替换为你的规划组名称

  // 设置目标姿态
  geometry_msgs::Pose target_pose;
  target_pose.orientation.w = 1.0;
  target_pose.position.x = 0.5;
  target_pose.position.y = 0.0;
  target_pose.position.z = 0.6;
  move_group.setPoseTarget(target_pose);

    //设置允许的规划时间
  move_group.setPlanningTime(10.0);

  // 规划
  moveit::planning_interface::MoveGroupInterface::Plan my_plan;
  bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);

  if (success) {
    ROS_INFO("Plan found! Executing...");
    // 执行
    move_group.execute(my_plan);
  } else {
    ROS_INFO("No plan found.");
  }

  ros::shutdown();
  return 0;
}

Python 示例

#!/usr/bin/env python

import sys
import copy
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg

moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface_tutorial', anonymous=True)

robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group = moveit_commander.MoveGroupCommander("manipulator")  # 替换为你的规划组名称

# 设置目标姿态
pose_target = geometry_msgs.msg.Pose()
pose_target.orientation.w = 1.0
pose_target.position.x = 0.7
pose_target.position.y = -0.05
pose_target.position.z = 1.1
group.set_pose_target(pose_target)

# 设置规划时间
group.set_planning_time(10.0)

# 规划并执行
plan = group.go(wait=True)  # go() 方法结合了 plan() 和 execute()

# 清理
group.stop()
group.clear_pose_targets()

moveit_commander.roscpp_shutdown()

重要注意事项

• URDF 和 SRDF: 确保你的机器人 URDF 模型准确描述了机器人的连杆和关节，SRDF 文件正确定义了规划组和末端执行器。
• 规划组: 在 SRDF 文件中定义规划组，并在代码中使用正确的规划组名称。
• 坐标系: 注意不同坐标系之间的转换。MoveIt 通常使用机器人的基坐标系作为参考坐标系。
• 规划时间: 根据任务的复杂性设置合适的规划时间。如果规划时间太短，可能找不到解。
• 错误处理: 检查 plan() 和 execute() 函数的返回值，以确保规划和执行成功。
• 仿真 vs. 真实机器人: 在将代码部署到真实机器人之前，先在仿真环境中进行测试。

RViz 可视化

1. 启动 MoveIt 配置包的 demo.launch 文件: 这将启动 RViz 并加载机器人模型和 MoveIt 配置。
2. 添加 MotionPlanning 显示: 在 RViz 的 Displays 窗口中，点击 Add，选择 MotionPlanning。
3. 设置规划组: 在 MotionPlanning 显示的 Global Options 中，设置 Robot Description 和 Planning Scene Topic。
4. 交互式标记: 使用 RViz 中的交互式标记来设置目标姿态。
5. 规划和执行: 在 MotionPlanning 显示的 Planning 标签中，点击 Plan 或 Plan & Execute 按钮。

进阶主题

• 约束 (Constraints): 路径约束、姿态约束、关节约束。
• 自定义规划器 (Custom Planners): 如果 OMPL 中的规划器不能满足你的需求，可以创建自定义规划器。
• 轨迹后处理 (Trajectory Post-processing): 对规划的轨迹进行平滑处理。
• 与感知集成 (Integration with Perception): 使用 OctoMap 等工具构建环境地图，并将其用于避障。
• MoveIt! Task Constructor: 构造复杂的操作任务。

MoveIt 是一个功能丰富但结构复杂的系统。希望这些详细的说明和示例能帮助你入门 MoveIt! ，能帮助你理解其核心架构和关键组件，并为深入探索源码提供一个好的起点。@TOC