小场景下基于ROS的GPS经纬高度值转换为平面XYZ坐标值，并用RVIZ显示轨迹

一、实现原理

       在小范围场景下，可以假设GPS经纬度值都在一个平面上，地理正东方向为经度正方向，正北方向为纬度正方向，正上方向为高度正方向，至此经纬高度坐标系已经建立。而我们要做的是将其转换到一个以米为度量单位的平面坐标系下，平面坐标系X轴指向地理正东方向、Y轴指向地理正北方向、Z轴指向正上方向，具体如下图所示：

       在确定好各坐标系之后，计算出X轴方向单位米对应经度的变化量gridLon、Y轴方向单位米对应纬度变化量gridLat以及Z轴方向单位米对应高度变化量gridHei。接着选择一个起始点作为经纬高度坐标系原点（，，）和XYZ平面坐标系原点（，，）。则各点GPS经纬高度值（，，）转换为平面XYZ坐标值（，，）关系式如下所示：

       如此，便可以实现小场景下GPS经纬高度值转换为平面XYZ坐标值。

二、代码实现

       测试环境：Ubuntu18.04系统 + Ros-Melodic版本 + C++编程；

       1、gps_to_xyz.cpp

#include
#include 
#include 
#include 
using namespace std;
//角度制转弧度制
double rad(double d)
{
	return d * 3.14159265 / 180.0;
}
// ROS订阅者和发布者
ros::Subscriber gps_data_sub;
ros::Publisher path_pub;
// 全局变量
string gps_sub_topic = "";
string output_frame_name ="";
double z_rotate_value = 0.0;
double origin_latitude_value = 0.0;
double origin_longitude_value = 0.0;
double origin_altitude_value = 0.0;
double latitude_resolution = 0.0;
double longitude_resolution = 0.0;
double altitude_resolution = 0.0;
bool init_flag = true;
nav_msgs::Path path;
void gps_to_xyz(const sensor_msgs::NavSatFix::ConstPtr& gps_msg)
{
    if (init_flag == true)
    {
        origin_latitude_value = gps_msg->latitude;
        origin_longitude_value = gps_msg->longitude;
        origin_altitude_value = gps_msg->altitude;
        init_flag = false;
    }
    else
    {
        double gps_lat = gps_msg->latitude;
        double gps_lon = gps_msg->longitude;
        double gps_hei = gps_msg->altitude;
        double gps_xx = (gps_lon - origin_longitude_value) / longitude_resolution;
        double gps_yy = (gps_lat - origin_latitude_value) / latitude_resolution;
        double gps_zz = (gps_hei - origin_altitude_value) / altitude_resolution;
        double gps_x = cos(rad(z_rotate_value)) * gps_xx + (-sin(rad(z_rotate_value))) * gps_yy;
        double gps_y = sin(rad(z_rotate_value)) * gps_xx + cos(rad(z_rotate_value)) * gps_yy;
        double gps_z = gps_zz;

        if (path_pub.getNumSubscribers() > 0)
        {
            geometry_msgs::PoseStamped this_pose_stamped;
            this_pose_stamped.header.frame_id = output_frame_name;
            this_pose_stamped.pose.position.x = gps_x;
            this_pose_stamped.pose.position.y = gps_y;
            this_pose_stamped.pose.position.z = gps_z;
            this_pose_stamped.pose.orientation.x = 0.0;
            this_pose_stamped.pose.orientation.y = 0.0;
            this_pose_stamped.pose.orientation.z = 0.0;
            this_pose_stamped.pose.orientation.w = 1.0;
            path.poses.push_back(this_pose_stamped);
            path.header.frame_id = output_frame_name;
        }
        path_pub.publish(path);
    }
}
int main(int argc,char **argv)
{
    ros::init(argc,argv,"gps_deal");
    ros::NodeHandle nh;
    nh.param("gps_sub_topic", gps_sub_topic, "/gps");
    nh.param("output_frame_name", output_frame_name, "map");
    nh.param("auto_get_origin_gps", init_flag, true);
    nh.param("z_rotate_value", z_rotate_value, 1.0);
    nh.param("origin_latitude_value", origin_latitude_value, 1.0);
    nh.param("origin_longitude_value", origin_longitude_value, 1.0);
    nh.param("origin_altitude_value", origin_altitude_value, 1.0);
    nh.param("latitude_resolution", latitude_resolution, 1.0);
    nh.param("longitude_resolution", longitude_resolution, 1.0);
    nh.param("altitude_resolution", altitude_resolution, 1.0);
    gps_data_sub = nh.subscribe(gps_sub_topic, 1000, gps_to_xyz);
    path_pub = nh.advertise("/gpsTrack",1, true);
    ros::spin();
    return 0;
}

       2、gps_to_xyz.launch

                    // 订阅GPS数据话题名称
                 // 输出frame名称
              // 是否自动获取起始点（数据集第一个GPS数据）经纬高度信息
                    // 对整个轨迹进行旋转操作
            // 手动输入起始点经度值
             // 手动输入起始点纬度值
             // 手动输入起始点高度值
   // X轴单位米对应经度变化量
    // Y轴单位米对应纬度变化量
               // Z轴单位米对应高度变化量

  
  

三、实现过程

       1、首先选定一个GPS经纬高度数据点作为起始点和平面XYZ坐标系原点；

       2、分别计算出X、Y、Z轴方向上单位米对应经度、纬度、高度变化量，即经纬高度分辨率，计算方式可以通过Google Earth（需要科学上网）进行测量计算;

       3、修改gps_to_xyz.launch文件相应参数;

       4、运行程序;

       5、播放包含gps数据的rosbag包;

       6、完成；

四、实现结果

GPS经纬高度值转换为平面XYZ坐标值并用RVIZ动态显示

五、总结

       代码开源地址：https://gitee.com/wccworld/gps_to_xyz

       创作不易，如有错误，敬请批评指正。