从ROS1切到ROS2再顺手把MoveIt换成MoveIt2这个过程中我花掉的时间比预期多了差不多一倍。最典型的一个场景就是笛卡尔轨迹规划明明在MoveIt1里写得好好的MoveGroupInterface接口到了ROS2不光头文件路径变了连消息类型都从geometry_msgs::Pose变成了geometry_msgs::msg::Pose编译直接给你一大堆红色报错。后来我索性换了个思路用Panda机械臂把整套流程从零完完整整过了一遍环境搭建、笛卡尔路径规划、添加障碍物避障、轨迹执行每一步都跑通并留下了代码。这篇文章就是那条已经趟平的路目标很直接让你在Ubuntu 22.04 ROS2 Humble环境下用MoveIt2驱动Panda机械臂按笛卡尔waypoint运动同时学会正确地把障碍物“塞”进规划场景里做出一条真正能避开障碍的轨迹。1. 为什么是Panda MoveIt2笛卡尔避障选型时的几个现实问题1.1 MoveIt2和MoveIt1的差距不在名字而在底层很多从ROS1迁移过来的人第一反应是“MoveIt2就是换个ROS2接口”实际上远不止如此。MoveIt2在Humble中的版本已经是2.5.x接口全面转向ROS2风格节点用rclcpp::Node::SharedPtr传递消息类型全部带上msg命名空间参数系统从ROS1的param server换成了ROS2的参数接口Action从actionlib换成了rclcpp_action。这些变化让老的MoveIt1代码基本无法直接编译。但MoveIt2的长期支持状态其实比很多人想象的好。Humble的ros-humble-moveit二进制包已经非常成熟官方维护的教程也都在Humble上跑通了。对于做机械臂应用开发的工程师来说现在入ROS2 MoveIt2是合适的时间点没有必要再守着Foxy或者老MoveIt1不放。1.2 为什么拿Panda做示例PandaFranka Emika Panda在MoveIt社区里几乎是“教科书级”的存在。原因有三第一它是7自由度机械臂冗余自由度多避障时有更多关节空间可以选择比6自由度机械臂更容易规划出绕行路径第二MoveIt资源库直接维护了它的全套配置URDF、SRDF、控制器配置、demo launch文件都齐全装完就能跑不用自己写模型文件第三社区里所有教程、截图、示例代码几乎都用Panda你遇到问题时搜到的资料最多。更重要的是Panda的配置结构很标准你照着它的格式换成自己的URDF基本只需要改模型路径和规划组名其他逻辑完全可以复用。所以拿Panda做实战并不是“只会Panda”而是“通过Panda学会通用流程”。1.3 笛卡尔避障为什么比关节空间规划更麻烦机械臂最常见的规划方式是关节空间规划OMPL里的RRT、PRM这些算法都属于这一类。它只要求给定起点和终点的关节位置中间过程由采样器在关节空间里随机搜索只要找到一条无碰撞路径就算成功。好处是搜索空间大、容易找到解坏处是末端执行器走的轨迹不可控可能是一条奇怪的弧线。笛卡尔路径则不一样。它要求末端执行器沿着一条直线或者圆弧“走位”这意味着路径上每一个中间点都必须做逆运动学求解把末端位姿转换成关节位置然后再做碰撞检测。任何一个中间点不可达、或者该点处机械臂与障碍物碰撞整条路径就会在那一截断掉返回的完成比例fraction就会下降。这就是笛卡尔避障的本质难点路径被严格约束在任务空间规划器没有太多“绕路”的自由只能靠你提供合理的中间waypoint或者调整障碍物配置。2. 环境搭建与验证从一键安装到Demo跑通的完整链路2.1 最小安装清单环境我推荐Ubuntu 22.04 ROS2 Humble这也是目前MoveIt2支持最稳定的组合。安装MoveIt2本身不复杂直接装二进制包sudo apt install ros-humble-moveit sudo apt install ros-humble-moveit-resources-panda-moveit-config第二条装的是Panda的MoveIt配置包里面包含了URDF、SRDF、launch文件和控制器配置。如果你只是想先跑官方的教程Demo这两个包就够了。网上流行的一键安装脚本能帮你把ROS2和MoveIt2一起装好省去折腾环境的痛苦但我的建议是装完以后手动敲一遍apt list --installed | grep moveit确认一下版本号免得后面排查问题时分不清是版本不匹配还是配置错误。2.2 启动Demo后应该检查什么装好后开两个终端分别执行# 终端1 source /opt/ros/humble/setup.bash ros2 launch moveit_resources_panda_moveit_config demo.launch.py # 终端2 source /opt/ros/humble/setup.bash ros2 node listros2 node list至少能看到/move_group这个节点。move_group是MoveIt2的核心节点它负责加载规划器、维护规划场景、处理规划请求和轨迹执行。如果这个节点没起来后面所有代码都白搭。在RViz2界面里把机器人模型拖动一下再点Plan应该能规划出一条关节空间轨迹并执行机械臂模型会跟着动起来。这一步确认了最基本的“规划-执行”链路没问题再往下才谈得上笛卡尔路径和避障。如果你之前用过MoveIt1建议在启动后额外执行ros2 param get /move_group planning_plugin正常会返回类似ompl_interface/OMPLPlanner的值。确认用的是OMPL规划器后面对照参数调整时才不会蒙圈。2.3 为什么不建议一上来就编译源码很多教程喜欢让你从源码编译MoveIt2说是为了调试方便。我的观点是除非你要改MoveIt2源码本身否则直接用二进制包能省下大量时间。源码编译要拉一堆依赖、处理版本冲突、编译半小时起步而二进制包已经打过包、做过测试稳定性远超自己编的版本。等你把应用跑通了再决定要不要源码编译也不迟。2.4 关于Gazebo仿真的一些提醒MoveIt2自带的demo launch不上Gazebo机械臂的“执行”走的是FakeController轨迹会直接体现在RViz2的模型上没有物理仿真。如果你要做Gazebo仿真需要额外装sudo apt install ros-humble-gazebo-ros ros-humble-gazebo-ros2-control然后在launch里加载Gazebo的空世界、把Panda的URDF转成gazebo能读的格式并配置ros2_control的hardware interface。这一步涉及的内容比MoveIt2本身还多建议先把RViz2这条链路跑熟再考虑Gazebo。3. 笛卡尔路径在MoveIt2里的实现逻辑waypoint、eef_step与碰撞检查3.1 三个入口的取舍在我实际调研MoveIt2笛卡尔路径时发现社区里能用的入口其实有三个很多人搞不清区别入口语言稳定性适合场景MoveGroupInterface::computeCartesianPathC高精确控制笛卡尔路径最常用推荐首选moveit_py的 PlanningComponentPython中版本变化快关节空间快速demo、原型验证直接构造MotionPlanRequest并设置cartesian_pathC/Python依赖具体规划器Pilz等工业规划器做LIN/CIRC插补我平时做项目优先用C接口。原因是computeCartesianPath这个方法从MoveIt1到MoveIt2变化最小API也很稳定网上资料多出了问题好查。moveit_py在Humble里虽然能跑但版本之间API差异大尤其是笛卡尔路径相关的方法不同版本可能完全不同。如果你用Python要有随时查源码的准备。3.2 computeCartesianPath的参数到底是什么意思computeCartesianPath的典型调用是double fraction move_group.computeCartesianPath( waypoints, // std::vectorgeometry_msgs::msg::Pose eef_step, // 末端步长单位米 jump_threshold,// 关节跳跃阈值 trajectory, // 输出moveit_msgs::msg::RobotTrajectory avoid_collisions // 是否做碰撞检测 );waypoints是一串末端位姿。相邻两个位姿之间MoveIt2的平移部分做线性插值旋转部分做球面插值SLERP这样能保证姿态过渡平滑。eef_step是每步采样时末端走过的距离单位是米。0.01意味着每采样一个路径点末端移动1厘米。这个值越小路径越精细但计算量成倍增加规划时间变长。我做Panda这类桌面机械臂常用0.005到0.01之间足够平滑也不至于卡顿。jump_threshold是允许的“关节跳跃”阈值单位是弧度/秒实际它不是直接的速度而是相邻采样点之间关节位置的突变量限制。0表示不做限制允许关节瞬时改变位置。大多数场景设0就行因为算法本身会做后处理优化。avoid_collisions设为true时规划器会在每个采样点调用FCL柔性碰撞库做碰撞检测。这个参数就是避障的关键开关。3.3 fraction的深层含义规划失败还是路径被截断computeCartesianPath的返回值fraction表示路径的完成比例取值范围0到1。很多人一看到fraction小于1就以为规划失败其实不准确。fraction为0.85的真实含义是规划器成功生成了前85%的路径点从第85%的位置开始某个waypoint的IK解不出来或者发生了碰撞所以后面的路径被截断了。这里有个实际判断技巧如果fraction在0.9以上可以直接执行剩下的由控制器微调如果fraction在0.5左右说明路径大概率被某个障碍物明显拦截这时候要先检查障碍物位置和路径之间的关系再考虑加绕行点。我在第5章的代码里会演示这个判断逻辑。4. 把障碍物告诉规划器PlanningScene Interface的正确用法4.1 你要修改的是move_group手里的那张“地图”MoveIt2里所有和几何环境相关的信息都存在于规划场景PlanningScene中。它包含机器人模型、周围障碍物的碰撞几何、允许碰撞矩阵等信息。move_group节点维护这份规划场景每次规划请求发出后规划器在当前的规划场景上做碰撞检测。因此避障的第一步不是修改规划器参数而是把障碍物正确写进规划场景。向规划场景添加障碍物的标准接口是PlanningSceneInterface。它会把CollisionObject消息发给move_group节点move_group再更新内部的PlanningScene。4.2 CollisionObject消息的几个关键字段一个完整的moveit_msgs::msg::CollisionObject消息最常用的字段是id障碍物名字必须唯一后续删除或修改时靠id定位。header.frame_id障碍物所在的坐标系比如panda_link0或world。primitives基础几何体列表支持BOX、SPHERE、CYLINDER等。primitive_poses每个几何体的位姿相对于frame_id。operationADD、REMOVE、APPEND等操作类型。有个容易踩的坑是header.frame_id的选择。我建议始终用move_group.getPoseReferenceFrame()获取当前规划参考坐标系而不是凭感觉写world或者base_link。Panda的配置里base_link与panda_link0往往重合但如果你的机械臂模型自定义过写错坐标系会导致障碍物出现在完全错误的位置RViz2里看起来就是“根本不在机器人旁边”。obstacle.header.frame_id move_group.getPoseReferenceFrame();4.3 ADD和ATTACH的区别PlanningSceneInterface除了applyCollisionObject还有attachCollisionObject和detachCollisionObject。这两组操作的含义不同applyCollisionObjectADD把障碍物放在场景中的固定位置比如桌面上的箱子、墙面的围栏。它不会跟随机械臂运动。attachCollisionObject把物体附着到某个机械臂连杆上典型场景是抓取。物体附着上去后会随末端执行器一起运动碰撞检测也会把物体与机械臂本体的碰撞关系排除。在第5章的示例里我们只需要固定障碍物所以用applyCollisionObject就够了。但你要知道记忆里还有ATTACH这条路做抓取任务时会用到。还有一个实操细节applyCollisionObject是异步的add之后立刻发起规划请求move_group可能还没来得及更新场景导致碰撞检查没有生效。稳妥做法是add之后等几百毫秒rclcpp::sleep_for(std::chrono::milliseconds(500));等规划场景刷新完再规划后面代码里我会保留这个sleep别删。5. 代码实战Panda绕过柱状障碍完成直线轨迹的完整实现5.1 新建一个功能包先在工作空间里建包mkdir -p ~/ros2_ws/src cd ~/ros2_ws/src ros2 pkg create panda_cartesian_demo --build-type ament_cmake --dependencies rclcpp moveit_ros_planning_interface geometry_msgs moveit_msgs shape_msgspackage.xml里的depend标签会自动加上这些依赖。然后写CMakeLists.txt把可执行文件加进去add_executable(cartesian_avoidance src/cartesian_avoidance.cpp) ament_target_dependencies(cartesian_avoidance rclcpp moveit_ros_planning_interface geometry_msgs moveit_msgs shape_msgs ) install(TARGETS cartesian_avoidance DESTINATION lib/${PROJECT_NAME} )5.2 核心代码笛卡尔路径绕障下面这段代码就是整个文章的核心。逻辑分为三步第一步添加一个柱状障碍物第二步构造一条会撞到它的直线路径第三步通过waypoint绕行方案绕过障碍物并执行。#include rclcpp/rclcpp.hpp #include moveit/move_group_interface/move_group_interface.h #include moveit/planning_scene_interface/planning_scene_interface.h #include geometry_msgs/msg/pose.hpp #include moveit_msgs/msg/collision_object.hpp #include shape_msgs/msg/solid_primitive.hpp #include thread #include chrono int main(int argc, char** argv) { rclcpp::init(argc, argv); auto node std::make_sharedrclcpp::Node(panda_cartesian_avoidance); // move_group接口内部依赖action通信需要spin executor auto executor std::make_sharedrclcpp::executors::SingleThreadedExecutor(); executor-add_node(node); std::thread([executor]() { executor-spin(); }).detach(); using moveit::planning_interface::MoveGroupInterface; using moveit::planning_interface::PlanningSceneInterface; MoveGroupInterface move_group(node, panda_arm); PlanningSceneInterface planning_scene_interface; // 1. 添加柱状障碍物 moveit_msgs::msg::CollisionObject pillar; pillar.id central_pillar; pillar.header.frame_id move_group.getPoseReferenceFrame(); shape_msgs::msg::SolidPrimitive box; box.type shape_msgs::msg::SolidPrimitive::BOX; box.dimensions.push_back(0.1); // x box.dimensions.push_back(0.1); // y box.dimensions.push_back(0.35); // z geometry_msgs::msg::Pose box_pose; box_pose.position.x 0.45; box_pose.position.y 0.0; box_pose.position.z 0.175; // 半高让柱体从桌面向上长 pillar.primitives.push_back(box); pillar.primitive_poses.push_back(box_pose); pillar.operation moveit_msgs::msg::CollisionObject::ADD; planning_scene_interface.applyCollisionObject(pillar); // 等待move_group刷新规划场景 rclcpp::sleep_for(std::chrono::milliseconds(500)); // 2. 构造笛卡尔路径 std::vectorgeometry_msgs::msg::Pose waypoints; auto current_pose move_group.getCurrentPose().pose; // 起点当前姿态 waypoints.push_back(current_pose); // 目标点移动到柱子正上方的前方区域 geometry_msgs::msg::Pose goal_pose current_pose; goal_pose.position.x 0.60; goal_pose.position.y 0.0; goal_pose.position.z 0.45; // 如果直接用goal_pose做直线规划路径会从柱体中间穿过 // avoid_collisionstrue时fraction会大幅降低。 // 绕行方案先抬升到柱体上方再横向移动最后下降。 geometry_msgs::msg::Pose wp1 current_pose; wp1.position.z 0.65; geometry_msgs::msg::Pose wp2 wp1; wp2.position.x goal_pose.position.x; geometry_msgs::msg::Pose wp3 goal_pose; waypoints.push_back(wp1); waypoints.push_back(wp2); waypoints.push_back(wp3); // 3. 规划并执行 moveit_msgs::msg::RobotTrajectory trajectory; double fraction move_group.computeCartesianPath( waypoints, // 路径点 0.01, // eef_step 末端步长1cm 0.0, // jump_threshold 不做关节跳跃限制 trajectory, // 输出的轨迹 true // 开启碰撞检测 ); RCLCPP_INFO(node-get_logger(), Cartesian path fraction: %.2f, fraction); if (fraction 0.9) { RCLCPP_WARN(node-get_logger(), 路径完成度太低请检查障碍物位置与waypoint是否合理); rclcpp::shutdown(); return 1; } moveit::planning_interface::MoveGroupInterface::Plan plan; plan.trajectory_ trajectory; auto result move_group.execute(plan); if (result moveit::core::MoveItErrorCode::SUCCESS) { RCLCPP_INFO(node-get_logger(), 轨迹执行成功); } else { RCLCPP_ERROR(node-get_logger(), 轨迹执行失败错误码: %d, result.val); } rclcpp::shutdown(); return 0; }5.3 代码背后的路径思路这段代码里最关键的设计是waypoints序列从“一条直线”变成“三段折线”。从起点先竖直抬升到0.65m这高过了柱体顶面0.175 0.175 0.35m所以水平方向上即使从柱体上方飞过也不会碰撞然后再水平移动到目标x位置最后俯冲到目标点0.45m高度。这就是笛卡尔避障的核心技巧不要指望MoveIt2自动绕开障碍物而是把你对环境的理解转化成waypoint。规划器负责的只是“在已给路径点之间做无碰撞的笛卡尔插值”。5.4 编译与运行cd ~/ros2_ws colcon build --packages-select panda_cartesian_demo source install/setup.bash # 终端1启动Panda demo source /opt/ros/humble/setup.bash ros2 launch moveit_resources_panda_moveit_config demo.launch.py # 终端2运行避障程序 source ~/ros2_ws/install/setup.bash source /opt/ros/humble/setup.bash ros2 run panda_cartesian_demo cartesian_avoidance如果一切正常RViz2里会先出现一个灰色柱体然后机械臂末端先升起来水平移过柱子再落下来整个过程流畅无碰撞。6. 实测中我踩过的四个坑位与排查链路6.1 坑一fraction莫名变低机械臂走了一半就停这是最常遇到的问题。你可能发现fraction只有0.5左右机械臂执行时只走了一小段就停止。我的排查链路是先打印fraction确认低于阈值。在RViz2里打开MotionPlanning插件的“Planned Path”显示看看规划出的轨迹断在什么位置。如果有显示把avoid_collisions参数临时改成false重新规划一次。如果改成false之后fraction变成1.0说明路径本身没问题问题一定出在碰撞环节。检查障碍物的位置和机器人当前位置确认路径确实被障碍物截断了。最直接的解决方式是调整waypoints加绕行点不要走直线。很多人在第4步就放弃了只顾着把障碍物位置挪开其实一旦你确认了“障碍物挡路”正确思路不是去除障碍物而是调整路径让它绕开。6.2 坑二RViz2里看不到障碍物代码明明applyCollisionObject了RViz2里就是不显示规划也没有避障效果。排查链路确认frame_id正确。如果panda_link0和world的位置关系和我们想的不一样障碍物会出现在很远的地方规划自然不受影响。在终端订阅规划场景ros2 topic echo /planning_scene如果消息里能看到central_pillar的几何数据和ADD操作说明move_group已经收到了问题多半在RViz2显示设置。 3. 检查RViz2的MotionPlanning插件面板确认“Scene Geometry”和“Show Robot Visual”都勾选了。 4. 确认添加障碍物后确实等了足够时间。不等500毫秒就立刻规划可能move_group还没来得及把新场景同步到可视化端。6.3 坑三execute之后机械臂毫无反应这种情况通常不是MoveIt2规划出了问题而是执行端没有做好准备。先检查ros2 topic info /panda_arm_controller/joint_trajectory正常执行Panda的demo.launch.py后这个topic应该有发布者和订阅者。如果在ros2 topic info里只见Publisher、不见Subscription说明没有东西在执行轨迹。MoveIt2的demo默认用FakeController它只会把轨迹广播出来本身不驱动真实电机——RViz2里的机械臂模型之所以会动是RViz2显示端订阅了这个topic然后更新模型位姿。如果确实没有订阅者最常见原因是你没有把demo.launch.py完整启动起来只单独跑了move_group节点。确认启动命令用的是ros2 launch moveit_resources_panda_moveit_config demo.launch.py而不是手动启动单个节点。6.4 坑四时间戳问题导致规划失败在笛卡尔规划里即使你把header.frame_id写对了如果Pose的header.stamp没有设置或者use_sim_time参数不一致move_group可能会认为坐标信息过期拒绝规划。Panda的demo.launch.py默认use_sim_time:false。如果你在Gazebo环境里跑记得保持所有节点的use_sim_time一致。在代码里构造Pose时我习惯不显式设stamp让tf2使用最新变换如果确实需要指定用rclcpp::Time(0)表示“最新可用”避免指定一个过去时间导致TF等待报错。下面是四个坑的速查表现象可能原因快速验证解决思路fraction低障碍物挡路或I K不可达关闭碰撞检测看fraction是否回升调整waypoint绕行RViz2不显示障碍物frame_id错误、显示未开启、场景未刷新topic echo /planning_scene统一坐标系、等待刷新execute无动作控制器未加载或没有订阅者ros2 topic info joint_trajectory完整启动demo.launch.py规划报TF超时use_sim_time不一致、时间戳过期检查launch参数统一时间源、stamp设为07. 从固定障碍到动态避障进阶方向与真实系统迁移7.1 动态避障的本质从离线场景到实时更新第5章的代码针对的是静态障碍物场景固定规划一次就行。但实际机器人应用中更多场景是障碍物会动比如人走过来、传送带上工件在移动。这时候避障策略要从“规划一次”变成“持续重规划”。机械臂动态避障的主流方案是“感知-地图-规划”闭环用RGB-D相机或者激光雷达采集点云通过八叉树地图Octomap把点云更新到规划场景中MoveIt2的PlanningSceneMonitor订阅这张地图每一帧规划时都基于最新场景做碰撞检测。八叉树的好处是内存可控、更新增量高效移动机器人导航里也常用它做环境表示。这里要顺便说清楚一点机械臂的笛卡尔避障和移动机器人的DWA动态窗口法不是一回事。DWA是在速度空间里采样候选轨迹然后选一条避开障碍物的局部路径核心是“速度采样代价评估”机械臂笛卡尔避障则是在任务空间或者关节空间里搜索满足无碰撞约束的路径核心是“逆运动学碰撞检测”。两者目标相同但几何空间和算法思路完全不同。如果你要从移动机器人避障转过来做机械臂避障别把DWA的思路硬套。7.2 从MoveIt2的demo到Gazebo真机仿真很多读者会问“你的代码在RViz2里跑通了放到Gazebo里能用吗”答案是能但要额外配置。MoveIt2 demo默认的FakeController只是把轨迹发布出来不产生物理反馈。Gazebo里要驱动Panda需要把Panda的URDF/Xacro加载到Gazebo世界。在URDF里加ros2_control描述定义joint_state_broadcaster和joint_trajectory_controller。用gazebo_ros2_control插件作为hardware interface让MoveIt2的轨迹真正作用于Gazebo里的关节。运行方式也从原来单纯的MoveIt demo变成“先启动Gazebo controller manager再启动MoveIt2让MoveIt2连接controller manager执行轨迹”。这中间时间同步问题特别容易出现记得控制端和MoveIt2的use_sim_time要保持一致Gazebo里一般设trueRViz2里也要同步开启。7.3 我的一点实际经验如果你也是刚从ROS1的MoveIt1迁过来我建议别急着在公司机器人或者复杂模型上折腾先在Panda这套官方配置上把“添加障碍物 - 笛卡尔规划 - 执行”这条链路彻底跑通。这条链路一打通你手里就有了一个完全可控的实验环境后面换URDF、改规划组、接深度相机都是在同一个框架里加东西而已。还有个小技巧调试笛卡尔路径时不要每次都用真实机器人试错学会先用RViz2的虚拟模式把fraction和路径形状看清楚。虚拟环境里确认没问题了再上Gazebo或真机能省下大量现场排查时间。我在实际项目里踩过几次“现场执行卡住”的坑事后复盘基本都是虚拟环境里没把障碍物位置调准这个习惯帮我省了不少返工成本。
