#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
import numpy as np
import math
from pyproj import Proj, transform
from sensor_msgs.msg import NavSatFix
# UTM52 좌표계
proj_UTM52 = Proj(init='epsg:32652')
# WGS84 GPS
proj_WGS84 = Proj(init='epsg:4326')
f=open('/home/pjh/reference/school_path/sidae.txt', 'w')
total_time = 0
count = 0.0
def getNavFix(data):
global proj_UTM52, proj_WGS84,total_time, count
status = data.status.status
cov = data.position_covariance[0]
# WGS84 -> UTM52
s = rospy.Time.now()
x,y = transform(proj_WGS84, proj_UTM52, data.longitude, data.latitude )
d = rospy.Time.now() - s
total_time = total_time + d
count = count + 1
print x, y
f.write(" %f %f %d %f \n" %(x,y,status, cov))
def main():
global total_time
rospy.init_node("utm52")
total_time = rospy.Duration(0.0)
rospy.Subscriber("/ublox1/fix", NavSatFix, getNavFix)
rospy.spin()
if __name__ == "__main__":
main()
f.close()
t = total_time.to_sec()
print "total time (sec): ", t, "1 time (ms)",t/count*1000
JavaScript
복사