gazebo仿真中小车、机器人或某个link部件的位姿真值获取

问题：
在gazebo中仿真带有摄像头、imu等传感器的小车时，需要获得传感器的数据和实时对应的传感器在世界坐标系中的位姿。我的目的是得到相机图像与对应时刻相机的位姿真值，但是仿真得到的话题仅有图像话题和里程计话题以及tf，而tf中仅有base_link坐标系到小车两个轮子right_wheel_link以及left_wheel_link各自坐标系的变换，以及里程计坐标系odom到小车底座base_footprint坐标系的变换，而里程计坐标系下的小车位姿仅仅是通过仿真中的轮式里程计计算得到的，和小车在世界坐标系中的位姿存在偏差（仿真环境中不知道有没有偏差）。到现在小车（摄像头）在世界坐标系中的位姿还是未知的。
解决办法
对于两个link之间变换关系的求取，参考这里介绍的一些方法，其中最方便直接的是用以下方法得到：
直接使用gazebo中的libgazebo_ros_p3d.so插件来发布机器人或者某个link的位姿真值。即在urdf/xacro文件中添加：

    <!-- 在gazebo仿真模型文件中添加： -->
    <gazebo>
      <plugin name="p3d_base_controller" filename="libgazebo_ros_p3d.so">
        <alwaysOn>true</alwaysOn>
        <updateRate>50.0</updateRate>
        <bodyName>base_footprint</bodyName>
        <topicName>base_pose_ground_truth</topicName>
        <gaussianNoise>0.0</gaussianNoise>
        <frameName>world</frameName>
        <xyzOffsets>0 0 0</xyzOffsets>
        <rpyOffsets>0 0 0</rpyOffsets>
      </plugin>
    </gazebo>

其中bodyName是机器人身上的link，可以是小车本体或者摄像头或者imu，frameName是参考的坐标系，就是世界坐标系的原点world。topicName是发布两个link变换关系的话题名称。发布的这个话题类型是–，发布时可以使用以下命令来显示：

rostopic echo base_pose_ground_truth

输出结果：

然后再进行一些处理比如写一个节点去订阅这个话题再在回调函数中解析就可以利用得到的传感器数据与采集这个数据时的传感器在视觉坐标系中的位姿。