以插件形式获取

可以在机器人的urdf模型中加入插件。

    <!-- Fake localization plugin -->
    <plugin name="ground_truth_odometry" filename="libgazebo_ros_p3d.so">
      <alwaysOn>true</alwaysOn>
      <updateRate>100.0</updateRate>
      <bodyName>base_link</bodyName>
      <topicName>base_pose_ground_truth</topicName>
      <gaussianNoise>0.01</gaussianNoise>
      <frameName>map</frameName>-->
        <!-- initialize odometry for fake localization-->
      <xyzOffsets>0 0 0</xyzOffsets>
      <rpyOffsets>0 0 0</rpyOffsets>
    </plugin>

以订阅者形式获取

订阅话题/gazebo/model_states，找到对应模型的名称，例如机器人名称为turtlebot3。

void _modelStatesCallback(const gazebo_msgs::ModelStatesConstPtr &msg)
{
  int modelCount = msg->name.size();

  for(int modelInd = 0; modelInd < modelCount; ++modelInd)
  {
      if(msg->name[modelInd] == "turtlebot3")
      {
          _current_pose.pose = msg->pose[modelInd];
          _current_velocity.twist = msg->twist[modelInd];
          break;
      }
  }
}