如何在输出端口回调中获取MultibodyPlant几何层级信息

SceneGraph的原生设计只负责几何碰撞检测、位姿查询、可视化相关的计算，内部存储的帧结构是扁平的，本身不维护刚体之间的运动学父子层级关系，你仅连接它的graph query端口，自然无法获取URDF定义的连杆、关节挂载树结构——这类拓扑信息在模型Finalize之后，只会存储在MultibodyPlant的内部实例中。

不需要修改你现有的TF发布LeafSystem和SceneGraph的连接，只需要做少量调整即可：

• 保留现有接收QueryObject的输入端口，这个端口仍然负责获取所有几何帧的实时世界系位姿。
• 为LeafSystem新增一个抽象类型输入端口，端口数据类型设为const MultibodyPlant*。这个端口不需要做动态数据流连接，在整个Diagram构建完成、调用Finalize()之前，直接获取该端口的可变引用，调用FixValue()方法将你加载完iiwa模型、已经完成Finalize的MultibodyPlant实例const指针固定赋值给该端口即可。MultibodyPlant的运动学拓扑结构在Finalize之后就完全固定，不会随仿真运行改变，用固定值端口传入完全符合Drake的系统设计规范，没有线程安全或者生命周期问题。
• 修改输出端口的计算回调逻辑：
  1. 首次执行回调时，从传入的MultibodyPlant指针中提取全量帧拓扑：可以直接调用MultibodyPlant::GetFrameParentIds()获取所有帧的父帧索引映射，同时提取每个关节定义的固定原点偏移，构建<帧ID, 父帧ID, 父子固定偏移>的查找表，后续回调不需要重复构建该表。
  2. 从SceneGraph的QueryObject中读取每个帧的当前世界系绝对位姿，对照查找表的父子关系，将绝对位姿转换为对应父帧下的相对位姿，再生成ROS TF消息，即可还原URDF定义的真实层级结构，不会出现所有帧直接挂载在world下的问题。

该方案不是仅对MultibodyPlant有效，但也无法无修改适配所有带几何信息的系统：

• 对所有拓扑结构固定、帧层级关系不会在运行时动态变化的几何相关系统，都可以沿用「固定值端口传入系统实例指针拿静态拓扑 + 接SceneGraph query端口拿实时位姿」的架构实现带层级的TF发布。
• 如果是运行时会动态增删帧、修改帧父子挂载关系的自定义几何系统，只需要把固定值传入拓扑的部分改成动态端口：让对应几何系统新增一个输出端口，每个仿真步输出当前最新的帧父子关系映射，TF发布LeafSystem连接该端口获取动态拓扑即可，核心位姿转换、TF生成的逻辑不需要改动。
• 对本身就不维护帧层级、所有帧默认平级挂载在world帧下的几何系统，本身不存在可提取的层级信息，自然无法生成带树状结构的TF。

内容的提问来源于stack exchange，提问作者Geoff