ROS中可视化车辆运动路径-采用RVIZ进行显示

一：序言

在进行机器人自动驾驶过程中数据分析必不可少，为了更方便我们进行处理和观看这个功能效果，进行可视化是必不可少的，车辆行驶路径往往是以路径点的信息进行保存，为了更直观看到显示的效果，可以将其在RVIZ上可视化出来。
如果觉得学习了C++和ROS不知道怎么入手自动驾驶项目的可以参考这个项目，或者C++和ROS不是很熟练的我也推荐结合相关课程一块学习
无人车采用纯跟踪算法跟随离线路径（ROS，C++实现）第一部分

二：代码和原理

将GPS轨迹，从经纬度WGS-84坐标转换到真实世界xyz坐标系（东北天ENU）下（思路：
计算出每个gps坐标相对与第一个坐标的距离（m为单位），比较相邻两点的经纬度变化，
赋予位移的方向，然后累加得到轨迹）
部分代码

//计算相对位置
        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;