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