[Python] GPS test code

Basic code for the GPS test

Code

#! /usr/bin/env python
# -*- coding: utf-8 -*-

import rospy
#from sensor_msgs.msg import Imu
from nav_msgs.msg import Path
from sensor_msgs.msg import NavSatFix
from geometry_msgs.msg import PoseStamped
from pyproj import Proj, transform
#from tf.transformations import euler_from_quaternion, quaternion_from_euler
import math




# Definition of Projection
# WGS84
proj_WGS84 = Proj(init='epsg:4326') # WGS84 long, lat, GPS earth coordinate

'''
*WGS84 경위도: GPS가 사용하는 좌표계
EPSG:4326, EPSG:4166 (Korean 1995)
+proj=longlat +ellps=WGS84 +datum=WGS84 +no_defs 

*Bessel 1841 경위도: 한국과 일본에 잘 맞는 지역타원체를 사용한 좌표계
EPSG:4004, EPSG:4162 (Korean 1985)
+proj=longlat +ellps=bessel +no_defs +towgs84=-115.80,474.99,674.11,1.16,-2.31,-1.63,6.43

*GRS80 경위도: WGS84와 거의 유사
EPSG:4019, EPSG:4737 (Korean 2000)
+proj=longlat +ellps=GRS80 +no_defs

*Google Mercator: 구글지도/빙지도/야후지도/OSM 등 에서 사용중인 좌표계
EPSG:3857(공식), EPSG:900913(통칭)
+proj=merc +a=6378137 +b=6378137 +lat_ts=0.0 +lon_0=0.0 +x_0=0.0 +y_0=0 +k=1.0 +units=m +nadgrids=@null +no_defs

'''

# UTM 52N
proj_UTM52N = Proj(init='epsg:32651') # UTM-K (Bassel)


'''
*UTM52N (WGS84): 경도 120~126도 사이에서 사용
EPSG:32652
+proj=utm +zone=52 +ellps=WGS84 +datum=WGS84 +units=m +no_defs 

*UTM51N (WGS84): 경도 126~132도 사이에서 사용
EPSG:32651
+proj=utm +zone=51 +ellps=WGS84 +datum=WGS84 +units=m +no_defs 
'''

# origin
ori_x = 0
ori_y = 0

count = 0

callback_cnt = 0

def callback(msg):
  global count, ori_x, ori_y
  global path
  global callback_cnt
  
  callback_cnt +=1
  print('\n\n  === Callback count : {}'.format(callback_cnt))
  
  if count == 0:
    count = 1
  print('----[GPS Data]')
  print(' frame_id    : {}'.format(msg.header.frame_id))
  print(' seq         : {}'.format(msg.header.seq))
  print(' status      : {}'.format(msg.status.status))
  print('    NO_FIX    = -1')
  print('    FIX       =  0')
  print('    SBAS_FIX  =  1')
  print('    GBAS_FIX  =  2')
  print(' service     : {}'.format(msg.status.service))
  print('    GPS       =  1')
  print('    GLONASS   =  2')
  print('    COMPASS   =  4')
  print('    GALILEO   =  8')
  print(' latitude    : {}'.format(msg.latitude))
  print(' longitude   : {}'.format(msg.longitude))
  print(' altitude    : {}'.format(msg.altitude))
  print(' position_cov: {}'.format(msg.position_covariance))
  
  # Lat Lon to Meter using pyproj
  utm_x, utm_y = transform(proj_WGS84, proj_UTM52N, msg.latitude, msg.longitude)
  print('\n--- UTM')
  print(utm_x, utm_y)
  
#  utm_x *= 10^6
#  utm_y *= 10^6
  
  # Set origin
  if (count == 1):
    ori_x = utm_x
    ori_y = utm_y
    count = 2
  
  print('\n--- Origin')
  print(ori_x, ori_y)
  
  # Curr position
  print('\n--- Curr position')
  print('  x  : {}'.format((ori_x - utm_x)))
  print('  y  : {}'.format((ori_y - utm_y)))
  
  # Pose
  pose = PoseStamped()
  pose.header.frame_id = 'camera_init'
  
  pose.pose.position.x = ori_x - utm_x
  pose.pose.position.y = ori_y - utm_y
  
  path.poses.append(pose)
  
  pub.publish(path)
  
rospy.init_node('get_imu_data_node')
rate = rospy.Rate(10)
sub = rospy.Subscriber('gps_data', NavSatFix, callback)
pub = rospy.Publisher('gps_path', Path, queue_size=1)
path = Path()
path.header.frame_id = 'camera_init'
path.header.stamp = rospy.Time.now()

rospy.spin()