ROS：GPS坐标序列可视化（在Rviz中显示轨迹）

GPS坐标是经纬度，无法直接在rviz中形成轨迹，本程序实现了以下功能：
将GPS轨迹，从经纬度WGS-84坐标转换到真实世界xyz坐标系（东北天ENU）下（思路：计算出每个gps坐标相对与第一个坐标的距离（m为单位），比较相邻两点的经纬度变化，赋予位移的方向，然后累加得到轨迹）

gps_to_rviz.cpp

#include 
#include "turtlesim/Pose.h"
#include 
#include 
#include 
#include 

struct my_pose
{
    double latitude;
    double longitude;
    double altitude;
};
//角度制转弧度制
double rad(double d) 
{
	return d * 3.1415926 / 180.0;
}
//全局变量
static double EARTH_RADIUS = 6378.137;//地球半径
ros::Publisher state_pub_;
nav_msgs::Path ros_path_;
bool init;
my_pose init_pose;


void gpsCallback(const sensor_msgs::NavSatFixConstPtr& gps_msg_ptr)
{
    //初始化
    if(!init)
    {
        init_pose.latitude = gps_msg_ptr->latitude;
        init_pose.longitude = gps_msg_ptr->longitude;
        init_pose.altitude = gps_msg_ptr->altitude;
        init = true;
    }
    else
    {
    //计算相对位置
        double radLat1 ,radLat2, radLong1,radLong2,delta_lat,delta_long,x,y;
		radLat1 = rad(init_pose.latitude);
        radLong1 = rad(init_pose.longitude);
		radLat2 = rad(gps_msg_ptr->latitude);
		radLong2 = rad(gps_msg_ptr->longitude);
    
        //计算x
        delta_long = 0;
	delta_lat = radLat2 - radLat1;  //(radLat1,radLong1)-(radLat2,radLong1)
	if(delta_lat>0)
        x = 2*asin( sqrt( pow( sin( delta_lat/2 ),2) + cos( radLat1 )*cos( radLat2)*pow( sin( delta_long/2 ),2 ) ));
        else
	x=-2*asin( sqrt( pow( sin( delta_lat/2 ),2) + cos( radLat1 )*cos( radLat2)*pow( sin( delta_long/2 ),2 ) ));

        x = x*EARTH_RADIUS*1000;

        //计算y
	delta_lat = 0;
        delta_long = radLong2  - radLong1;   //(radLat1,radLong1)-(radLat1,radLong2)
	if(delta_long>0)
	y = 2*asin( sqrt( pow( sin( delta_lat/2 ),2) + cos( radLat2 )*cos( radLat2)*pow( sin( delta_long/2 ),2 ) ) );
	else
	y=-2*asin( sqrt( pow( sin( delta_lat/2 ),2) + cos( radLat2 )*cos( radLat2)*pow( sin( delta_long/2 ),2 ) ) );
        //double y = 2*asin( sin( delta_lat/2 ) + cos( radLat2 )*cos( radLat2)* sin( delta_long/2 )   );
        y = y*EARTH_RADIUS*1000;

        //计算z
        double z = gps_msg_ptr->altitude - init_pose.altitude;

        //发布轨迹
        ros_path_.header.frame_id = "path";
        ros_path_.header.stamp = ros::Time::now();  

        geometry_msgs::PoseStamped pose;
        pose.header = ros_path_.header;

        pose.pose.position.x = x;
        pose.pose.position.y = y;
        pose.pose.position.z = z;

        ros_path_.poses.push_back(pose);

        ROS_INFO("( x:%0.6f ,y:%0.6f ,z:%0.6f)",x ,y ,z );

        state_pub_.publish(ros_path_);
    }
}
int main(int argc,char **argv)
{
    init = false;
    ros::init(argc,argv,"gps_to_rviz");
    ros::NodeHandle n;
    ros::Subscriber pose_sub=n.subscribe("/fix",10,gpsCallback);
        
    state_pub_ = n.advertise<nav_msgs::Path>("gps_path", 10);

    ros::spin();
    return 0;
}

更改CMakeLists：

 add_executable(gps_to_rviz src/gps_to_rviz_node.cpp)
 target_link_libraries(gps_to_rviz     ${catkin_LIBRARIES} )

launch文件：

<launch>
    

    <node name="gps_to_rviz" pkg="gps_to_rviz" type="gps_to_rviz" output="screen" />
    
     <node pkg="rosbag" type="play" name="playe" args="--clock /home/zhao/newton2/YOUR.bag"/>  
    <node pkg="rviz" type="rviz" name="rviz" output="screen" 
      args="-d $(find gps_to_rviz)/rviz/default.rviz" required="true">
    </node>

</launch>

注意：

1. launch文件中的数据集名称需要修改成你数据集的名称；
2. rviz最好先设置好，并保存为default.rviz文件，此处主要有两项设置：
   a. 修改 Fixed Frame 为path；
   b. 左下角点击Add，添加path，topic一栏改成/gps_path。

以上代码亲测可用！

参考文献
https://blog.csdn.net/qinqinxiansheng/article/details/111291110