1. 项目概述从零构建你的第一个MoveIt2 C应用如果你正在ROS 2 Humble环境下看着MoveIt2的官方文档感到无从下手想写一个能控制机械臂真实动起来的程序却不知从何编译起那么这篇内容就是为你准备的。我花了相当一段时间在Humble版本上从配置环境、理解CMakeLists.txt的坑到调试出第一个能让仿真机械臂在RViz里画出一条平滑轨迹的程序把整个过程里最关键、最易卡住的环节都梳理了出来。这不是对官方教程的简单翻译而是一个实战派开发者踩过坑、验证过的“生存指南”。我们将聚焦于创建一个最精简、但五脏俱全的C节点它能够调用MoveIt2的API规划一条机械臂末端执行器比如夹爪从点A到点B的运动轨迹并能在RViz中可视化。无论你是机器人方向的学生还是刚转入ROS 2生态的工程师这篇内容都将帮你跨过“从看懂到跑通”的第一道门槛。2. 环境准备与工作空间剖析在开始敲代码之前一个正确配置的ROS 2 Humble环境和清晰的项目结构能避免后续至少80%的编译和依赖问题。很多人卡在第一步就是因为环境没装对或者工作空间没理清。2.1 ROS 2 Humble与MoveIt2安装要点官方安装指南通常会让你apt install一大堆元包但对于开发而言我更推荐精准安装保持系统简洁。首先确保你的ROS 2 Humble是从二进制包安装且来源正确。打开终端依次输入以下命令进行验证和安装# 1. 检查ROS 2环境是否已正确source echo $ROS_DISTRO # 应该输出 ‘humble‘如果没有需要手动source source /opt/ros/humble/setup.bash # 2. 安装MoveIt2核心包和教程依赖这是最小集足够我们入门使用 sudo apt update sudo apt install ros-humble-moveit sudo apt install ros-humble-moveit-resources-pr2-description # 用于获取测试用的机器人模型 sudo apt install ros-humble-rviz2 ros-humble-moveit-visual-tools ros-humble-xacro注意ros-humble-moveit这个包是一个元包metapackage它本身不包含太多代码但会拉取一系列MoveIt2运行所需的依赖包如moveit_core,moveit_ros_planning等。安装时请确保网络通畅因为包体积较大。安装完成后一个快速的验证方法是尝试运行MoveIt2的演示配置助手Setup Assistant虽然我们本次不用它但能运行说明基础安装成功ros2 launch moveit_setup_assistant setup_assistant.launch.py如果能看到GUI界面弹出说明MoveIt2框架安装基本没问题可以关掉它继续下一步。2.2 创建专属的工作空间与功能包我强烈建议为这个教程单独创建一个工作空间与你的其他项目隔离避免依赖冲突。假设我们的工作空间叫moveit2_tutorial_ws。# 创建并进入工作空间目录 mkdir -p ~/moveit2_tutorial_ws/src cd ~/moveit2_tutorial_ws/src接下来创建我们的功能包。这里有个关键点MoveIt2功能包的CMakeLists.txt和package.xml与普通ROS 2包有显著不同因为它需要链接一系列复杂的MoveIt库。我们使用ament_cmake构建类型并手动声明依赖。# 创建功能包名字就叫 first_moveit_program ros2 pkg create first_moveit_program \ --build-type ament_cmake \ --dependencies rclcpp moveit_core moveit_ros_planning_interface tf2_ros tf2_geometry_msgs创建完成后进入包目录查看结构cd first_moveit_program ls你应该会看到CMakeLists.txt,package.xml,include/和src/目录。现在打开package.xml文件我们需要对其进行关键性修改。默认生成的依赖可能不全我们需要确保它包含MoveIt规划、执行、可视化以及机器人模型解析所需的所有组件。用文本编辑器如nano或vscode打开package.xml在depend标签区域进行补充修改后关键部分如下?xml version1.0? ?xml-model hrefhttp://download.ros.org/schema/package_format3.xsd schematypenshttp://www.w3.org/2001/XMLSchema? package format3 namefirst_moveit_program/name version0.0.0/version descriptionMy first MoveIt2 C program/description maintainer emailyouexample.comYour Name/maintainer licenseApache License 2.0/license !-- 关键的依赖声明 -- dependrclcpp/depend dependgeometry_msgs/depend dependtf2_ros/depend dependtf2_geometry_msgs/depend !-- MoveIt2 核心依赖 -- dependmoveit_core/depend dependmoveit_ros_planning/depend dependmoveit_ros_planning_interface/depend dependmoveit_ros_move_group/depend dependmoveit_visual_tools/depend !-- 用于加载机器人模型 -- dependmoveit_resources_pr2_description/depend dependsrdfdom/depend dependurdf/depend !-- RViz 可视化 -- dependrviz2/depend dependrviz_common/depend dependrviz_rendering/depend test_dependament_lint_auto/test_depend test_dependament_lint_common/test_depend export build_typeament_cmake/build_type /export /package这些依赖项不是随意添加的。moveit_core和moveit_ros_planning提供了规划算法和核心数据结构moveit_ros_planning_interface是我们主要编程接口它封装了复杂的底层调用提供了MoveGroupInterface这样易用的类moveit_visual_tools用于在RViz中发布可视化标记如轨迹、文本等对于调试至关重要moveit_resources_pr2_description则提供了PR2机器人的URDF和SRDF文件这是一个标准的测试机器人模型。3. 核心代码解析与编写环境就绪后我们来编写核心的C节点。这个节点的目标是初始化一个MoveGroup控制名为“panda_arm”的规划组这是MoveIt资源中PR2机器人模型的一部分规划一条从当前位姿到一个目标位姿的轨迹并可视化。3.1 编写主程序first_moveit_node.cpp在src/目录下创建文件first_moveit_node.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_visual_tools/moveit_visual_tools.h #include geometry_msgs/msg/pose.hpp int main(int argc, char* argv[]) { // 初始化ROS 2 rclcpp::init(argc, argv); auto node std::make_sharedrclcpp::Node(first_moveit_node); auto logger node-get_logger(); // 创建一个用于执行ROS 2消息收发的静态单线程执行器 // 注意MoveGroupInterface内部需要spin因此我们需要一个线程来执行它 rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(node); std::thread executor_thread([executor]() { executor.spin(); }); RCLCPP_INFO(logger, Starting first_moveit_node...); // 1. 初始化MoveGroupInterface // 参数“panda_arm”应对应于你机器人URDF/SRDF中定义的规划组名称 // 这里我们使用MoveIt资源包中的PR2模型其左臂规划组名为“left_arm” // 但为了通用性我们假设一个名为“panda_arm”的组。实际运行时需要匹配。 static const std::string PLANNING_GROUP panda_arm; moveit::planning_interface::MoveGroupInterface move_group(node, PLANNING_GROUP); // 2. 初始化MoveItVisualTools // 用于在RViz中发布可视化标记 auto visual_tools std::make_sharedmoveit_visual_tools::MoveItVisualTools( node, panda_link0, rviz_visual_tools::RVIZ_MARKER_TOPIC, move_group.getRobotModel()); visual_tools-deleteAllMarkers(); visual_tools-loadRemoteControl(); // 获取规划组的关节模型和末端执行器链接名称 const moveit::core::JointModelGroup* joint_model_group move_group.getCurrentState()-getJointModelGroup(PLANNING_GROUP); const std::string eef_link move_group.getEndEffectorLink(); RCLCPP_INFO_STREAM(logger, Planning frame: move_group.getPlanningFrame()); RCLCPP_INFO_STREAM(logger, End effector link: eef_link); RCLCPP_INFO_STREAM(logger, Available Planning Groups:); for (const std::string name : move_group.getJointModelGroupNames()) { RCLCPP_INFO_STREAM(logger, - name); } // 3. 设置一个目标位姿 geometry_msgs::msg::Pose target_pose; target_pose.orientation.w 1.0; // 四元数表示无旋转 target_pose.position.x 0.3; target_pose.position.y 0.0; target_pose.position.z 0.5; move_group.setPoseTarget(target_pose); // 在RViz中可视化目标位姿 visual_tools-publishAxisLabeled(target_pose, target_pose); visual_tools-publishText( visual_tools-getBaseFrame(), Goal Pose, rviz_visual_tools::WHITE, rviz_visual_tools::XLARGE); visual_tools-trigger(); // 4. 进行运动规划 moveit::planning_interface::MoveGroupInterface::Plan my_plan; bool success (move_group.plan(my_plan) moveit::core::MoveItErrorCode::SUCCESS); if (success) { RCLCPP_INFO(logger, Planning succeeded!); // 在RViz中可视化规划出的轨迹 visual_tools-publishTrajectoryLine( my_plan.trajectory, joint_model_group-getLinkModel(eef_link), joint_model_group); visual_tools-trigger(); RCLCPP_INFO(logger, Visualizing plan as trajectory line); // 注意这里我们只规划不执行。实际控制机器人需要取消下面一行的注释。 // move_group.execute(my_plan); } else { RCLCPP_ERROR(logger, Planning failed!); } // 5. 清理与关闭 visual_tools-deleteAllMarkers(); visual_tools-trigger(); RCLCPP_INFO(logger, Shutting down.); rclcpp::shutdown(); executor_thread.join(); return 0; }这段代码虽然不长但包含了与MoveIt2交互的核心流程。我来拆解几个关键点执行器Executor线程这是ROS 2编程的一个常见模式。MoveGroupInterface内部需要处理ROS话题和服务通信因此我们必须让节点spin起来。这里我们创建了一个单独的线程来运行执行器主线程则继续执行规划逻辑。这是一种简单有效的处理方式。规划组名称PLANNING_GROUP变量必须严格匹配你机器人配置中的规划组名称。我们这里使用了“panda_arm”但如果你加载的是PR2模型可能需要改为“left_arm”或“right_arm”。这个名称在机器人的SRDF语义机器人描述格式文件中定义。目标位姿设置我们通过geometry_msgs::msg::Pose设置了一个简单的目标位置(0.3, 0.0, 0.5)和单位四元数朝向。在实际应用中这个位姿通常来自于视觉系统或其他任务规划模块。规划与执行分离move_group.plan(my_plan)只进行轨迹规划将结果存储在my_plan中。move_group.execute(my_plan)才是真正将轨迹发送给控制器去执行。在调试阶段强烈建议先规划并可视化确认轨迹合理后再执行尤其是对真实机器人操作时。3.2 配置构建系统CMakeLists.txt代码写好了但要让ament_cmake正确编译它我们需要修改CMakeLists.txt。这是新手最容易出错的地方因为MoveIt2的库路径和依赖关系比较复杂。打开CMakeLists.txt将其内容替换为以下经过验证的版本cmake_minimum_required(VERSION 3.8) project(first_moveit_program) # Default to C17 if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) endif() # 查找所有必需的依赖包 find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2_geometry_msgs REQUIRED) # MoveIt2 核心包 find_package(moveit_core REQUIRED) find_package(moveit_ros_planning REQUIRED) find_package(moveit_ros_planning_interface REQUIRED) find_package(moveit_ros_move_group REQUIRED) find_package(moveit_visual_tools REQUIRED) # 机器人模型相关 find_package(moveit_resources_pr2_description REQUIRED) find_package(srdfdom REQUIRED) find_package(urdf REQUIRED) # RViz 相关 (moveit_visual_tools 依赖) find_package(rviz2 REQUIRED) find_package(rviz_common REQUIRED) find_package(rviz_rendering REQUIRED) # 定义可执行文件及其依赖 add_executable(first_moveit_node src/first_moveit_node.cpp) # 极其关键的一步正确链接所有库 # 链接顺序很重要一般遵循从具体到抽象避免未定义引用错误。 target_link_libraries(first_moveit_node # MoveIt 接口层 moveit_ros_planning_interface::moveit_ros_planning_interface moveit_ros_move_group::moveit_ros_move_group # MoveIt 核心与规划层 moveit_ros_planning::moveit_ros_planning moveit_core::moveit_core # 可视化工具 moveit_visual_tools::moveit_visual_tools # ROS 2 核心与消息 rclcpp::rclcpp geometry_msgs::geometry_msgs tf2_ros::tf2_ros tf2_geometry_msgs::tf2_geometry_msgs # RViz 相关通过moveit_visual_tools间接依赖但显式声明更安全 rviz2::rviz2 rviz_common::rviz_common rviz_rendering::rviz_rendering # 模型解析 urdf::urdf srdfdom::srdfdom ) # 添加包含目录 target_include_directories(first_moveit_node PUBLIC $BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include $INSTALL_INTERFACE:include ) # 安装目标 install(TARGETS first_moveit_node DESTINATION lib/${PROJECT_NAME}) # 导出依赖使其他包能找到本包 ament_export_dependencies( rclcpp geometry_msgs tf2_ros tf2_geometry_msgs moveit_core moveit_ros_planning moveit_ros_planning_interface moveit_ros_move_group moveit_visual_tools ) ament_package()这个CMakeLists.txt有几个需要强调的地方find_package必须找到所有在package.xml中声明的依赖。如果编译时报错找不到某个包首先检查这里是否find_package了。target_link_libraries链接顺序有时会导致编译错误。通常的顺序是先链接高级的、应用层的库如moveit_ros_planning_interface再链接底层的核心库如moveit_core最后是基础库如rclcpp。上述顺序在我多次实践中是稳定的。C标准MoveIt2 Humble 要求至少 C14我们设置为 C17 以确保兼容性和使用现代特性。4. 启动与调试实战代码和构建配置都准备好了现在进入最激动人心的环节编译并运行它。这个过程我们会遇到一些典型的配置问题我会带你一步步解决。4.1 编译工作空间回到工作空间的根目录 (~/moveit2_tutorial_ws)使用colcon进行编译。colcon是ROS 2的构建工具。cd ~/moveit2_tutorial_ws # 推荐使用 --symlink-install这样对python脚本的修改无需重新编译安装 colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPERelease编译过程可能会持续几分钟。如果一切顺利你会在最后看到类似“Summary: X packages finished [Y.Zs]”的成功信息。如果编译失败最常见的原因是依赖缺失错误信息通常包含“Could not find a package configuration file for ...”。请根据错误提示的包名使用sudo apt install ros-humble-包名安装。注意有些MoveIt2包的名称可能与find_package中的略有不同例如moveit_visual_tools对应的apt包是ros-humble-moveit-visual-tools。链接错误如“undefined reference to ...”。这几乎总是CMakeLists.txt中target_link_libraries的顺序或遗漏导致的。请严格按照上一节提供的顺序和列表进行核对。C语法错误检查你的first_moveit_node.cpp是否有拼写错误或缺少分号。4.2 配置Launch文件并运行我们的节点需要在一个包含机器人模型和MoveIt2配置的环境中运行。最方便的方式是写一个launch文件。在功能包根目录下创建launch/文件夹并在其中创建demo.launch.py文件。import os from launch import LaunchDescription from launch.actions import IncludeLaunchDescription, ExecuteProcess, RegisterEventHandler from launch.event_handlers import OnProcessExit from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import PathJoinSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): # 1. 启动MoveIt2的演示启动文件它会加载PR2机器人并启动MoveGroup和RViz moveit_demo_launch IncludeLaunchDescription( PythonLaunchDescriptionSource([ PathJoinSubstitution([ FindPackageShare(moveit_resources_pr2_description), launch, demo.launch.py ]) ]) ) # 2. 启动我们自己的节点 first_moveit_node Node( packagefirst_moveit_program, executablefirst_moveit_node, outputscreen, namefirst_moveit_node, ) # 3. 可选添加一个事件处理当我们的节点退出后关闭整个启动进程 exit_event_handler RegisterEventHandler( event_handlerOnProcessExit( target_actionfirst_moveit_node, on_exit[LaunchDescription.actions.LogInfo(msgFirst MoveIt node exited. Shutting down.), LaunchDescription.actions.EmitEvent(eventLaunchDescription.events.Shutdown())] ) ) return LaunchDescription([ moveit_demo_launch, first_moveit_node, exit_event_handler, ])这个launch文件做了两件核心事情它通过IncludeLaunchDescription启动了MoveIt2官方提供的PR2演示launch文件。这个文件会做大量繁重的工作启动ROS 2核心、加载PR2机器人URDF/SRDF模型、启动move_group节点、启动RViz并加载一个预配置好的视图里面已经设置好了MotionPlanning插件。它启动了我们刚刚编译好的first_moveit_node节点。现在在一个终端中source你的工作空间并运行这个launch文件# 终端1 cd ~/moveit2_tutorial_ws source install/setup.bash ros2 launch first_moveit_program demo.launch.py如果一切正常你会看到一系列启动日志最后RViz窗口会弹出里面显示一个PR2机器人的模型。同时在终端里你应该能看到我们节点打印的日志信息例如“Planning frame: world”, “End effector link: l_gripper_tool_frame” (对于PR2左臂)以及“Planning succeeded!”。在RViz中你应该能看到机器人模型。一个由红色、绿色、蓝色箭头组成的坐标系标记在我们设置的目标位姿(0.3, 0, 0.5)处。一条从机器人当前位姿连接到目标位姿的彩色轨迹线通常是渐变的彩虹色。实操心得第一次运行时RViz加载和模型解析可能需要十几秒甚至更久请耐心等待。如果RViz卡住或者模型显示不全可以尝试在RViz中点击“File”-“Open Config”然后导航到/opt/ros/humble/share/moveit_resources_pr2_description/launch目录或类似路径加载里面附带的moveit.rviz配置文件。这能确保所有必要的显示插件被正确加载。4.3 关键调试技巧与问题排查即使按照步骤操作你也可能会遇到一些问题。这里我总结了一份常见问题速查表涵盖了从编译到运行的大部分坑。问题现象可能原因排查与解决方案编译错误找不到MoveIt相关包1. MoveIt2未安装或安装不完整。2.package.xml依赖声明错误。1. 运行dpkg -l | grep ros-humble-moveit检查安装。用sudo apt install --reinstall ros-humble-moveit重装。2. 核对package.xml中的depend标签确保包名正确如moveit_ros_planning_interface。链接错误undefined referenceCMakeLists.txt中target_link_libraries顺序错误或漏库。1. 严格按照本文3.2节的顺序链接库。2. 检查编译错误信息中缺失的符号属于哪个库将其添加到链接列表。运行节点时报错Group ‘panda_arm’ not found规划组名称不匹配。启动的演示环境PR2中不存在名为“panda_arm”的组。修改first_moveit_node.cpp中的PLANNING_GROUP变量。对于PR2左臂改为left_arm对于右臂改为right_arm。然后重新编译运行。RViz中看不到机器人模型或模型是白色网格RViz未能正确加载机器人描述URDF。1. 确保demo.launch.py正确启动了MoveIt2演示该演示会发布/robot_description话题。2. 在RViz中检查“Displays”面板确保“RobotModel”显示已添加并且“Description Topic”是/robot_description。规划失败返回错误码-1或INVALID_MOTION_PLAN1. 目标位姿超出机器人工作空间或不可达。2. 起始状态机器人当前状态异常。1. 调整target_pose的位置和姿态使其在机器人物理可达范围内。2. 在RViz的MotionPlanning插件中点击“Query”-“Start State”-“Use Current”将当前状态设为规划起点。或者在代码中调用move_group.setStartStateToCurrentState()。终端提示规划成功但RViz中看不到轨迹线moveit_visual_tools发布的可视化标记未正确显示。1. 在RViz的“Displays”面板中确保添加了“Marker”显示并且其“Marker Topic”是/rviz_visual_tools这是MoveItVisualTools的默认话题。2. 检查代码中visual_tools-trigger()是否在发布标记后被调用。启动launch文件后节点立刻退出可能因为executor.spin()在独立线程中异常退出或节点构造失败。1. 在first_moveit_node.cpp的main函数开始和结束处添加打印确认执行流程。2. 检查节点构造函数MoveGroupInterface初始化是否因参数错误如错误的规划组名而抛出异常。可以尝试用try-catch包裹初始化代码来捕获异常信息。一个重要的调试习惯在开发过程中不要只依赖自己节点的输出。多开几个终端使用ROS 2的命令行工具观察系统状态ros2 node list查看所有运行的节点确认你的节点是否在其中。ros2 topic list查看活跃的话题确认/planning_scene,/move_group/status等MoveIt2核心话题是否存在。ros2 topic echo /move_group/status实时查看MoveGroup的状态信息这在规划失败时非常有用。5. 从规划到执行让机械臂动起来到目前为止我们的程序只完成了“规划”和“可视化”。对于在Gazebo仿真或真实机器人上运行我们需要迈出最后一步执行。同时我们也需要理解如何与更复杂的场景交互比如设置多个路点、处理碰撞检测。5.1 执行规划轨迹让代码控制机械臂真正运动起来只需要在规划成功后取消对执行代码的注释。修改first_moveit_node.cpp中规划成功后的部分if (success) { RCLCPP_INFO(logger, Planning succeeded!); visual_tools-publishTrajectoryLine(my_plan.trajectory, ...); visual_tools-trigger(); RCLCPP_INFO(logger, Visualizing plan as trajectory line); // *** 关键修改添加执行代码 *** RCLCPP_INFO(logger, Executing the plan...); move_group.execute(my_plan); RCLCPP_INFO(logger, Execution completed.); }重要警告如果你连接的是真实机器人在执行前请务必确保机器人处于“可服务”状态伺服已打开无报警。机器人的当前位置与RViz中显示的“当前状态”一致。严重的不一致会导致剧烈运动非常危险。最好先在“慢速”或“模拟”模式下测试。确保机器人的工作空间内没有障碍物或者你已正确配置了碰撞检测。对于Gazebo仿真执行是相对安全的。你需要确保已经启动了包含对应机器人控制器的仿真环境。对于PR2MoveIt2演示launch文件通常已经启动了必要的控制器管理器。5.2 规划到关节空间目标除了指定末端执行器的位姿笛卡尔空间规划MoveIt2也支持直接指定关节角度目标关节空间规划。这种方式计算更快路径可能更直接。// 获取机器人当前状态 moveit::core::RobotStatePtr current_state move_group.getCurrentState(); // 获取规划组对应的关节名称向量 std::vectorstd::string joint_names joint_model_group-getVariableNames(); // 创建一个关节角度目标向量 std::vectordouble joint_group_positions; current_state-copyJointGroupPositions(joint_model_group, joint_group_positions); // 修改其中一些关节的角度例如第一个关节旋转90度 joint_group_positions[0] 1.5708; // 弧度制约90度 // 设置关节空间目标 move_group.setJointValueTarget(joint_group_positions); // 进行规划后续步骤与位姿目标相同 bool joint_success (move_group.plan(my_plan) moveit::core::MoveItErrorCode::SUCCESS);关节空间规划避免了复杂的逆运动学求解特别适用于你知道机器人具体构型目标的场景。5.3 添加碰撞物体与场景交互一个实用的机器人程序必须能感知环境。MoveIt2的PlanningSceneInterface允许我们在规划场景中添加、移除和更新碰撞物体。在first_moveit_node.cpp的main函数中初始化部分之后添加// 初始化PlanningSceneInterface moveit::planning_interface::PlanningSceneInterface planning_scene_interface; // 定义一个立方体碰撞物体 moveit_msgs::msg::CollisionObject collision_object; collision_object.header.frame_id move_group.getPlanningFrame(); collision_object.id box1; // 定义物体的形状一个边长为0.1米的立方体 shape_msgs::msg::SolidPrimitive primitive; primitive.type primitive.BOX; primitive.dimensions.resize(3); primitive.dimensions[0] 0.1; // x primitive.dimensions[1] 0.1; // y primitive.dimensions[2] 0.1; // z // 定义物体的位姿放在机器人前方 geometry_msgs::msg::Pose box_pose; box_pose.orientation.w 1.0; box_pose.position.x 0.25; box_pose.position.y 0.0; box_pose.position.z 0.25; collision_object.primitives.push_back(primitive); collision_object.primitive_poses.push_back(box_pose); collision_object.operation collision_object.ADD; // 将物体添加到场景中 std::vectormoveit_msgs::msg::CollisionObject collision_objects; collision_objects.push_back(collision_object); planning_scene_interface.addCollisionObjects(collision_objects); RCLCPP_INFO(logger, Added a collision object into the world); visual_tools-publishCollisionBlock(box_pose, primitive.dimensions, rviz_visual_tools::RED); visual_tools-trigger();添加碰撞物体后MoveIt2的规划器会自动避开它。你可以尝试将之前的目标位姿设置在这个盒子内部或紧贴其表面规划很可能会失败因为路径被阻挡了。这演示了MoveIt2核心的碰撞避免功能。通过以上步骤你已经完成了一个具备基本规划、可视化、执行和环境交互功能的MoveIt2 C程序。这个程序虽然基础但已经包含了MoveIt2应用开发的骨架。你可以在此基础上结合具体的机器人模型替换掉PR2、传感器信息如摄像头点云和任务逻辑开发出更复杂的机器人应用比如抓取、装配或移动操作。记住理解每个API调用背后的含义是阻塞还是非阻塞是同步还是异步以及规划场景、机器人状态这些核心概念是高效使用MoveIt2的关键。
ROS 2 Humble下MoveIt2 C++应用开发实战：从环境配置到轨迹规划
发布时间：2026/7/16 5:07:36
1. 项目概述从零构建你的第一个MoveIt2 C应用如果你正在ROS 2 Humble环境下看着MoveIt2的官方文档感到无从下手想写一个能控制机械臂真实动起来的程序却不知从何编译起那么这篇内容就是为你准备的。我花了相当一段时间在Humble版本上从配置环境、理解CMakeLists.txt的坑到调试出第一个能让仿真机械臂在RViz里画出一条平滑轨迹的程序把整个过程里最关键、最易卡住的环节都梳理了出来。这不是对官方教程的简单翻译而是一个实战派开发者踩过坑、验证过的“生存指南”。我们将聚焦于创建一个最精简、但五脏俱全的C节点它能够调用MoveIt2的API规划一条机械臂末端执行器比如夹爪从点A到点B的运动轨迹并能在RViz中可视化。无论你是机器人方向的学生还是刚转入ROS 2生态的工程师这篇内容都将帮你跨过“从看懂到跑通”的第一道门槛。2. 环境准备与工作空间剖析在开始敲代码之前一个正确配置的ROS 2 Humble环境和清晰的项目结构能避免后续至少80%的编译和依赖问题。很多人卡在第一步就是因为环境没装对或者工作空间没理清。2.1 ROS 2 Humble与MoveIt2安装要点官方安装指南通常会让你apt install一大堆元包但对于开发而言我更推荐精准安装保持系统简洁。首先确保你的ROS 2 Humble是从二进制包安装且来源正确。打开终端依次输入以下命令进行验证和安装# 1. 检查ROS 2环境是否已正确source echo $ROS_DISTRO # 应该输出 ‘humble‘如果没有需要手动source source /opt/ros/humble/setup.bash # 2. 安装MoveIt2核心包和教程依赖这是最小集足够我们入门使用 sudo apt update sudo apt install ros-humble-moveit sudo apt install ros-humble-moveit-resources-pr2-description # 用于获取测试用的机器人模型 sudo apt install ros-humble-rviz2 ros-humble-moveit-visual-tools ros-humble-xacro注意ros-humble-moveit这个包是一个元包metapackage它本身不包含太多代码但会拉取一系列MoveIt2运行所需的依赖包如moveit_core,moveit_ros_planning等。安装时请确保网络通畅因为包体积较大。安装完成后一个快速的验证方法是尝试运行MoveIt2的演示配置助手Setup Assistant虽然我们本次不用它但能运行说明基础安装成功ros2 launch moveit_setup_assistant setup_assistant.launch.py如果能看到GUI界面弹出说明MoveIt2框架安装基本没问题可以关掉它继续下一步。2.2 创建专属的工作空间与功能包我强烈建议为这个教程单独创建一个工作空间与你的其他项目隔离避免依赖冲突。假设我们的工作空间叫moveit2_tutorial_ws。# 创建并进入工作空间目录 mkdir -p ~/moveit2_tutorial_ws/src cd ~/moveit2_tutorial_ws/src接下来创建我们的功能包。这里有个关键点MoveIt2功能包的CMakeLists.txt和package.xml与普通ROS 2包有显著不同因为它需要链接一系列复杂的MoveIt库。我们使用ament_cmake构建类型并手动声明依赖。# 创建功能包名字就叫 first_moveit_program ros2 pkg create first_moveit_program \ --build-type ament_cmake \ --dependencies rclcpp moveit_core moveit_ros_planning_interface tf2_ros tf2_geometry_msgs创建完成后进入包目录查看结构cd first_moveit_program ls你应该会看到CMakeLists.txt,package.xml,include/和src/目录。现在打开package.xml文件我们需要对其进行关键性修改。默认生成的依赖可能不全我们需要确保它包含MoveIt规划、执行、可视化以及机器人模型解析所需的所有组件。用文本编辑器如nano或vscode打开package.xml在depend标签区域进行补充修改后关键部分如下?xml version1.0? ?xml-model hrefhttp://download.ros.org/schema/package_format3.xsd schematypenshttp://www.w3.org/2001/XMLSchema? package format3 namefirst_moveit_program/name version0.0.0/version descriptionMy first MoveIt2 C program/description maintainer emailyouexample.comYour Name/maintainer licenseApache License 2.0/license !-- 关键的依赖声明 -- dependrclcpp/depend dependgeometry_msgs/depend dependtf2_ros/depend dependtf2_geometry_msgs/depend !-- MoveIt2 核心依赖 -- dependmoveit_core/depend dependmoveit_ros_planning/depend dependmoveit_ros_planning_interface/depend dependmoveit_ros_move_group/depend dependmoveit_visual_tools/depend !-- 用于加载机器人模型 -- dependmoveit_resources_pr2_description/depend dependsrdfdom/depend dependurdf/depend !-- RViz 可视化 -- dependrviz2/depend dependrviz_common/depend dependrviz_rendering/depend test_dependament_lint_auto/test_depend test_dependament_lint_common/test_depend export build_typeament_cmake/build_type /export /package这些依赖项不是随意添加的。moveit_core和moveit_ros_planning提供了规划算法和核心数据结构moveit_ros_planning_interface是我们主要编程接口它封装了复杂的底层调用提供了MoveGroupInterface这样易用的类moveit_visual_tools用于在RViz中发布可视化标记如轨迹、文本等对于调试至关重要moveit_resources_pr2_description则提供了PR2机器人的URDF和SRDF文件这是一个标准的测试机器人模型。3. 核心代码解析与编写环境就绪后我们来编写核心的C节点。这个节点的目标是初始化一个MoveGroup控制名为“panda_arm”的规划组这是MoveIt资源中PR2机器人模型的一部分规划一条从当前位姿到一个目标位姿的轨迹并可视化。3.1 编写主程序first_moveit_node.cpp在src/目录下创建文件first_moveit_node.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_visual_tools/moveit_visual_tools.h #include geometry_msgs/msg/pose.hpp int main(int argc, char* argv[]) { // 初始化ROS 2 rclcpp::init(argc, argv); auto node std::make_sharedrclcpp::Node(first_moveit_node); auto logger node-get_logger(); // 创建一个用于执行ROS 2消息收发的静态单线程执行器 // 注意MoveGroupInterface内部需要spin因此我们需要一个线程来执行它 rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(node); std::thread executor_thread([executor]() { executor.spin(); }); RCLCPP_INFO(logger, Starting first_moveit_node...); // 1. 初始化MoveGroupInterface // 参数“panda_arm”应对应于你机器人URDF/SRDF中定义的规划组名称 // 这里我们使用MoveIt资源包中的PR2模型其左臂规划组名为“left_arm” // 但为了通用性我们假设一个名为“panda_arm”的组。实际运行时需要匹配。 static const std::string PLANNING_GROUP panda_arm; moveit::planning_interface::MoveGroupInterface move_group(node, PLANNING_GROUP); // 2. 初始化MoveItVisualTools // 用于在RViz中发布可视化标记 auto visual_tools std::make_sharedmoveit_visual_tools::MoveItVisualTools( node, panda_link0, rviz_visual_tools::RVIZ_MARKER_TOPIC, move_group.getRobotModel()); visual_tools-deleteAllMarkers(); visual_tools-loadRemoteControl(); // 获取规划组的关节模型和末端执行器链接名称 const moveit::core::JointModelGroup* joint_model_group move_group.getCurrentState()-getJointModelGroup(PLANNING_GROUP); const std::string eef_link move_group.getEndEffectorLink(); RCLCPP_INFO_STREAM(logger, Planning frame: move_group.getPlanningFrame()); RCLCPP_INFO_STREAM(logger, End effector link: eef_link); RCLCPP_INFO_STREAM(logger, Available Planning Groups:); for (const std::string name : move_group.getJointModelGroupNames()) { RCLCPP_INFO_STREAM(logger, - name); } // 3. 设置一个目标位姿 geometry_msgs::msg::Pose target_pose; target_pose.orientation.w 1.0; // 四元数表示无旋转 target_pose.position.x 0.3; target_pose.position.y 0.0; target_pose.position.z 0.5; move_group.setPoseTarget(target_pose); // 在RViz中可视化目标位姿 visual_tools-publishAxisLabeled(target_pose, target_pose); visual_tools-publishText( visual_tools-getBaseFrame(), Goal Pose, rviz_visual_tools::WHITE, rviz_visual_tools::XLARGE); visual_tools-trigger(); // 4. 进行运动规划 moveit::planning_interface::MoveGroupInterface::Plan my_plan; bool success (move_group.plan(my_plan) moveit::core::MoveItErrorCode::SUCCESS); if (success) { RCLCPP_INFO(logger, Planning succeeded!); // 在RViz中可视化规划出的轨迹 visual_tools-publishTrajectoryLine( my_plan.trajectory, joint_model_group-getLinkModel(eef_link), joint_model_group); visual_tools-trigger(); RCLCPP_INFO(logger, Visualizing plan as trajectory line); // 注意这里我们只规划不执行。实际控制机器人需要取消下面一行的注释。 // move_group.execute(my_plan); } else { RCLCPP_ERROR(logger, Planning failed!); } // 5. 清理与关闭 visual_tools-deleteAllMarkers(); visual_tools-trigger(); RCLCPP_INFO(logger, Shutting down.); rclcpp::shutdown(); executor_thread.join(); return 0; }这段代码虽然不长但包含了与MoveIt2交互的核心流程。我来拆解几个关键点执行器Executor线程这是ROS 2编程的一个常见模式。MoveGroupInterface内部需要处理ROS话题和服务通信因此我们必须让节点spin起来。这里我们创建了一个单独的线程来运行执行器主线程则继续执行规划逻辑。这是一种简单有效的处理方式。规划组名称PLANNING_GROUP变量必须严格匹配你机器人配置中的规划组名称。我们这里使用了“panda_arm”但如果你加载的是PR2模型可能需要改为“left_arm”或“right_arm”。这个名称在机器人的SRDF语义机器人描述格式文件中定义。目标位姿设置我们通过geometry_msgs::msg::Pose设置了一个简单的目标位置(0.3, 0.0, 0.5)和单位四元数朝向。在实际应用中这个位姿通常来自于视觉系统或其他任务规划模块。规划与执行分离move_group.plan(my_plan)只进行轨迹规划将结果存储在my_plan中。move_group.execute(my_plan)才是真正将轨迹发送给控制器去执行。在调试阶段强烈建议先规划并可视化确认轨迹合理后再执行尤其是对真实机器人操作时。3.2 配置构建系统CMakeLists.txt代码写好了但要让ament_cmake正确编译它我们需要修改CMakeLists.txt。这是新手最容易出错的地方因为MoveIt2的库路径和依赖关系比较复杂。打开CMakeLists.txt将其内容替换为以下经过验证的版本cmake_minimum_required(VERSION 3.8) project(first_moveit_program) # Default to C17 if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) endif() # 查找所有必需的依赖包 find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2_geometry_msgs REQUIRED) # MoveIt2 核心包 find_package(moveit_core REQUIRED) find_package(moveit_ros_planning REQUIRED) find_package(moveit_ros_planning_interface REQUIRED) find_package(moveit_ros_move_group REQUIRED) find_package(moveit_visual_tools REQUIRED) # 机器人模型相关 find_package(moveit_resources_pr2_description REQUIRED) find_package(srdfdom REQUIRED) find_package(urdf REQUIRED) # RViz 相关 (moveit_visual_tools 依赖) find_package(rviz2 REQUIRED) find_package(rviz_common REQUIRED) find_package(rviz_rendering REQUIRED) # 定义可执行文件及其依赖 add_executable(first_moveit_node src/first_moveit_node.cpp) # 极其关键的一步正确链接所有库 # 链接顺序很重要一般遵循从具体到抽象避免未定义引用错误。 target_link_libraries(first_moveit_node # MoveIt 接口层 moveit_ros_planning_interface::moveit_ros_planning_interface moveit_ros_move_group::moveit_ros_move_group # MoveIt 核心与规划层 moveit_ros_planning::moveit_ros_planning moveit_core::moveit_core # 可视化工具 moveit_visual_tools::moveit_visual_tools # ROS 2 核心与消息 rclcpp::rclcpp geometry_msgs::geometry_msgs tf2_ros::tf2_ros tf2_geometry_msgs::tf2_geometry_msgs # RViz 相关通过moveit_visual_tools间接依赖但显式声明更安全 rviz2::rviz2 rviz_common::rviz_common rviz_rendering::rviz_rendering # 模型解析 urdf::urdf srdfdom::srdfdom ) # 添加包含目录 target_include_directories(first_moveit_node PUBLIC $BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include $INSTALL_INTERFACE:include ) # 安装目标 install(TARGETS first_moveit_node DESTINATION lib/${PROJECT_NAME}) # 导出依赖使其他包能找到本包 ament_export_dependencies( rclcpp geometry_msgs tf2_ros tf2_geometry_msgs moveit_core moveit_ros_planning moveit_ros_planning_interface moveit_ros_move_group moveit_visual_tools ) ament_package()这个CMakeLists.txt有几个需要强调的地方find_package必须找到所有在package.xml中声明的依赖。如果编译时报错找不到某个包首先检查这里是否find_package了。target_link_libraries链接顺序有时会导致编译错误。通常的顺序是先链接高级的、应用层的库如moveit_ros_planning_interface再链接底层的核心库如moveit_core最后是基础库如rclcpp。上述顺序在我多次实践中是稳定的。C标准MoveIt2 Humble 要求至少 C14我们设置为 C17 以确保兼容性和使用现代特性。4. 启动与调试实战代码和构建配置都准备好了现在进入最激动人心的环节编译并运行它。这个过程我们会遇到一些典型的配置问题我会带你一步步解决。4.1 编译工作空间回到工作空间的根目录 (~/moveit2_tutorial_ws)使用colcon进行编译。colcon是ROS 2的构建工具。cd ~/moveit2_tutorial_ws # 推荐使用 --symlink-install这样对python脚本的修改无需重新编译安装 colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPERelease编译过程可能会持续几分钟。如果一切顺利你会在最后看到类似“Summary: X packages finished [Y.Zs]”的成功信息。如果编译失败最常见的原因是依赖缺失错误信息通常包含“Could not find a package configuration file for ...”。请根据错误提示的包名使用sudo apt install ros-humble-包名安装。注意有些MoveIt2包的名称可能与find_package中的略有不同例如moveit_visual_tools对应的apt包是ros-humble-moveit-visual-tools。链接错误如“undefined reference to ...”。这几乎总是CMakeLists.txt中target_link_libraries的顺序或遗漏导致的。请严格按照上一节提供的顺序和列表进行核对。C语法错误检查你的first_moveit_node.cpp是否有拼写错误或缺少分号。4.2 配置Launch文件并运行我们的节点需要在一个包含机器人模型和MoveIt2配置的环境中运行。最方便的方式是写一个launch文件。在功能包根目录下创建launch/文件夹并在其中创建demo.launch.py文件。import os from launch import LaunchDescription from launch.actions import IncludeLaunchDescription, ExecuteProcess, RegisterEventHandler from launch.event_handlers import OnProcessExit from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import PathJoinSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare def generate_launch_description(): # 1. 启动MoveIt2的演示启动文件它会加载PR2机器人并启动MoveGroup和RViz moveit_demo_launch IncludeLaunchDescription( PythonLaunchDescriptionSource([ PathJoinSubstitution([ FindPackageShare(moveit_resources_pr2_description), launch, demo.launch.py ]) ]) ) # 2. 启动我们自己的节点 first_moveit_node Node( packagefirst_moveit_program, executablefirst_moveit_node, outputscreen, namefirst_moveit_node, ) # 3. 可选添加一个事件处理当我们的节点退出后关闭整个启动进程 exit_event_handler RegisterEventHandler( event_handlerOnProcessExit( target_actionfirst_moveit_node, on_exit[LaunchDescription.actions.LogInfo(msgFirst MoveIt node exited. Shutting down.), LaunchDescription.actions.EmitEvent(eventLaunchDescription.events.Shutdown())] ) ) return LaunchDescription([ moveit_demo_launch, first_moveit_node, exit_event_handler, ])这个launch文件做了两件核心事情它通过IncludeLaunchDescription启动了MoveIt2官方提供的PR2演示launch文件。这个文件会做大量繁重的工作启动ROS 2核心、加载PR2机器人URDF/SRDF模型、启动move_group节点、启动RViz并加载一个预配置好的视图里面已经设置好了MotionPlanning插件。它启动了我们刚刚编译好的first_moveit_node节点。现在在一个终端中source你的工作空间并运行这个launch文件# 终端1 cd ~/moveit2_tutorial_ws source install/setup.bash ros2 launch first_moveit_program demo.launch.py如果一切正常你会看到一系列启动日志最后RViz窗口会弹出里面显示一个PR2机器人的模型。同时在终端里你应该能看到我们节点打印的日志信息例如“Planning frame: world”, “End effector link: l_gripper_tool_frame” (对于PR2左臂)以及“Planning succeeded!”。在RViz中你应该能看到机器人模型。一个由红色、绿色、蓝色箭头组成的坐标系标记在我们设置的目标位姿(0.3, 0, 0.5)处。一条从机器人当前位姿连接到目标位姿的彩色轨迹线通常是渐变的彩虹色。实操心得第一次运行时RViz加载和模型解析可能需要十几秒甚至更久请耐心等待。如果RViz卡住或者模型显示不全可以尝试在RViz中点击“File”-“Open Config”然后导航到/opt/ros/humble/share/moveit_resources_pr2_description/launch目录或类似路径加载里面附带的moveit.rviz配置文件。这能确保所有必要的显示插件被正确加载。4.3 关键调试技巧与问题排查即使按照步骤操作你也可能会遇到一些问题。这里我总结了一份常见问题速查表涵盖了从编译到运行的大部分坑。问题现象可能原因排查与解决方案编译错误找不到MoveIt相关包1. MoveIt2未安装或安装不完整。2.package.xml依赖声明错误。1. 运行dpkg -l | grep ros-humble-moveit检查安装。用sudo apt install --reinstall ros-humble-moveit重装。2. 核对package.xml中的depend标签确保包名正确如moveit_ros_planning_interface。链接错误undefined referenceCMakeLists.txt中target_link_libraries顺序错误或漏库。1. 严格按照本文3.2节的顺序链接库。2. 检查编译错误信息中缺失的符号属于哪个库将其添加到链接列表。运行节点时报错Group ‘panda_arm’ not found规划组名称不匹配。启动的演示环境PR2中不存在名为“panda_arm”的组。修改first_moveit_node.cpp中的PLANNING_GROUP变量。对于PR2左臂改为left_arm对于右臂改为right_arm。然后重新编译运行。RViz中看不到机器人模型或模型是白色网格RViz未能正确加载机器人描述URDF。1. 确保demo.launch.py正确启动了MoveIt2演示该演示会发布/robot_description话题。2. 在RViz中检查“Displays”面板确保“RobotModel”显示已添加并且“Description Topic”是/robot_description。规划失败返回错误码-1或INVALID_MOTION_PLAN1. 目标位姿超出机器人工作空间或不可达。2. 起始状态机器人当前状态异常。1. 调整target_pose的位置和姿态使其在机器人物理可达范围内。2. 在RViz的MotionPlanning插件中点击“Query”-“Start State”-“Use Current”将当前状态设为规划起点。或者在代码中调用move_group.setStartStateToCurrentState()。终端提示规划成功但RViz中看不到轨迹线moveit_visual_tools发布的可视化标记未正确显示。1. 在RViz的“Displays”面板中确保添加了“Marker”显示并且其“Marker Topic”是/rviz_visual_tools这是MoveItVisualTools的默认话题。2. 检查代码中visual_tools-trigger()是否在发布标记后被调用。启动launch文件后节点立刻退出可能因为executor.spin()在独立线程中异常退出或节点构造失败。1. 在first_moveit_node.cpp的main函数开始和结束处添加打印确认执行流程。2. 检查节点构造函数MoveGroupInterface初始化是否因参数错误如错误的规划组名而抛出异常。可以尝试用try-catch包裹初始化代码来捕获异常信息。一个重要的调试习惯在开发过程中不要只依赖自己节点的输出。多开几个终端使用ROS 2的命令行工具观察系统状态ros2 node list查看所有运行的节点确认你的节点是否在其中。ros2 topic list查看活跃的话题确认/planning_scene,/move_group/status等MoveIt2核心话题是否存在。ros2 topic echo /move_group/status实时查看MoveGroup的状态信息这在规划失败时非常有用。5. 从规划到执行让机械臂动起来到目前为止我们的程序只完成了“规划”和“可视化”。对于在Gazebo仿真或真实机器人上运行我们需要迈出最后一步执行。同时我们也需要理解如何与更复杂的场景交互比如设置多个路点、处理碰撞检测。5.1 执行规划轨迹让代码控制机械臂真正运动起来只需要在规划成功后取消对执行代码的注释。修改first_moveit_node.cpp中规划成功后的部分if (success) { RCLCPP_INFO(logger, Planning succeeded!); visual_tools-publishTrajectoryLine(my_plan.trajectory, ...); visual_tools-trigger(); RCLCPP_INFO(logger, Visualizing plan as trajectory line); // *** 关键修改添加执行代码 *** RCLCPP_INFO(logger, Executing the plan...); move_group.execute(my_plan); RCLCPP_INFO(logger, Execution completed.); }重要警告如果你连接的是真实机器人在执行前请务必确保机器人处于“可服务”状态伺服已打开无报警。机器人的当前位置与RViz中显示的“当前状态”一致。严重的不一致会导致剧烈运动非常危险。最好先在“慢速”或“模拟”模式下测试。确保机器人的工作空间内没有障碍物或者你已正确配置了碰撞检测。对于Gazebo仿真执行是相对安全的。你需要确保已经启动了包含对应机器人控制器的仿真环境。对于PR2MoveIt2演示launch文件通常已经启动了必要的控制器管理器。5.2 规划到关节空间目标除了指定末端执行器的位姿笛卡尔空间规划MoveIt2也支持直接指定关节角度目标关节空间规划。这种方式计算更快路径可能更直接。// 获取机器人当前状态 moveit::core::RobotStatePtr current_state move_group.getCurrentState(); // 获取规划组对应的关节名称向量 std::vectorstd::string joint_names joint_model_group-getVariableNames(); // 创建一个关节角度目标向量 std::vectordouble joint_group_positions; current_state-copyJointGroupPositions(joint_model_group, joint_group_positions); // 修改其中一些关节的角度例如第一个关节旋转90度 joint_group_positions[0] 1.5708; // 弧度制约90度 // 设置关节空间目标 move_group.setJointValueTarget(joint_group_positions); // 进行规划后续步骤与位姿目标相同 bool joint_success (move_group.plan(my_plan) moveit::core::MoveItErrorCode::SUCCESS);关节空间规划避免了复杂的逆运动学求解特别适用于你知道机器人具体构型目标的场景。5.3 添加碰撞物体与场景交互一个实用的机器人程序必须能感知环境。MoveIt2的PlanningSceneInterface允许我们在规划场景中添加、移除和更新碰撞物体。在first_moveit_node.cpp的main函数中初始化部分之后添加// 初始化PlanningSceneInterface moveit::planning_interface::PlanningSceneInterface planning_scene_interface; // 定义一个立方体碰撞物体 moveit_msgs::msg::CollisionObject collision_object; collision_object.header.frame_id move_group.getPlanningFrame(); collision_object.id box1; // 定义物体的形状一个边长为0.1米的立方体 shape_msgs::msg::SolidPrimitive primitive; primitive.type primitive.BOX; primitive.dimensions.resize(3); primitive.dimensions[0] 0.1; // x primitive.dimensions[1] 0.1; // y primitive.dimensions[2] 0.1; // z // 定义物体的位姿放在机器人前方 geometry_msgs::msg::Pose box_pose; box_pose.orientation.w 1.0; box_pose.position.x 0.25; box_pose.position.y 0.0; box_pose.position.z 0.25; collision_object.primitives.push_back(primitive); collision_object.primitive_poses.push_back(box_pose); collision_object.operation collision_object.ADD; // 将物体添加到场景中 std::vectormoveit_msgs::msg::CollisionObject collision_objects; collision_objects.push_back(collision_object); planning_scene_interface.addCollisionObjects(collision_objects); RCLCPP_INFO(logger, Added a collision object into the world); visual_tools-publishCollisionBlock(box_pose, primitive.dimensions, rviz_visual_tools::RED); visual_tools-trigger();添加碰撞物体后MoveIt2的规划器会自动避开它。你可以尝试将之前的目标位姿设置在这个盒子内部或紧贴其表面规划很可能会失败因为路径被阻挡了。这演示了MoveIt2核心的碰撞避免功能。通过以上步骤你已经完成了一个具备基本规划、可视化、执行和环境交互功能的MoveIt2 C程序。这个程序虽然基础但已经包含了MoveIt2应用开发的骨架。你可以在此基础上结合具体的机器人模型替换掉PR2、传感器信息如摄像头点云和任务逻辑开发出更复杂的机器人应用比如抓取、装配或移动操作。记住理解每个API调用背后的含义是阻塞还是非阻塞是同步还是异步以及规划场景、机器人状态这些核心概念是高效使用MoveIt2的关键。
