collab/igvc2018
git clone https://git.julia.blue/collab/igvc2018
cef15e4
imu
1#!/usr/bin/env python 2 3from vn200 .msg import vn_200_accel_gyro_compass ,vn_200_gps_soln ,vn_200_ins_soln 4from geometry_msgs .msg import Vector3 5from std_msgs .msg import Header 6import rospy 7import time 8import math 9 10gps_msg = None 11 12def callback (data ): 13global gps_msg 14gps_msg = data 15 16def listener (): 17lat_avg = float (0 ) 18long_avg = float (0 ) 19 20min_latitude = float (100 ) 21min_longitude = float (- 80 ) 22 23max_latitude = float (0 ) 24max_longitude = float (- 100 ) 25 26rospy .init_node ('gps_testing' ,anonymous = True ) 27 28for i in range (0 ,5000 ): 29gps_sub = rospy .Subscriber ("vn200_gps" ,vn_200_gps_soln ,callback ) 30 31global gps_msg 32 33if gps_msg == None : 34return 35 36latitude = gps_msg .latitude 37longitude = gps_msg .longitude 38 39lat_avg = lat_avg + latitude 40long_avg = long_avg + longitude 41 42if (latitude > max_latitude ): 43max_latitude = latitude 44if (longitude > max_longitude ): 45max_longitude = longitude 46 47if (latitude < min_latitude ): 48min_latitude = latitude 49if (longitude < min_longitude ): 50min_longitude = longitude 51 52lat_avg = lat_avg / 5000 53long_avg = long_avg / 5000 54 55"" ) 56 57#print("Lat avg: %3.9f" % lat_avg) 58#print("Long avg: %3.9f" % long_avg) 59#print("Max lat: %3.9f" % max_latitude) 60#print("Max long: %3.9f" % max_longitude) 61#print("Min lat: %3.9f" % min_latitude) 62#print("Min Long: %3.9f" % min_longitude) 63 64#print("") 65 66# Coordinates in decimal degrees (e.g. 2.89078, 12.79797) 67 68lon1 = min_longitude 69lon2 = max_longitude 70 71lat1 = min_latitude 72lat2 = max_latitude 73 74R = 6372880 # radius of Earth in meters 75phi_1 = math .radians (lat1 ) 76phi_2 = math .radians (lat2 ) 77 78delta_phi = math .radians (lat2 - lat1 ) 79delta_lambda = math .radians (lon2 - lon1 ) 80 81a = math .sin (delta_phi / 2.0 )** 2 + math .cos (phi_1 )* math .cos (phi_2 )* math .sin (delta_lambda / 2.0 )** 2 82 83c = 2 * math .atan2 (math .sqrt (a ),math .sqrt (1 - a )) 84 85meters = R * c # output distance in meters 86km = meters / 1000.0 # output distance in kilometers 87 88meters = round (meters ,3 ) 89km = round (km ,3 ) 90 91"Distance: %3.9f m" % meters ) 92"Distance: %3.9f km" % km ) 93 94rospy .signal_shutdown ("quitting" ) 95 96if __name__ == '__main__' : 97try : 98while not rospy .is_shutdown (): 99listener () 100except rospy .ROSInterruptException : 101pass 102 103 104 105 106 107