一:序言
在进行机器人自动驾驶过程中数据分析必不可少,为了更方便我们进行处理和观看这个功能效果,进行可视化是必不可少的,车辆行驶路径往往是以路径点的信息进行保存,为了更直观看到显示的效果,可以将其在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;