使用 tf 庫 獲得機器人的實時位置

使用listener.lookupTransform（）來獲得機器人位置
主要代碼如下：

//.h頭文件中相關定義
#include <tf/transform_listener.h>

struct state
{
    float x;
    float y;
    float yaw;
};

tf::TransformListener listener;

state robot_pose;

//.cpp文件中主函數中的主循環
state robot_pose;
tf::StampedTransform transform;
while (node.ok()){ 
    try{
       listener.waitForTransform("map","base_link",ros::Time(0),ros::Duration(3.0));
       listener.lookupTransform("/map","/base_link",ros::Time(0),transform);
    }
    catch(tf::TransformException &ex){
        ROS_ERROR("%s",ex.what());
        ros::Duration(1.0).sleep();
        continue;
    }
    
    double roll,pitch,yaw;
    tf::Matrix3x3(transform.getRotation()).getEulerYPR(yaw,pitch,roll);

    //tf::Quaternion quat;
    //tf::quaternionMsgToTF(odom_msg.pose.pose.orientation,quat);
    //double roll,pitch,yaw;
    //tf::Matrix3x3(quat).getRPY(roll,pitch,yaw);

    robot_pose.x = transform.getOrigin().x();
    robot_pose.y = transform.getOrigin().y();
    robot_pose.yaw = yaw;

    return robot_pose;
}

注意： 其中lookupTransform:
不可以把ros::Time(0)改成ros::time::now()，因爲監聽做不到實時，會有幾毫秒的延遲。ros::Time(0)指最近時刻存儲的數據，ros::time::now()則指當下，如果非要使用ros::time::now,則需要結合waitForTransform()使用

一些相關鏈接：
https://blog.csdn.net/Hansry/article/details/84848884
https://blog.csdn.net/sru_alo/article/details/92804479
https://www.jianshu.com/p/17c016778879