获取机器人turtlebot3 在gazebo 中仿真导航时的位姿真值

背景：机器人在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获取机器人位置