gazebo中获得任意model的groundtruth的pose

初衷
在利用gazebo做实验的时候，我们往往需要获得某个model的pose，拿视觉SLAM来说，很多时候缺少camera 的 groundtruth的pose。那么我们应该怎么获得呢？

做法
我们可以在模型的urdf文件或者sdf文件中加入下面这段代码，这是给模型加入一个ros plugin——libgazebo_ros_p3d。

<gazebo>
  <plugin name="p3d_base_controller" filename="libgazebo_ros_p3d.so">
    <alwaysOn>true</alwaysOn>
    <updateRate>50.0</updateRate>
    <bodyName>base_link</bodyName>
    <topicName>ground_truth/state</topicName>
    <gaussianNoise>0.01</gaussianNoise>
    <frameName>world</frameName>
    <xyzOffsets>0 0 0</xyzOffsets>
    <rpyOffsets>0 0 0</rpyOffsets>
  </plugin>
</gazebo>

加入之后，启动gazebo，加载好了模型之后，在console中敲

rostopic list

然后就会出现**/groundtruth/state**这个topic，这个topic发布的数据类型为：nav_msgs/odometry。然后在ros node中订阅这个topic就可以收到这个model的pose了。