GPS坐标是经纬度,无法直接在rviz中形成轨迹,本程序实现了以下功能:
将GPS轨迹,从经纬度WGS-84坐标转换到真实世界xyz坐标系(东北天ENU)下(思路:计算出每个gps坐标相对与第一个坐标的距离(m为单位),比较相邻两点的经纬度变化,赋予位移的方向,然后累加得到轨迹)
gps_to_rviz.cpp
#include <ros/ros.h>
#include "turtlesim/Pose.h"
#include <sensor_msgs/NavSatFix.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Path.h>
#include <math.h>
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>
注意:
- launch文件中的数据集名称需要修改成你数据集的名称;
- rviz最好先设置好,并保存为default.rviz文件,此处主要有两项设置:
a. 修改 Fixed Frame 为path;
b. 左下角点击Add,添加path,topic一栏改成/gps_path。
以上代码亲测可用!
参考文献
https://blog.csdn.net/qinqinxiansheng/article/details/111291110