collab/igvc2018

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

rickyc106Merge branch 'imu' of https://github.com/ut-ras/IGVC2018 into imu035bd4e

imu
10.5 KiB338 linesraw
1#!/usr/bin/env python
2#
3#driver for VN-200 IMU
4#datasheet:
5#Author: Cruz Monrreal II
6
7#Edited vn_200_gps_soln.msg file: Position accuracy estimate not acceleration
8
9#Edited node.py: Ins status read from 3rd element not 2nd. (Shifts every index down one)
10#If above doesn't work try shifting back one
11
12#Edited queue size for publishers. ROS requires this.
13
14#you MUST import vector3 and header packages from corresponding ros messages
15#ROS init node must take in this file's name as the name parameter. Use anonymous = True to allow for duplicates
16
17#--- If ctrl + c does not quit use ctrl + shift + \
18
19from vn200.msg import vn_200_accel_gyro_compass, vn_200_gps_soln, vn_200_ins_soln
20from geometry_msgs.msg import Vector3
21from std_msgs.msg import Header, Float32MultiArray
22import rospy
23import serial
24import time
25
26DO_SCALING = True
27
28IMU_MSG_LEN = 12
29GPS_SOLN_MSG_LEN = 16
30INS_SOL_MSG_LEN = 16
31MAG_COMP_MSG_LEN = 13
32
33ser = serial.Serial(port = '/dev/ttyUSB0', baudrate=921600)
34
35imu_pub = rospy.Publisher('vn200_accel_gyro_compass', vn_200_accel_gyro_compass, queue_size = 10)
36gps_pub = rospy.Publisher('vn200_gps', vn_200_gps_soln, queue_size = 10)
37ins_pub = rospy.Publisher('vn200_ins', vn_200_ins_soln, queue_size = 10)
38mag_comp_c_pub = rospy.Publisher('vn200_mag_comp_c_matrix', Float32MultiArray, queue_size = 10)
39mag_comp_b_pub = rospy.Publisher('vn200_mag_comp_b_matrix', Float32MultiArray, queue_size = 10)
40
41def read_data() :
42
43    data = "$"
44
45    while not rospy.is_shutdown():
46        c = ser.read(1)
47        if c == '$':
48            break
49
50    while not rospy.is_shutdown():
51        c = ser.read(1)
52        if c == '\n':
53            break
54        elif c == '\x00':
55            continue
56        data += c
57
58    return data
59
60def validate_checksum(msg):
61    try:
62        xor = 0
63        msg = msg.strip()
64        #rospy.loginfo("~~~~~~~" + msg + "~~~~~~");
65        #rospy.loginfo("~~~~~~~" + str(msg[-2]) + str(msg[-1])+ "!~~~~~~~~");
66        chksum = int(str(msg[-2]) + str(msg[-1]), 16)
67        #chksum = int(str(msg[-3]) +str([-2]), 16)
68        data = msg[1:-3].upper()
69
70        for char in data:
71            xor = xor ^ ord(char)
72        #rospy.loginfo(">>>>>" + str(xor) + ">>>>>>>>" + str(chksum))
73        return xor == chksum
74    except ValueError:
75        return False
76
77def cmd(string):
78    xor = 0
79
80    for char in string.upper():
81        xor = xor ^ ord(char)
82
83    return '$' + string + '*' + hex(xor & 0xFF)[2:4].upper() + '\n'
84
85READ_CMDS = [cmd("VNRRG,54"), # read IMU data from the register
86             cmd("VNRRG,58"), # read GPS soln from the register
87             cmd("VNRRG,63"), # read INS soln from the register
88             cmd("VNRRG,23")] # read mag compensation from the register
89
90def strip_tag_and_checksum (data):
91    # remove the $VNRRG tag in front of the message and the checksum behind the message
92    return data[7:-4].split(',')
93
94def publish_imu_data(imu_data):
95    global imu_pub
96
97    imu_msg = vn_200_accel_gyro_compass()
98    imu_msg.header.stamp = rospy.get_rostime()
99
100    imu_msg.compass.x = float(imu_data[1])
101    imu_msg.compass.y = float(imu_data[2])
102    imu_msg.compass.z = float(imu_data[3])
103
104    imu_msg.accelerometer.x = float(imu_data[4])
105    imu_msg.accelerometer.y = float(imu_data[5])
106    imu_msg.accelerometer.z = float(imu_data[6])
107
108    imu_msg.gyro.x = float(imu_data[7])
109    imu_msg.gyro.y = float(imu_data[8])
110    imu_msg.gyro.z = float(imu_data[9])
111
112    imu_pub.publish(imu_msg)
113
114def publish_gps_data(gps_data):
115    global gps_pub
116
117    gps_msg = vn_200_gps_soln()
118    gps_msg.header.stamp = rospy.get_rostime()
119    gps_msg.err_string = ""
120    gps_msg.error_present = False
121
122    gps_fix = int(gps_data[3])
123
124    if gps_fix == 0:
125        rospy.logwarn("GPS: No GPS Fix")
126        gps_msg.error_present = True
127        gps_msg.err_string += "No GPS fix. \n"
128
129    elif gps_fix == 1:
130        rospy.logwarn("GPS: Time only in GPS soln")
131        gps_msg.error_present = True
132        gps_msg.err_string += "Time only in GPS soln \n"
133    elif gps_fix == 2:
134        rospy.logwarn("GPS: 2D only. Horizontal Position may be affected")
135
136    gps_msg.num_satellites = float(gps_data[4])
137
138    if gps_msg.num_satellites < 3:
139        rospy.logwarn("Less than 3 satellites found. Try moving to an open area")
140        gps_msg.error_present = True
141        gps_msg.err_string += "Less than 3 satellites found \n"
142
143    gps_msg.latitude = float(gps_data[5])
144    gps_msg.longitude = float(gps_data[6])
145    gps_msg.altitude = float(gps_data[7])
146
147    gps_msg.ned_velocities.x = float(gps_data[8])
148    gps_msg.ned_velocities.y = float(gps_data[9])
149    gps_msg.ned_velocities.z = float(gps_data[10])
150
151    gps_msg.ned_accuracy.x = float(gps_data[11])
152    gps_msg.ned_accuracy.y = float(gps_data[12])
153    gps_msg.ned_accuracy.z = float(gps_data[13])
154
155    gps_msg.speed_accuracy_estimate = float(gps_data[14])
156
157    gps_pub.publish(gps_msg)
158
159def publish_ins_data(ins_data):
160    global ins_pub
161
162    ins_msg = vn_200_ins_soln()
163    ins_msg.header.stamp = rospy.get_rostime()
164    ins_msg.error_present = False
165    ins_msg.err_string = ""
166
167    ins_status = int(ins_data[3], 16) & 0xFFFF
168
169    ins_mode = ins_status & 0x0003
170    ins_gps_fix = ins_status & 0x0004
171    ins_error = ins_status & 0x0078
172
173    if ins_mode == 0:
174        rospy.logwarn("INS: Not tracking. Insufficient dynamic motion to esimtate attitude.")
175        ins_msg.error_present = True
176        ins_msg.err_string += "Insufficient dynamic motion. \n"
177
178    elif ins_mode == 1:
179        rospy.logwarn("INS: Sufficient dynamic motion, but solution not within performance specs.")
180        ins_msg.error_present = True
181        ins_msg.err_string += "Sufficient dynamic motion, but solution not within performance specs. \n"
182
183    if ins_gps_fix != 1:
184        rospy.logwarn("INS: No GPS fix.")
185        ins_msg.error_present = True
186        ins_msg.err_string += "No GPS fix found. \n"
187
188    if ins_error == 1:
189        rospy.logwarn("INS: Filter loop exceeds 5 ms!")
190        ins_msg.error_present = True
191        ins_msg.err_string += "INS Filter loop exceds 5 ms. \n"
192
193    elif ins_error == 2:
194        rospy.logwarn("INS: IMU communication error detected. Try resetting.")
195        ins_msg.error_present = True
196        ins_msg.err_string += "IMU communcation error detected. \n"
197
198    elif ins_error == 4:
199        rospy.logwarn("INS: Magnetometer or Pressure Sensor Error Detected.")
200        ins_msg.error_present = True
201        ins_msg.err_string += "Magnetometer or pressure sensor error detected. \n"
202
203    elif ins_error == 8:
204        rospy.logwarn("INS: GPS communication error detected.")
205        ins_msg.error_present = True
206        ins_msg.err_string += "GPS communication error detected. \n"
207
208    ins_msg.yaw_heading = float(ins_data[4])
209    ins_msg.pitch = float(ins_data[5])
210    ins_msg.roll = float(ins_data[6])
211
212    ins_msg.geodetic_latitude = float(ins_data[7])
213    ins_msg.geodetic_longitude = float(ins_data[8])
214    ins_msg.altitude = float(ins_data[9])
215
216    ins_msg.ned_velocities.x = float(ins_data[10])
217    ins_msg.ned_velocities.y = float(ins_data[11])
218    ins_msg.ned_velocities.z = float(ins_data[12])
219
220    ins_msg.attitude_uncertainty = float(ins_data[13])
221    ins_msg.position_uncertainty = float(ins_data[14])
222    ins_msg.velocity_uncertainty = float(ins_data[15])
223
224    ins_pub.publish(ins_msg)
225
226def publish_mag_comp_data(mag_comp_data):
227    global mag_comp_pub
228
229    c_matrix = Float32MultiArray()
230    b_matrix = Float32MultiArray()
231
232    for i in range(9):
233    	c_matrix.data.append(float(mag_comp_data[i+1]))
234    for i in range(3):
235    	b_matrix.data.append(float(mag_comp_data[i+10]))
236
237    mag_comp_c_pub.publish(c_matrix)
238    mag_comp_b_pub.publish(b_matrix)
239
240def process_and_publish(data):
241
242    global GPS_SOLN_MSG_LEN
243    global INS_SOL_MSG_LEN
244    global IMU_MSG_LEN
245
246    if data[7:9] == "54" and data[1:6] == "VNRRG":#data[1:6] == "VNIMU":
247        imu_data = strip_tag_and_checksum(data)
248        # discard the message of if it does not have all the necessaty fields
249        if len(imu_data) is not IMU_MSG_LEN:
250            rospy.logwarn("ERROR reading IMU message")
251            return
252        publish_imu_data(imu_data)
253
254    elif data[7:9] == "58" and data[1:6] == "VNRRG":#data[1:6] == "VNGPE"
255        gps_data = strip_tag_and_checksum(data)
256        if len(gps_data) is not GPS_SOLN_MSG_LEN:
257            rospy.logwarn("ERROR reading GPS message")
258            return
259        publish_gps_data(gps_data)
260        #return gps_data[5:6]
261        #print(gps_data)
262
263    elif  data[7:9] == "63" and data[1:6] == "VNRRG":#data[1:6] == "VNINS":
264        ins_data = data[7:-4].split(',')
265        if len(ins_data) is not INS_SOL_MSG_LEN:
266           rospy.logwarn("ERROR reading INS message")
267           return
268        #rospy.loginfo('!!!!INS' + data)
269        publish_ins_data(ins_data)
270
271    elif  data[7:9] == "23" and data[1:6] == "VNRRG":#data[1:6] == "VNMAGCOMP":
272        mag_compensation_data = data[7:-4].split(',')
273        if len(mag_compensation_data) is not MAG_COMP_MSG_LEN:
274           rospy.logwarn("ERROR reading Mag compensation message")
275           return
276        publish_mag_comp_data(mag_compensation_data)
277
278    else:
279        rospy.loginfo("Unknown message: " + str(data) + '\n')
280
281def vn200_main():
282
283    global READ_CMDS
284    global ser
285
286    rospy.init_node('vn_200_node')
287    rospy.loginfo("VN-200 IMU listener is running on " + ser.portstr )
288
289    ser.flush()
290    ser.close()
291    ser.open()
292
293    ser.write(cmd("VNWRG,05,921600"))
294    ser.write(cmd("VNWRG,07,50"))
295    ser.write(cmd("VNWRG,06,0"))
296    #ser.write(cmd("VNWRG,06,247"))
297
298    r = rospy.Rate(20)
299
300    while not rospy.is_shutdown():
301
302        for cmds in READ_CMDS:
303            ser.write(cmds)
304
305        # read 3 responses
306        for i in range(len(READ_CMDS)):
307            # read the data from the serial port
308            data = read_data()
309
310            '''
311            if validate_checksum(data):
312                # check if data was an IMU, INS SOLN or GPS SOLN reply,
313                # process accordingly and then publish the appropriate message
314                try:
315                    process_and_publish(data)
316                    #print(imu_data)
317                    #print(gps_data)
318                    #print(ins_data)
319                except:
320                    rospy.logwarn("vn_200_imu data dropped")
321            else:
322                rospy.logwarn("Checksum incorrect for %s. Dropping packet", data)
323            '''
324            process_and_publish(data)
325
326            '''
327            if data[7:9] == "54" and data[1:6] == "VNRRG":#data[1:6] == "VNIMU":
328                imu_data = strip_tag_and_checksum(data)
329                print(imu_data[4])
330            '''
331
332            r.sleep()
333
334if __name__ == "__main__":
335    try:
336        vn200_main()
337    except rospy.ROSInterruptException:
338        pass