4、Planning Scene通信机制
好，咱们今天来聊聊Planning Scene的通信。这玩意儿在MoveIt里，说白了就是机器人的“世界模型”。它不光知道机器人自己长啥样，还得知道周围环境里有什么障碍物、抓取目标在哪。我刚开始接触MoveIt时，觉得这层不就是个话题发布嘛，后来踩了坑才发现，里面的差分更新机制才是精髓。
Planning Scene的发布与订阅
Planning Scene的数据结构，核心就是moveit_msgs::PlanningScene这个消息。它包含了机器人状态、场景中的物体、碰撞检测信息等等。它的发布和订阅，走的是ROS的Topic机制，话题名就是/planning_scene。
你想想看，一个完整的Planning Scene消息有多大？如果每次环境有一点小变化，比如桌子上的杯子移动了1厘米，你就把整个场景重新发一遍，那网络带宽和CPU都得哭。所以，MoveIt的设计者很聪明，他们搞了一套差分更新机制。
Planning Scene的差分更新机制（PlanningSceneDiff）
什么是差分更新？说白了就是“只发变化的部分”。
在moveit_msgs::PlanningScene消息里，有一个字段叫is_diff。当这个字段被设为true时，这个消息就不再是一个完整的场景描述，而是一个“补丁”。它只包含相对于上一个完整场景的变化。
举个例子：
- 完整场景：包含机器人模型、地面、桌子、杯子A、杯子B。
- 差分更新：如果杯子A被移走了，差分消息里只包含“移除杯子A”这个操作，其他东西都不带。
我个人习惯在调试时，用rostopic echo /planning_scene看一眼，你会发现大部分消息的is_diff都是True。只有当你第一次启动MoveIt，或者手动触发场景重发时，才会看到is_diff: False的完整消息。
核心要点：差分更新机制极大地降低了网络负载和序列化开销。在复杂场景下，这几乎是必须的。我曾经在一个有50个动态障碍物的项目里，如果不用差分更新，CPU占用率直接飙到80%，用了之后降到15%以下。
Planning Scene Monitor的工作原理
那么，谁来负责接收这些差分消息，并维护一个完整的、最新的场景呢？答案是PlanningSceneMonitor。
你可以把它理解成一个“场景管家”。它的工作流程是这样的：
- 订阅话题：它订阅
/planning_scene话题，接收所有发布的Planning Scene消息。 - 维护场景副本：它在内部维护一个
PlanningScene对象，作为当前场景的“黄金副本”。 - 应用差分：当收到一个
is_diff为True的消息时，它会把这个差分“打补丁”到自己的黄金副本上。 - 提供接口：其他组件（比如运动规划器）可以通过
getPlanningScene()方法，随时获取当前最新的、完整的场景。
嗯，这里要注意，PlanningSceneMonitor不仅仅依赖/planning_scene话题。它还会监听/attached_collision_object（附着在机器人上的物体）和/collision_object（环境中的物体）等话题。这些话题通常用于更细粒度的物体操作。
避坑指南：我曾经犯过一个错误，在代码里直接修改了从getPlanningScene()拿到的场景指针，以为这样就能更新场景。结果发现，因为PlanningSceneMonitor内部用的是共享指针，我修改的是它内部的副本，但下一次差分更新一来，我的修改就被覆盖了。正确的做法是，通过发布差分消息来更新场景，而不是直接修改内部对象。
为了让你更直观地理解这三者之间的关系，我画了一张图：
从这张图里，你可以清晰地看到数据流向：其他节点发布差分或完整场景到/planning_scene话题，PlanningSceneMonitor订阅并应用这些变化，维护一个黄金副本，最后运动规划器通过接口获取这个副本进行规划。
注意事项：如果你在写自己的节点，需要发布场景变化，请务必正确设置is_diff字段。如果设置错误，比如把差分消息的is_diff设为False，会导致PlanningSceneMonitor用这个不完整的消息覆盖掉整个黄金副本，造成场景丢失。这个bug我排查了整整一个下午才找到原因。
总结一下，Planning Scene的通信机制，核心就是“差分发布 + 本地维护”。它既保证了场景的实时性，又避免了不必要的网络开销。理解了这一点，你就能更好地掌控MoveIt在复杂环境下的行为。