python ros gps转xyz坐标系

Posted 2023-02-23 怪皮蛇皮怪

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 np

import matplotlib.pyplot as plt


class 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:East
        lat_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.0
        if 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, y

    def callback_sentence(self, data):
        msg=PoseStamped()
        msg.header=data.header

        str_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=x
            msg.pose.position.y=y

        # 发布消息
        self.gnss_pub.publish(msg)



if __name__ == '__main__':
    Transfer()
    rospy.spin()