collab/igvc2018
git clone https://git.julia.blue/collab/igvc2018
7275bdd
imu
1#!/usr/bin/env python 2 3import rospy 4from std_msgs .msg import Float32 5from vn200 .msg import vn_200_gps_soln ,vn_200_ins_soln 6import math 7from collections import deque 8 9 10waypoints = [[123.0 ,456.0 ], [456.0 ,123.0 ]]# obviously replace these 11 12ourPos = [0.0 ,0.0 ] 13ourDir = 0.0 14angle_deadzone = 10 # Change later 15turnspeed = .1 # 0.1 is alright, might be better to go slower for testing 16straightspeed = .1 17distance_deadzone = .000025 18 19 20def yaw_cb (msg ): 21global ourDir 22ourDir = (msg .data + 360 )% 360 - 112 23# print "you just got a letter", ourDir 24 25def ins_cb (msg ): 26global ourDir 27ourDir = (msg .yaw_heading + 360 )% 360 - 112 28 29 30def gps_cb (msg ): 31global ourPos 32ourPos [0 ]= msg .longitude 33ourPos [1 ]= msg .latitude 34 35 36def compass_bearing (pointA ,pointB ): 37 38lat1 = math .radians (pointA [0 ]) 39lat2 = math .radians (pointB [0 ]) 40 41diffLong = math .radians (pointB [1 ]- pointA [1 ]) 42 43x = math .sin (diffLong )* math .cos (lat2 ) 44y = math .cos (lat1 )* math .sin (lat2 )- (math .sin (lat1 )* math .cos (lat2 )* math .cos (diffLong )) 45 46initial_bearing = math .atan2 (x ,y ) 47 48initial_bearing = math .degrees (initial_bearing ) 49 50compass_bearing = (initial_bearing + 360 )% 360 51return compass_bearing 52 53 54def get_distance (pointA ,pointB ): 55 56distance = math .sqrt ( 57pow ((pointA [0 ]- pointB [0 ]),2 )+ pow ((pointA [1 ]- pointB [1 ]),2 )) 58return distance 59 60 61def driver (): 62global ourPos 63global ourDir 64global angle_deadzone 65global turnspeed 66global straightspeed 67global distance_deadzone 68global tracker_array 69 70tracker_array = deque ([0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ,0 ]) 71rospy .init_node ('gps_waypoints' ,log_level = rospy .INFO ) 72 73imu_sub = rospy .Subscriber ("yaw_heading" ,Float32 ,yaw_cb ) 74 75#ins_sub = rospy.Subscriber("vn200_ins", vn_200_ins_soln, ins_cb) 76 77# Choose INS sub if you want to receive yaw_heading from INS 78# Choose IMU sub otherwise -- I.e IMU: filtered raw data & INS: Direct from factory 79# NOTE: Different angle offsets. Currently IMU has 112 degree offset 80 81gps_sub = rospy .Subscriber ("vn200_gps" ,vn_200_gps_soln ,gps_cb ) 82 83"made subs" 84 85left = rospy .Publisher ('left' ,Float32 ,queue_size = 10 ) 86right = rospy .Publisher ('right' ,Float32 ,queue_size = 10 ) 87 88"made pubs" 89 90rate = rospy .Rate (10 )# number of times per second we run our loop 91"set rate" 92while not rospy .is_shutdown (): 93"please" 94for next_waypoint in waypoints : 95 96 97while get_distance (ourPos ,next_waypoint )> distance_deadzone : 98 99get_distance (ourPos ,next_waypoint ) 100currentbearing = compass_bearing (ourPos ,next_waypoint ) 101dif1 = abs (ourDir - currentbearing ) 102dif2 = ourDir - currentbearing 103"dif:" ,dif1 ,"bear:" ,currentbearing ,"ourDir:" ,ourDir 104while dif1 > 0 105prop = (dif1 )/ (50 ) 106intgral = sum (tracker_array ) 107if dif1 < 180 : 108left .publish (turnspeed + ((.2 )* prop ))+ (.1 )* integral 109right .publish (turnspeed ) 110tracker_array .popleft () 111tracker_array .append (dif2 ) 112else : 113left .publish (turnspeed ) 114right .publish (turnspeed + ((.2 )* prop ))+ (.1 )* integral 115tracker_array .popleft () 116tracker_array .append (dif2 ) 117 118else : 119rate .sleep () 120 121 122if __name__ == '__main__' : 123try : 124"test stuff" 125driver () 126except rospy .ROSInterruptException : 127pass 128 129 130 131 132 133