SLAM笔记——获取gazebo真实位姿

调用gazebo/get_model_state服务，通过机器人的名字直接获得真实位姿信息，并以pose发布

/*
 * @Author: Jimazeyu
 * @Date: 2022-02-18 03:19:20
 * @LastEditors: OBKoro1
 * @LastEditTime: 2022-02-18 04:38:48
 * @FilePath: /catkin_ws/src/map_tutorials/src/pose_publisher.cpp
 */
#include"ros/ros.h"
#include"iostream"
#include"gazebo_msgs/GetModelState.h"
#include"geometry_msgs/PoseStamped.h"
using namespace std;

int main(int argc,char**argv)
{
    ros::init(argc, argv, "true_pose_publisher");
    ros::NodeHandle n("~");
    ros::Rate loop_rate(1);
    //获取机器人名字
    string robot_name;
    n.param<std::string>("robot_name", robot_name, "tb3_0");
    cout<<robot_name<<endl;
    //用于发布机器人真实坐标
    ros::Publisher pose_publisher=n.advertise<geometry_msgs::PoseStamped>("true_pose",10);
    //用于获得真实坐标
    ros::ServiceClient client=n.serviceClient<gazebo_msgs::GetModelState>("/gazebo/get_model_state");
    int seq=0;//发布序号
    while(ros::ok())
    {
        gazebo_msgs::GetModelState srv;
        srv.request.model_name = robot_name; //指定要获取的机器人在gazebo中的名字；
        if (client.call(srv))
        {
            geometry_msgs::PoseStamped pose;
            pose.header.frame_id="gazebo";
            pose.header.seq=seq++;
            pose.header.stamp=ros::Time::now();
            pose.pose=srv.response.pose;
            pose_publisher.publish(pose);
        }
        else
        {
            ROS_ERROR("Failed to call service /gazebo/get_model_state");
        }
        loop_rate.sleep();
    }

    return 0;
}