小场景下基于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<ros/ros.h>
#include <nav_msgs/Path.h>
#include <sensor_msgs/NavSatFix.h>
#include <geometry_msgs/PoseStamped.h>
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<string>("gps_sub_topic", gps_sub_topic, "/gps");nh.param<string>("output_frame_name", output_frame_name, "map");nh.param<bool>("auto_get_origin_gps", init_flag, true);nh.param<double>("z_rotate_value", z_rotate_value, 1.0);nh.param<double>("origin_latitude_value", origin_latitude_value, 1.0);nh.param<double>("origin_longitude_value", origin_longitude_value, 1.0);nh.param<double>("origin_altitude_value", origin_altitude_value, 1.0);nh.param<double>("latitude_resolution", latitude_resolution, 1.0);nh.param<double>("longitude_resolution", longitude_resolution, 1.0);nh.param<double>("altitude_resolution", altitude_resolution, 1.0);gps_data_sub = nh.subscribe<sensor_msgs::NavSatFix>(gps_sub_topic, 1000, gps_to_xyz);path_pub = nh.advertise<nav_msgs::Path>("/gpsTrack",1, true);ros::spin();return 0;
}

2、gps_to_xyz.launch

<launch><param name="gps_sub_topic" value = "/gps" />                  // 订阅GPS数据话题名称<param name="output_frame_name" value = "map" />               // 输出frame名称<param name="auto_get_origin_gps" value = "true" />            // 是否自动获取起始点（数据集第一个GPS数据）经纬高度信息<param name="z_rotate_value" value = "0.0" />                  // 对整个轨迹进行旋转操作<param name="origin_longitude_value" value = "1.0" />          // 手动输入起始点经度值<param name="origin_latitude_value" value = "1.0" />           // 手动输入起始点纬度值<param name="origin_altitude_value" value = "1.0" />           // 手动输入起始点高度值<param name="longitude_resolution" value = "1.09244689e-05" /> // X轴单位米对应经度变化量<param name="latitude_resolution" value = "9.01006167e-06" />  // Y轴单位米对应纬度变化量<param name="altitude_resolution" value = "1.0" />             // Z轴单位米对应高度变化量<node pkg="gps_to_xyz" name="gps_to_xyz" type="gps_to_xyz" output="screen" /><node pkg="rviz" type="rviz" name="rviz" args="-d $(find gps_to_xyz)/rviz/gps_to_xyz.rviz" />
</launch>

三、实现过程

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

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

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

Optitrack视觉定位下基于ROS及PX4搭建四旋翼多机飞行平台
Optitrack视觉定位下基于ROS及PX4搭建四旋翼多机飞行平台搭建 1 单机平台 1.1 四旋翼硬件组装 a)注意 1.2 机载板环境配置 1.3 飞控参数配置 a)注意 1.4 实飞全流程 2 ...
Ubuntu18.04下基于ROS和PX4的无人机仿真平台的基础配置搭建（XTDrone的）
摘自:https://www.ngui.cc/51cto/show-23557.html Ubuntu18.04下基于ROS和PX4的无人机仿真平台的基础配置搭建编程学习 · 2020/7/12 1 ...
ROS Rviz 显示轨迹 Python
ROS Rviz 显示轨迹 Python 1. 缘由 2. Python实现 3. 效果 1. 缘由 3月一直在调试设备,还要持续一段时间,没空余时间工作上也遇到很多非技术问题同事的帮忙,最近状态 ...
在复杂场景下基于VIO辅助的运动恢复结构方案
点云PCL免费知识星球,点云论文速读. 文章:VIO-Aided Structure from Motion Under Challenging Environments 作者:Zijie Jiang ...
一种公路场景下基于视觉的车辆检测和计数系统
1 文章信息本次介绍的是题目为<Vision-based vehicle detection and counting system using deep learning in highwa ...
直播 | 复旦大学许燚：少量标注样本场景下基于数据编程的半监督分类
「AI Drive」是由 PaperWeekly 和 biendata 共同发起的学术直播间,旨在帮助更多的青年学者宣传其最新科研成果.我们一直认为,单向地输出知识并不是一个最好的方式,而有效地反馈和 ...
Ubuntu16.04下基于ROS控制Robotiq_2f_85_gripper
一.实验设备操作系统:Ubuntu 16.04 夹爪型号:Robotiq 2f-85 转换线:RS485转USB 二.Robotiq功能包安装与编译 1.下载Robotiq功能包上Github 寻 ...
ROS入门:GPS坐标转换Rviz显示轨迹
GPS信息是无法直接绘制轨迹的,因为其x,y为经纬度,z为高度,单位不一样,本程序实现了以下功能: 1.将GPS轨迹,从经纬度WGS-84坐标转换到真实世界xyz坐标系下(思路:计算出每个 ...
sqlserver检测到基于一致性的逻辑_面试官;解决分布式场景下数据一致性问题
在这一篇中主要回答目前分布式事务问题是怎么解决的?行业中有什么解决方案?这些解决方案分别有什么优缺点?别人是怎么做的?我们可以怎么来做? 支付重构考虑支付重构的时候,自然想到原本属于一个本地事务中的 ...

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

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

最新文章

热门文章