python ros gps转xyz坐标系

gps转xyz参考文章
在文章的基础上添加了ros订阅转发的部分
其中self.init_pose是原点的经纬度坐标，需要各位根据自己情况自行更改

import rospy
from geometry_msgs.msg import PoseStamped
from nmea_msgs.msg import Sentence
import time
import math
import numpy as npimport matplotlib.pyplot as pltclass Transfer():def __init__(self):self.CONSTANTS_RADIUS_OF_EARTH = 6371000.     # meters (m)#初始化节点rospy.init_node('gps_to_xyz')self.gnss_pub = rospy.Publisher('/gnss_pose', PoseStamped, queue_size=100)#订阅节点rospy.Subscriber('/nmea_sentence', Sentence, self.callback_sentence)self.init_pose=[0，0，0]# 设置循环的频率rate = rospy.Rate(10)def GPStoXY(self, lat, lon, ref_lat, ref_lon):# input GPS and Reference GPS in degrees# output XY in meters (m) X:North Y:Eastlat_rad = math.radians(lat)lon_rad = math.radians(lon)ref_lat_rad = math.radians(ref_lat)ref_lon_rad = math.radians(ref_lon)sin_lat = math.sin(lat_rad)cos_lat = math.cos(lat_rad)ref_sin_lat = math.sin(ref_lat_rad)ref_cos_lat = math.cos(ref_lat_rad)cos_d_lon = math.cos(lon_rad - ref_lon_rad)arg = np.clip(ref_sin_lat * sin_lat + ref_cos_lat * cos_lat * cos_d_lon, -1.0, 1.0)c = math.acos(arg)k = 1.0if abs(c) > 0:k = (c / math.sin(c))x = float(k * (ref_cos_lat * sin_lat - ref_sin_lat * cos_lat * cos_d_lon) * self.CONSTANTS_RADIUS_OF_EARTH)y = float(k * cos_lat * math.sin(lon_rad - ref_lon_rad) * self.CONSTANTS_RADIUS_OF_EARTH)return x, ydef callback_sentence(self, data):msg=PoseStamped()msg.header=data.headerstr_split=data.sentence.split(',')if str_split[0]=='$GPFPD':heading=float(str_split[3])pitch=float(str_split[4])roll=float(str_split[5])lattitude=float(str_split[6])longitude=float(str_split[7])altitude=float(str_split[8])ve=float(str_split[9])vn=float(str_split[10])vu=float(str_split[11])baseline=float(str_split[12])nsv1=int(str_split[13])nsv2=int(str_split[14])x,y=self.GPStoXY(lat=lattitude,lon=longitude,ref_lat=self.init_pose[0],ref_lon=self.init_pose[1])# print(lattitude,longitude,altitude,x,y)msg.pose.position.x=xmsg.pose.position.y=y# 发布消息self.gnss_pub.publish(msg)if __name__ == '__main__':Transfer()rospy.spin()

python ros gps转xyz坐标系相关推荐

ROS系统实现 tf坐标系广播与监听
安装Ros中 tf 相关功能包 sudo apt-get install ros-melodic-turtle-tf 启动launch文件,这个launch文件相当于一个脚本,可以一次性启动很多节点 ...
人工智能基础（高中版）教材补充和资源分享之番外篇 Cozmo+Python+ROS+AI
ROS Melodic的迷失与救赎::https://blog.csdn.net/column/details/28058.html GitChat::沉迷机器人操作系统的一个理由和四种修仙秘籍原文 ...
Python学习：利用python解析GPS模块数据，并生成可视化地图显示
一.GPS模块数据格式笔者在项目中使用过移远L76K以及梦芯MXT905HM两款GPS定位芯片,两款芯片均是采用串口输出数据,而且数据包采用的是NEMA协议消息结构,消息结构如下: 数据包格式如下: ...
Cozmo+Python+ROS+AI会产生什么样的奇妙反应呢?（玩Cozmo机器人，学Python编程，掌握ROS和AI技术）
Cozmo+Python+ROS+AI会产生什么样的奇妙反应呢? (玩Cozmo机器人,学Python编程,掌握ROS和AI技术) 跟随绿色激光点运动?如何实现? 在黄色边缘线的赛道上行驶?如何实现? ...
WGS84(GPS)、火星坐标系（GCJ02）、百度地图（BD09）坐标系转换案例教程（附转换工具下载）
在做基于百度地图.高德地图等电子地图做为地图服务的二次开发时,通常需要将具有WGS84等坐标的矢量数据(如行政区划.地名.河流.道路等GIS地理空间数据)添加到地图上面,然而,在线地图大多使用的是火星 ...
树莓派连接GPS模块，python获取GPS数据
文章主要内容: 模块之间的连接树莓派识别gps模块使用Python语言获取GPS数据 1.连线 ---- 使用 USB转TTL转换器,将树莓派和GPS模块连接需要的设备:gps模块,gps天线, ...
Python ROS键盘控制机械臂
Python ROS键盘控制机械臂机械臂定义所有的全局变量,给他们一个初始值 import rospy import os import sys, select, termios, tty impo ...
【python+ROS+路径规划】二、理解并处理地图数据
目前打算使用python写出一个Astar的全局路径算法,总体分为三个部分:接收地图数据,设计路径(当然是顺过来的),发布路径. 文章目录一.建立功能包二.接受地图数据(处理上游) 查看地图发布的 ...
Python出租车GPS数据的路网匹配（TransBigData+leuvenmapmatching）
本例尝试使用TransBigData+leuvenmapmatching实现出租车GPS数据的路网匹配,使用的样例数据在:https://github.com/ni1o1/transbigdata/t ...

python ros gps转xyz坐标系

python ros gps转xyz坐标系相关推荐

最新文章

热门文章