如何获得gazebo仿真中的机器人位姿真值

如何获得gazebo仿真中的机器人位姿真值（这里主要讨论轮式移动机器人），这是一个很简单的问题。主要有三种方法。

1. 订阅gazebo话题 / 请求gazebo服务

在官网教程Tutorial: ROS Communication中，介绍了gazebo向ros发布的rostopic，例如：

Topics:

/clock : rosgraph_msgs/Clock - Publish simulation time, to be used with /use_sim_time parameter.

~/link_states : gazebo_msgs/LinkStates - Publishes states of all the links in simulation.

~/model_states : gazebo_msgs/ModelStates - Publishes states of all the models in simulation.

其中~/model_states发布的就是此次仿真中的所有模型的实时位姿真值，我们需要从中挑选出所关注的模型的信息。另外，link_states发布的是模型中部件的位姿信息，机械臂仿真中可能会需要此项信息。

同时，gazebo运行后还可以提供若干ros service，例如：

These services allow the user to retrieve state and property information about simulation and objects in simulation:

~/get_model_properties : gazebo_msgs/GetModelProperties- This service returns the properties of a model in simulation.

~/get_model_state : gazebo_msgs/GetModelState - This service returns the states of a model in simulation.

~/get_world_properties : gazebo_msgs/GetWorldProperties - This service returns the properties of the simulation world.

~/get_joint_properties : gazebo_msgs/GetJointProperties - This service returns the properties of a joint in simulation.

~/get_link_properties : gazebo_msgs/GetLinkProperties - This service returns the properties of a link in simulation.

~/get_link_state : gazebo_msgs/GetLinkState - This service returns the states of a link in simulation.

~/get_physics_properties : gazebo_msgs/GetPhysicsProperties - This service returns the properties of the physics engine used in simulation.

其中，get_model_state就是相应的获取机器人位姿真值的rosservice。

我们除了可以在命令行中用rostopic echo和rosservice call来获取返回值，也可以写代码来获取，例如请求一个service的写法：

// 开头别忘了引用头文件：
#include "gazebo_msgs/GetModelState.h"

程序里写：

      // ros常规操作：
      ros::NodeHandle n2;
      ros::ServiceClient client2 = n2.serviceClient<gazebo_msgs::GetModelState>("/gazebo/get_model_state");
      gazebo_msgs::GetModelState srv2;
      srv2.request.model_name = "robot"; //指定要获取的机器人在gazebo中的名字；

      if (client2.call(srv2))
      {
          // 如果获取服务成功了的话，
          // srv2.response就是请求服务后返回的数据，可以随便使用了。
      }
      else
      {
          ROS_ERROR("Failed to call service /gazebo/get_model_state");
      }

当然，也可以把这些内容放进一个callback函数里，让别的条件来触发获取真值的操作，例如：

//include略

// 注意callback函数的输入得和触发的topic格式对得上：
void aCallback(const geometry_msgs::Twist& msg)
{
      // ros常规操作：
      ros::NodeHandle n2;
      ros::ServiceClient client2 = n2.serviceClient<gazebo_msgs::GetModelState>("/gazebo/get_model_state");
      gazebo_msgs::GetModelState srv2;
      srv2.request.model_name = "robot"; //指定要获取的机器人在gazebo中的名字；

      if (client2.call(srv2))
      {
          // 如果获取服务成功了的话，
          // srv2.response就是请求服务后返回的数据，可以随便使用了。
      }
      else
      {
          ROS_ERROR("Failed to call service /gazebo/get_model_state");
      }
}

int main(int argc, char** argv){
    ros::init(argc, argv, "a_node");
    ros::NodeHandle n1; 
    // 例如让一个速度控制命令rostopic来触发真值获取：
    ros::Subscriber sub = n1.subscribe("/cmd_vel",1,&aCallback);
    ros::Rate loop_rate1(100);
    
    while (ros::ok())
    {   
        ros::spinOnce();
        loop_rate1.sleep();
    }
    return 0;
}

2. 使用tf

这是一种非常规的获取机器人位姿真值的操作。使用的是ros tf里的TransformListener，归根结底是通过获取空间中两个指定坐标系之间的位姿变换（transform）来得到机器人位姿的。非常适合获取机器人身上一个指定部件在某个坐标系下的实时位姿（例如一个机械臂末端执行器，或者IMU），还是非常有实际意义的。

应用的框架如下：

#include <tf/transform_listener.h>
//其他include略

int main(int argc, char** argv){
    ros::init(argc, argv, "a_node");
    ros::NodeHandle n1; 
    
    tf::TransformListener listener; //创建一个listener；

    ros::Rate rate(100.0);
    
    while (ros::ok()){
        tf::StampedTransform transform; //创建一个transform做储存坐标变换的容器；
        try{
        //选择监测哪两个坐标系；
        listener.lookupTransform("/world", "/aimed_link",  
                                    ros::Time(0), transform);
        }
        catch (tf::TransformException ex){
            ROS_ERROR("%s",ex.what());
            ros::Duration(0.5).sleep();
        }
		//至此transform内就储存了两个指定坐标系间的变换了，可以进行进一步应用：
        //position_x = transform.getOrigin().x();
        //position_y = transform.getOrigin().y();
        //position_z = transform.getOrigin().z();
        //orientation_x = transform.getRotation().x();
        //orientation_y = transform.getRotation().y();
        //orientation_z = transform.getRotation().z();
        //orientation_w = transform.getRotation().w();
      
        ros::spinOnce();
        rate.sleep();
    }
    return 0;
}

3. 应用插件

最方便的就是直接应用gazebo插件了，插件的概念详见Tutorial: Using Gazebo plugins with ROS

插件libgazebo_ros_p3d.so就具有发布机器人位姿真值的功能。

应用举例：

    <!-- 在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>map</frameName>
        <xyzOffsets>0 0 0</xyzOffsets>
        <rpyOffsets>0 0 0</rpyOffsets>
      </plugin>
    </gazebo>

其中bodyName是机器人身上的link，frameName是参考的坐标系，由此我们可以知道，有了这个插件，也可以实现获取机器人身上指定部件位姿真值的目的。