collab/igvc2018

git clone https://git.julia.blue/collab/igvc2018

rickyc106Clean up testing filescef15e4

imu
2.2 KiB107 linesraw
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):
13	global gps_msg
14	gps_msg = data
15
16def listener():
17	lat_avg = float(0)
18	long_avg = float(0)
19
20	min_latitude = float(100)
21	min_longitude = float(-80)
22
23	max_latitude = float(0)
24	max_longitude = float(-100)
25
26	rospy.init_node('gps_testing', anonymous = True)
27
28	for i in range(0,5000):
29		gps_sub = rospy.Subscriber("vn200_gps", vn_200_gps_soln, callback)
30
31		global gps_msg
32
33		if gps_msg==None:
34			return
35
36		latitude = gps_msg.latitude
37		longitude = gps_msg.longitude
38
39		lat_avg = lat_avg + latitude
40		long_avg = long_avg + longitude
41
42		if (latitude > max_latitude):
43			max_latitude = latitude
44		if (longitude > max_longitude):
45			max_longitude = longitude
46
47		if (latitude < min_latitude):
48			min_latitude = latitude
49		if (longitude < min_longitude):
50			min_longitude = longitude
51
52	lat_avg = lat_avg / 5000
53	long_avg = long_avg / 5000
54
55	print("")
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	
68	lon1 = min_longitude
69	lon2 = max_longitude
70
71	lat1 = min_latitude
72	lat2 = max_latitude
73
74	R = 6372880  # radius of Earth in meters
75	phi_1 = math.radians(lat1)
76	phi_2 = math.radians(lat2)
77
78	delta_phi = math.radians(lat2 - lat1)
79	delta_lambda = math.radians(lon2 - lon1)
80
81	a = math.sin(delta_phi / 2.0) ** 2 + math.cos(phi_1) * math.cos(phi_2) * math.sin(delta_lambda / 2.0) ** 2
82	
83	c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
84
85	meters = R * c  # output distance in meters
86	km = meters / 1000.0  # output distance in kilometers
87
88	meters = round(meters, 3)
89	km = round(km, 3)
90
91	print("Distance: %3.9f m" % meters)
92	print("Distance: %3.9f km" % km)
93
94	rospy.signal_shutdown("quitting")
95
96if __name__ == '__main__':
97	try:
98		while not rospy.is_shutdown():
99			listener()
100	except rospy.ROSInterruptException:
101		pass
102
103
104
105
106
107