4、规划场景（Planning Scene）初始化失败：场景对象冲突、默认规划组未设置、碰撞矩阵配置错误

规划场景初始化失败，这问题我见得太多了。说白了，就是 MoveIt 启动时，内部那个“世界模型”没搭起来。你想想看，机器人还没动，环境先崩了，后面的规划根本跑不起来。

我刚开始用 MoveIt 的时候，被这个问题卡了整整两天。后来发现，原因无非就三个：场景里对象打架了、默认规划组没告诉 MoveIt 用哪条胳膊、或者碰撞矩阵设得太死。咱们一个一个拆。

4.1 场景对象冲突：你加了个“桌子”，我也加了个“桌子”

规划场景里可以放各种对象，比如桌子、墙壁、工件。但 MoveIt 要求每个对象有唯一的 ID。如果你在代码里重复添加了同一个 ID 的对象，或者两个不同的节点同时往场景里塞了同名对象，初始化就会报错。

典型报错信息：

[ERROR] [1625000000.123]: Planning scene monitor: 
  Unable to update planning scene. 
  Object with id 'table' already exists in the world.

嗯，这里要注意。我遇到过最坑的一次，是 launch 文件里同时加载了两个不同的场景描述文件，里面都定义了一个叫“ground_plane”的对象。结果 MoveIt 直接罢工。

正确做法：

// 错误示例：重复 ID
moveit_msgs::CollisionObject obj1;
obj1.id = "table";
moveit_msgs::CollisionObject obj2;
obj2.id = "table";  // 冲突！

// 正确示例：唯一 ID
obj1.id = "assembly_table";
obj2.id = "inspection_table";

4.2 默认规划组未设置：MoveIt 不知道用哪条胳膊

规划组（Planning Group）就是机器人的一个“功能单元”，比如左臂、右臂、夹爪。如果你在配置里没指定默认规划组，或者指定的名字跟 SRDF 里定义的不一致，初始化时 MoveIt 就懵了。

典型报错：

[ERROR] [1625000000.456]: No planning group specified. 
  Cannot initialize planning scene monitor.

为什么会这样？我个人的习惯是，每次改完 SRDF 文件，都会用 check_srdf 工具验证一下。但很多人直接跳过这步。

<arg name="default_planning_group" value="arm_group" />

排查步骤：

1. 打开你的 SRDF 文件（通常在 config/ 目录下）。
2. 检查 <group name="..."> 标签，确认规划组名称。
3. 在 launch 文件中，确保 default_planning_group 参数值完全匹配（大小写敏感）。
4. 如果用了多个规划组，可以在 C++ 或 Python 代码里用 setPlanningGroup() 动态切换。

4.3 碰撞矩阵配置错误：自己跟自己过不去

碰撞矩阵（Allowed Collision Matrix, ACM）决定了哪些物体之间可以“穿透”。如果配置错了，比如把机器人的两个连杆设成“永远不能碰撞”，但它们在初始位姿下恰好挨在一起，初始化就会失败。

典型报错：

[ERROR] [1625000000.789]: Planning scene is in collision.
  Cannot proceed with initialization.

说白了，就是 MoveIt 发现机器人一开始就“撞了”。但很多时候不是真撞，是碰撞矩阵没设对。

配置示例（YAML 格式）：

allowed_collision_matrix:
  entries:
    - {link1: "base_link", link2: "arm_link1", allowed: true}   # 相邻，允许
    - {link1: "arm_link1", link2: "arm_link2", allowed: true}   # 相邻，允许
    - {link1: "base_link", link2: "arm_link3", allowed: false}  # 非相邻，需检测

我记得有一次，我在配置里把 base_link 和 arm_link1 设成了 never allowed。结果机器人一启动就说碰撞，我检查了半天才发现是这里写反了。你想想看，这两个连杆是通过一个旋转关节连着的，怎么可能不挨着？

4.4 综合排查清单

遇到规划场景初始化失败，别慌。按这个顺序查，基本能定位问题：

嗯，基本上把这五点走一遍，90% 的初始化问题都能解决。剩下的 10%，可能是 MoveIt 版本 bug 或者 ROS 环境变量问题，那就得去 GitHub issue 里翻一翻了。