GPS坐标转换并实时显示轨迹

文章目录

  • 下载数据集
  • 创建功能包
  • 主要代码
  • 使用

下载数据集

链接: https://pan.baidu.com/s/1zSXyNhMNQdaFlDFziDse1Q 密码: 6j6u

其中话题/gps/fix和/imu_correct分别为sensor_msgs/NavSatFix类型的gps数据和sensor_msgs/Imu类型的imu数据

查看相关话题信息
在这里插入图片描述

创建功能包

catkin_create_pkg display_trajectory roscpp nav_msgs sensor_msgs turtlesim

主要代码

GPS坐标是经纬度,无法直接在rviz中形成轨迹,本程序实现了以下功能:
将GPS轨迹,从经纬度WGS-84坐标转换到真实世界xyz坐标系(东北天ENU)下(思路:计算出每个gps坐标相对与第一个坐标的距离(m为单位),比较相邻两点的经纬度变化,赋予位移的方向,然后累加得到轨迹)

#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);//计算xdelta_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 ) ));elsex=-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 = 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 ) ) );elsey=-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;//计算zdouble z = gps_msg_ptr->altitude - init_pose.altitude;//发布轨迹ros_path_.header.frame_id = "odom";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("/gps/fix",10,gpsCallback);state_pub_ = n.advertise<nav_msgs::Path>("gps_path", 10);ros::spin();return 0;
}

使用

cd ${ws_root}
source ./devel/setup.bash
rosrun display_trajectory display_trajectory_node

在这里插入图片描述

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.mzph.cn/news/50260.shtml

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

【git】当git lab或者其他代码管理网站的搜索功能不好用时,可以尝试使用git命令辅助搜索关键字

查询git log具体某条记录的方法 1、将 <关键词> 替换为你想要查询的关键词。这将列出所有提交记录中包含指定关键词的记录&#xff1a; git log --grep<关键词> // 注意<>在实际使用时不必写 2、如果你知道要查询的提交记录的哈希值&#xff08;commit has…

创建型模式-抽象工厂模式

在抽象工厂模式中&#xff0c;接口是负责创建一个相关对象的工厂&#xff0c;不需要显式指定它们的类。每个生成的工厂都能按照工厂模式提供对象。 抽象工厂模式提供了一种创建一系列相关或相互依赖对象的接口&#xff0c;而无需指定具体实现类。通过使用抽象工厂模式&#xff…

Pytest和Unittest测试框架的区别?

如何区分这两者&#xff0c;很简单unittest作为官方的测试框架&#xff0c;在测试方面更加基础&#xff0c;并且可以再次基础上进行二次开发&#xff0c;同时在用法上格式会更加复杂&#xff1b;而pytest框架作为第三方框架&#xff0c;方便的地方就在于使用更加灵活&#xff0…

C#使用自定义的比较器对版本号(编码)字符串进行排序

给定一些数据&#xff0c;如下所示: “1.10.1.1.1.2”, “1.1”, “2.2”, “1.1.1.1”, “1.1.3.1”, “1.1.1”, “2.10.1.1.1”, “1.1.2.1”, “1.2.1.1”, “2.5.1.1”, “1.10.1.1”, “1.10.2.1”, “1.11.3.1”, “1.11.12.1”, “1.11.11.1”, “1.11.3.1”, “1”, “…

Matlab进阶绘图第26期—双向堆叠图

双向堆叠图是一种特殊的堆叠图&#xff0c;其通过在两个方向构建堆叠图&#xff0c;从而可以对两种类别的同名及综合属性进行全方位的比较。 由于Matlab中未收录双向堆叠图的绘制函数&#xff0c;因此需要大家自行设法解决。 本文分享一个简单的双向堆叠图的绘制方法&#xf…

SMC状态机 创建SMC.sm文件

XX.sm 文件是一个没有定义状态或转换的框架。包含以下特性: 使用%{ %}进行注释。 关键字讲解&#xff1a; 关键字 作用%package指定此FSM所属的类包&#xff0c;与关联的Task类包相同。%fsmclass指定生成…

java中把一个list转tree的方法

环境 我们有个需求&#xff0c;数据库要存一个无限级联的tree&#xff0c;比如菜单&#xff0c;目录&#xff0c;或者地区等数据&#xff0c;现有两个问题&#xff1a; 问如何设计表。怎么返回给前端一个无线级联的json数据。 思考 第一个问题 在设计表的时候&#xff0c;…

矩阵与图的关系:矩阵是图,图是矩阵

原文连接 线性代数最被低估的一个事实&#xff1a;矩阵是图&#xff0c;图是矩阵。 将矩阵编码为图是一种取巧的行为(cheat code)&#xff0c;它其使复杂的行为变得易于研究。 让我告诉你怎么做&#xff01; 1. 非负矩阵的有向图 &#xff08;The directed graph of a nonne…

验证码识别DLL ,滑块识别SDK,OCR图片转文字,机器视觉找物品

验证码识别DLL ,滑块识别SDK 你们用过哪些OCR提取文字&#xff0c;识图DLL&#xff0c;比如Opencv,Labview机器视觉找物品之类&#xff1f;

高等数学之洛必达法则

第二种 由小到大 其他形式 解题步骤 分子分母的式子是乘积的时候可以用等价无穷小替换&#xff0c;如果是-则不允许

数据分析实战│时间序列预测

时间序列预测问题是一类常见的数据分析问题。数据中往往包含时间标签,这类问题往往根据过去一段时间的数据,建立能够比较精确地反映序列中所包含的动态依存关系的数学模型,并对未来的数据进行预测。 01、问题描述及数据挖掘目标 本案例给出二战时期的某气象站温度记录值,通…

Golang Gorm 高级查询之where查询

插入测试数据 package mainimport ("fmt""gorm.io/driver/mysql""gorm.io/gorm" )type Student struct {ID int64Name string gorm:"size:6"Age intEmail *string }func (*Student) TableName() string {return "student&q…

ChatGPT帮助提升工作效率和质量:完成时间下降40%,质量评分上升 18%

自ChatGPT去年11月发布以来&#xff0c;人们就开始使用它来协助工作&#xff0c;热心的用户利用它帮助撰写各种内容&#xff0c;从宣传材料到沟通话术再到调研报告。 两名MIT经济学研究生近日在《科学》杂志上发表的一项新研究表明&#xff0c;ChatGPT可能有助于减少员工之…

ModuleNotFoundError: No module named ‘matlab.engine‘; ‘matlab‘ is not a package

问题原因及解决方法&#xff1a;程序文件命名为matlab导致了冲突&#xff0c;改变代码文件名称即可https://ww2.mathworks.cn/matlabcentral/answers/362824-no-module-named-matlab-engine-matlab-is-not-a-package

RedisTemplate和StringRedisTemplate的区别、对比

学习 Jedis、RedisTemplate、StringRedisTemplate之间的比较 博客中提到&#xff1a;一. Jedis是Redis官方推荐的面向Java的操作Redis的客户端。 二. RedisTemplate,StringRedisTemplate是SpringDataRedis中对JedisApi的高度封装。SpringDataRedis相对于Jedis来说可以方便地更…

maven工程的目录结构

https://maven.apache.org/guides/introduction/introduction-to-the-standard-directory-layout.html maven工程的目录结构&#xff1a; 在maven工程的根目录下面&#xff0c;是pom.xml文件。此外&#xff0c;还有README.txt、LICENSE.txt等文本文件&#xff0c;便于用户能够…

【GeoDa实用技巧100例】024:geoda计算全局(局部)莫兰指数Moran‘s I,LISA聚类地图,显著性地图

严重声明:本文及专栏《GeoDa空间计量案例教程100例》为CSDN博客专家刘一哥GIS原创,原文及专栏地址为:https://blog.csdn.net/lucky51222/category_12373659.html,谢绝转载或爬取!!! 文章目录 一、计算全局(或局部)单变量莫兰指数I1. 加载实验数据2. 加载权重矩阵3. 创建…

精密图纸被窃,知名手表品牌Seiko遭BlackCat勒索软件攻击

据BleepingComputer消息&#xff0c;日本著名手表制造商Seiko在7月末遭到了网络攻击&#xff0c;8月21日&#xff0c;BlackCat&#xff08;又名ALPHV&#xff09;勒索软件组织在其网站上宣布对这起攻击事件负责。 8 月 10 日&#xff0c;Seiko发布了一份数据泄露通知&#xff0…

IPv4,IPv6,TCP,路由

主要回顾一下TCP&#xff0f;IP的传输过程&#xff0c;在这个过程中&#xff0c;做了什么事情 ip : 网际协议,IP协议能让世界上任意两台计算机之间进行通信。 IP协议的三大功能&#xff1a; 寻址和路由传递服务&#xff1a;不可靠&#xff08;尽最大努力交付传输数据包&…

基于ssm校园快递代取系统源码和论文

基于ssm校园快递代取系统源码和论文056 开发工具&#xff1a;idea 数据库mysql5.7 数据库链接工具&#xff1a;navcat,小海豚等 技术&#xff1a;ssm 摘 要 现代经济快节奏发展以及不断完善升级的信息化技术&#xff0c;让传统数据信息的管理升级为软件存储&#xff0c;…