ROS入门:GPS坐标转换Rviz显示轨迹

GPS信息是无法直接绘制轨迹的，因为其x，y为经纬度，z为高度，单位不一样，本程序实现了以下功能：

1.将GPS轨迹，从经纬度WGS-84坐标转换到真实世界xyz坐标系下（思路：计算出每个gps坐标相对与第一个坐标的位置（m为单位），然后累加得到轨迹）
2.在ROS框架在，读取GPS信息，并发布真实坐标系下的坐标话题（用Rviz可显示）

#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;radLat1 = rad(init_pose.latitude);radLong1 = rad(init_pose.longitude);radLat2 = rad(gps_msg_ptr->latitude);radLong2 = rad(gps_msg_ptr->longitude);//计算xdelta_lat = radLat2 - radLat1;delta_long = 0;double 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;//计算ydelta_lat = 0;delta_long = radLong1  - radLong2;double y = 2*asin( sqrt( pow( sin( delta_lat/2 ),2) + cos( radLat2 )*cos( radLat2)*pow( sin( delta_long/2 ),2 ) ) );y = y*EARTH_RADIUS*1000;//计算zdouble 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_subscriber");ros::NodeHandle n;ros::Subscriber pose_sub=n.subscribe("/dji_osdk_ros/gps_position",10,gpsCallback);state_pub_ = n.advertise<nav_msgs::Path>("gps_path", 10);ros::spin();return 0;
}

ROS入门:GPS坐标转换Rviz显示轨迹相关推荐

ROS Rviz 显示轨迹 Python
ROS Rviz 显示轨迹 Python 1. 缘由 2. Python实现 3. 效果 1. 缘由 3月一直在调试设备,还要持续一段时间,没空余时间工作上也遇到很多非技术问题同事的帮忙,最近状态 ...
小场景下基于ROS的GPS经纬高度值转换为平面XYZ坐标值，并用RVIZ显示轨迹
一.实现原理在小范围场景下,可以假设GPS经纬度值都在一个平面上,地理正东方向为经度正方向,正北方向为纬度正方向,正上方向为高度正方向,至此经纬高度坐标系已经建立.而我们要做的是将其转换到一个以米为 ...
ROS+Turtlebot3+RVIZ 显示机器人历史轨迹方法
(六)ROS发布里程计(Odometry)消息并在rviz中显示_火星机器人life的博客-CSDN博客_ros 发布odom 一.如果只是RVIZ查看移动机器人的历史轨迹方法 1. 打开gazebo ...
ROS入门(八)——仿真机器人四（Gazebo+Rviz+雷达、摄像头、kinet仿真显示）
所用的学习链接: [奥特学园]ROS机器人入门课程<ROS理论与实践>零基础教程P278-288 [以上视频笔记见http://www.autolabor.com.cn/book/ROST ...
Rviz显示理想的运动的轨迹，并对比实时的运动轨迹
20210505更新前天的bug,修改了marker点的尺寸和颜色后在rviz中不显示的问题解决了,代码什么的完全没有改,只是用了sudo apt-get upgrade,把ros的一些包和库都升级 ...
ROS中rviz显示运动轨迹的常见方法
文章目录 1.利用nav_msgs/Path消息实现轨迹显示 2.利用visualization_msgs/Marker消息类型显示轨迹 1.利用nav_msgs/Path消息实现轨迹显示主函数sh ...
ROS Rviz 显示超声波测量范围 Python
ROS Rviz 显示超声波测量范围 Python 1. 缘由 2. Python实现 3. 效果最近几个月都在忙调试和工程的事情太忙了,两个月都没发布文章了过阵子应该就可以重新开始软件开发工作 ...
ROS Rviz 显示地图 Python
ROS Rviz 显示地图 Python 1. 缘由 2. Python实现 3. 效果 1. 缘由地图在自主移动机器人这个行业里是非常重要的第一步需要可视化地图来感官了解一下在网上的资料基本都 ...
ros中启动rviz显示段错误，核心以转储问题
ros中启动rviz显示段错误,核心以转储问题运行命令 $ rosrun rviz rviz -d rospack find turtle_tf/rviz/turtle_rviz.rviz 显示解 ...

ROS入门:GPS坐标转换Rviz显示轨迹

ROS入门:GPS坐标转换Rviz显示轨迹相关推荐

最新文章

热门文章