collab/igvc2018

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

rickyc106Add rosserial node. Fix gps subscriber. Lower speed.7275bdd

imu
3.7 KiB133 linesraw
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):
21    global ourDir
22    ourDir = (msg.data + 360) % 360 - 112
23    #  print "you just got a letter", ourDir
24
25def ins_cb(msg):
26	global ourDir
27	ourDir = (msg.yaw_heading + 360) % 360 - 112
28
29
30def gps_cb(msg):
31    global ourPos
32    ourPos[0] = msg.longitude
33    ourPos[1] = msg.latitude
34
35
36def compass_bearing(pointA, pointB):
37
38    lat1 = math.radians(pointA[0])
39    lat2 = math.radians(pointB[0])
40
41    diffLong = math.radians(pointB[1] - pointA[1])
42
43    x = math.sin(diffLong) * math.cos(lat2)
44    y = math.cos(lat1) * math.sin(lat2) - (math.sin(lat1)* math.cos(lat2) * math.cos(diffLong))
45
46    initial_bearing = math.atan2(x, y)
47
48    initial_bearing = math.degrees(initial_bearing)
49
50    compass_bearing = (initial_bearing + 360) % 360
51    return compass_bearing
52
53
54def get_distance(pointA, pointB):
55
56    distance = math.sqrt(
57        pow((pointA[0] - pointB[0]), 2) + pow((pointA[1] - pointB[1]), 2))
58    return distance
59
60
61def driver():
62    global ourPos
63    global ourDir
64    global angle_deadzone
65    global turnspeed
66    global straightspeed
67    global distance_deadzone
68    global tracker_array
69
70    tracker_array =  deque([0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0])
71    rospy.init_node('gps_waypoints', log_level=rospy.INFO)
72
73    imu_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
81    gps_sub = rospy.Subscriber("vn200_gps", vn_200_gps_soln, gps_cb)
82
83    print "made subs"
84
85    left = rospy.Publisher('left', Float32, queue_size=10)
86    right = rospy.Publisher('right', Float32, queue_size=10)
87
88    print "made pubs"
89
90    rate = rospy.Rate(10)  # number of times per second we run our loop
91    print "set rate"
92    while not rospy.is_shutdown():
93        print "please"
94        for next_waypoint in waypoints:
95
96
97            while get_distance(ourPos, next_waypoint) > distance_deadzone:
98
99                print get_distance(ourPos, next_waypoint)
100                currentbearing = compass_bearing(ourPos, next_waypoint)
101                dif1 = abs(ourDir - currentbearing)
102                dif2 = ourDir - currentbearing
103                print "dif:", dif1, "bear:", currentbearing, "ourDir:", ourDir
104                    while dif1 > 0
105                        prop = (dif1)/(50)
106                        intgral = sum(tracker_array)
107                        if dif1 < 180:
108                            left.publish(turnspeed +((.2)*prop)) + (.1)*integral
109                            right.publish(turnspeed)
110                            tracker_array.popleft()
111                            tracker_array.append(dif2)
112                        else:
113                            left.publish(turnspeed)
114                            right.publish(turnspeed + ((.2)*prop)) + (.1)*integral
115                            tracker_array.popleft()
116                            tracker_array.append(dif2)
117                
118                        else:
119                            rate.sleep()
120
121
122if __name__ == '__main__':
123    try:
124        print "test stuff"
125        driver()
126    except rospy.ROSInterruptException:
127        pass
128
129
130
131
132
133