RTK转GPS

rtk2gps.py

#!/usr/bin/env python
# coding=utf-8
from __future__ import print_function, division

from geometry_msgs.msg import Quaternion, Point
from sensor_msgs.msg import NavSatFix
from serial_port.msg import rtk
from nav_msgs.msg import Odometry
import rospy
import numpy as np
import transforms3d
import pyproj
import time


def cb_rtk_to_gps(rtk):
    # rpy
    roll = rtk.roll / 180 * np.pi
    pitch = rtk.pitch / 180 * np.pi
    yaw = rtk.yaw / 180 * np.pi

    odom = Odometry()
    odom.header.frame_id = 'odom'
    odom.child_frame_id = 'base_link'
    odom.header.stamp = rtk.header.stamp

    # TODO 确定惯导的imu坐标系
    # 惯导提供的是北东地(xyz)
    # geo_transform需要的是东北天(xyz)
    R_origin = transforms3d.euler.euler2mat(roll, pitch, yaw)
    # R_axes = transforms3d.euler.euler2mat(np.pi / 2, 0, np.pi, 'szyx')
    R_axes = transforms3d.euler.euler2mat(-np.pi / 2, 0, np.pi, 'szyx')
    qw, qx, qy, qz = transforms3d.quaternions.mat2quat(np.matmul(R_axes, R_origin))
    # qw, qx, qy, qz = transforms3d.quaternions.mat2quat(R_origin)
    # qw, qx, qy, qz = transforms3d.euler.euler2quat(pitch, roll, - yaw + np.pi / 2)
    odom.pose.pose.orientation = Quaternion(w=qw, x=qx, y=qy, z=qz)

    odom.pose.pose.position = Point(x=rtk.Lon, y=rtk.Lat, z=rtk.Ati)
    pub_odom.publish(odom)
    print(odom.pose.pose.position)



if __name__ == '__main__':
    rospy.init_node('rtk_to_fix')
    rospy.Subscriber('/RTK', rtk, cb_rtk_to_gps)
    pub_odom = rospy.Publisher('/nav_odom', Odometry, queue_size=1)
    rospy.spin()