ros通过监听机器人当前位姿

最新推荐文章于 2023-11-29 09:16:42 发布

lan_th

最新推荐文章于 2023-11-29 09:16:42 发布

阅读量371

点赞数

分类专栏： ros 文章标签：自动驾驶人工智能机器学习

本文链接：https://blog.csdn.net/weixin_48631463/article/details/125492217

版权

ros 专栏收录该内容

9 篇文章 0 订阅

订阅专栏

//.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;
}