背景：机器人在gazebo环境中仿真导航，除了实时获取传感器数据估计机器人位姿外，为了验证定位算法的精度，我们需要获得机器人在gazebo worlds下的真实位姿。
方法一：在机器人模型的urdf文件或者sdf文件中加入一个ros plugin——libgazebo_ros_p3d。
以turtlebot3_waffle_pi 为例：
（1）找到 turtlebot3 gazebo 模型的description文件
wsx@hello:~/catkin_ws$ rospack find turtlebot3_description /opt/ros/kinetic/share/turtlebot3_description
(2) 进入/opt/ros/kinetic/share/turtlebot3_description/ 打开turtlebot3_waffle_pi.gazebo.xacro文件：具体如下：
wsx@hello:~/catkin_ws$ cd wsx@hello:~$ cd /opt/ros/kinetic/share/turtlebot3_description wsx@hello:/opt/ros/kinetic/share/turtlebot3_description$ ls cmake meshes package.xml rviz urdf wsx@hello:/opt/ros/kinetic/share/turtlebot3_description$ cd urdf wsx@hello:/opt/ros/kinetic/share/turtlebot3_description/urdf$ ls common_properties.xacro turtlebot3_burger_for_autorace_2020.gazebo.xacro turtlebot3_burger_for_autorace_2020.urdf.xacro turtlebot3_burger_for_autorace.gazebo.xacro turtlebot3_burger_for_autorace.urdf.xacro turtlebot3_burger.gazebo.xacro turtlebot3_burger.urdf.xacro turtlebot3_waffle_for_open_manipulator.urdf.xacro turtlebot3_waffle.gazebo.xacro turtlebot3_waffle_pi_for_open_manipulator.gazebo.xacro turtlebot3_waffle_pi_for_open_manipulator.urdf.xacro turtlebot3_waffle_pi.gazebo.xacro turtlebot3_waffle_pi.urdf.xacro turtlebot3_waffle.urdf.xacro wsx@hello:/opt/ros/kinetic/share/turtlebot3_description/urdf$ sudo gedit turtlebot3_waffle_pi.gazebo.xacro
在urtlebot3_waffle_pi.gazebo.xacro文件后面将以下代码加入
<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>
保存退出
（3）重新启动turtlebot3_gazebo的导航仿真，再打开新终端,运行rostopic list 查看topic。
$ rostopic list
可以看到 /ground_truth/state 的话题。
rostopic echo /ground_truth/state
可以查看该topic 位姿的具体信息
header: seq: 207 stamp: secs: 1337 nsecs: 705000000 frame_id: "world" child_frame_id: "base_footprint" pose: pose: position: x: -4.5942145711 y: -4.00644960896 z: -0.0110085024163 orientation: x: 0.000522316202193 y: -0.00150770718405 z: -0.32162607462 w: -0.946865419228 covariance: [0.0001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0001] twist: twist: linear: x: 0.198892491109 y: 0.161655496399 z: 0.00287990416913 angular: x: 0.00520107668762 y: 0.0103735001011 z: 0.299275027601 covariance: [0.0001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0001]
base_footprint到base_link的坐标变换是：0 0 0.1 0 0 0，两者的z坐标不一样，base_link一般是与机器人中心重合，而base_footprint是base_link原点在地面的投影（所以z坐标不一样）。
其他方法参考
如何获得gazebo仿真中的机器人位姿真值_寒墨阁的博客-CSDN博客_gazebo获取机器人位置
