tf 四元素和欧拉角转换 获得机器人的实时位置

使用listener.lookupTransform()来获得机器人位置
主要代码如下:

//.h头文件中相关定义
#include 

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

你可能感兴趣的:(库,ROS相关)