ROS：GPS坐标序列可视化（在Rviz中显示轨迹） |
您所在的位置：网站首页 › ros定位导航到导航点之后转圈圈 › ROS：GPS坐标序列可视化（在Rviz中显示轨迹） |
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("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文件中的数据集名称需要修改成你数据集的名称；rviz最好先设置好，并保存为default.rviz文件，此处主要有两项设置： a. 修改 Fixed Frame 为path； b. 左下角点击Add，添加path，topic一栏改成/gps_path。以上代码亲测可用！ 参考文献 https://blog.csdn.net/qinqinxiansheng/article/details/111291110 |
【本文地址】
今日新闻 |
推荐新闻 |