获取机器人当前位姿(C++)

1.生成功能包：

catkin_create_pkg auto_elevator std_msgs roscpp

2.编写代码：
创建find_robot_pose.cpp文件

//.h头文件中相关定义
#include <tf/transform_listener.h>

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

int main(int argc, char **argv)
{
    ros::init(argc, argv, "find_robot_pose");
    ros::NodeHandle n, turtle_node;
    tf::TransformListener listener;
    //.cpp文件中主函数中的主循环
    state robot_pose;
    tf::StampedTransform transform;
    while (n.ok())
    {
        try
        {
            listener.waitForTransform("map", "base_link", ros::Time(0), ros::Duration(3.0));
            listener.lookupTransform("/map", "/base_link", ros::Time(0), transform);
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;

        ROS_INFO("robot pose x: %f", robot_pose.x);
        ROS_INFO("robot pose y: %f", robot_pose.y);
        ROS_INFO("robot pose yaw: %f", robot_pose.yaw);
        return 0;
    }
}

配置CMakeLists.txt

add_executable(${PROJECT_NAME}_node src/find_robot_pose.cpp)
target_link_libraries(${PROJECT_NAME}_node
  ${catkin_LIBRARIES}
)

3.运行代码：