00001
00002 import roslib; roslib.load_manifest('xsens_driver')
00003 import rospy
00004 import select
00005
00006 import mtdevice
00007
00008 from std_msgs.msg import Header, Float32, String, UInt16
00009 from sensor_msgs.msg import Imu, NavSatFix, NavSatStatus
00010 from geometry_msgs.msg import TwistStamped, Vector3Stamped
00011 from gps_common.msg import GPSFix, GPSStatus
00012 from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
00013 import time
00014
00015
00016
00017 from math import pi, radians
00018 from tf.transformations import quaternion_from_matrix, quaternion_from_euler, identity_matrix
00019
00020 def get_param(name, default):
00021 try:
00022 v = rospy.get_param(name)
00023 rospy.loginfo("Found parameter: %s, value: %s"%(name, str(v)))
00024 except KeyError:
00025 v = default
00026 rospy.logwarn("Cannot find value for parameter: %s, assigning "
00027 "default: %s"%(name, str(v)))
00028 return v
00029
00030 class XSensDriver(object):
00031
00032 def __init__(self):
00033
00034 device = get_param('~device', 'auto')
00035 baudrate = get_param('~baudrate', 0)
00036 if device=='auto':
00037 devs = mtdevice.find_devices()
00038 if devs:
00039 device, baudrate = devs[0]
00040 rospy.loginfo("Detected MT device on port %s @ %d bps"%(device,
00041 baudrate))
00042 else:
00043 rospy.logerr("Fatal: could not find proper MT device.")
00044 rospy.signal_shutdown("Could not find proper MT device.")
00045 return
00046 if not baudrate:
00047 baudrate = mtdevice.find_baudrate(device)
00048 if not baudrate:
00049 rospy.logerr("Fatal: could not find proper baudrate.")
00050 rospy.signal_shutdown("Could not find proper baudrate.")
00051 return
00052
00053 rospy.loginfo("MT node interface: %s at %d bd."%(device, baudrate))
00054 self.mt = mtdevice.MTDevice(device, baudrate)
00055
00056 self.frame_id = get_param('~frame_id', '/base_imu')
00057
00058 self.diag_pub = rospy.Publisher('/diagnostics', DiagnosticArray)
00059 self.diag_msg = DiagnosticArray()
00060 self.stest_stat = DiagnosticStatus(name='mtnode: Self Test', level=1,
00061 message='No status information')
00062 self.xkf_stat = DiagnosticStatus(name='mtnode: XKF Valid', level=1,
00063 message='No status information')
00064 self.gps_stat = DiagnosticStatus(name='mtnode: GPS Fix', level=1,
00065 message='No status information')
00066 self.diag_msg.status = [self.stest_stat, self.xkf_stat, self.gps_stat]
00067
00068 self.imu_pub = rospy.Publisher('imu/data', Imu)
00069 self.gps_pub = rospy.Publisher('fix', NavSatFix)
00070 self.xgps_pub = rospy.Publisher('fix_extended', GPSFix)
00071 self.vel_pub = rospy.Publisher('velocity', TwistStamped)
00072 self.mag_pub = rospy.Publisher('magnetic', Vector3Stamped)
00073 self.temp_pub = rospy.Publisher('temperature', Float32)
00074 self.press_pub = rospy.Publisher('pressure', Float32)
00075 self.analog_in1_pub = rospy.Publisher('analog_in1', UInt16)
00076 self.analog_in2_pub = rospy.Publisher('analog_in2', UInt16)
00077
00078 self.old_bGPS = 256
00079
00080
00081 self.str_pub = rospy.Publisher('imu_data_str', String)
00082
00083
00084
00085 def spin(self):
00086 try:
00087 while not rospy.is_shutdown():
00088 self.spin_once()
00089
00090
00091 except select.error:
00092 pass
00093
00094 def spin_once(self):
00095 '''Read data from device and publishes ROS messages.'''
00096
00097 h = Header()
00098 h.stamp = rospy.Time.now()
00099 h.frame_id = self.frame_id
00100
00101
00102 imu_msg = Imu()
00103 imu_msg.orientation_covariance = (-1., )*9
00104 imu_msg.angular_velocity_covariance = (-1., )*9
00105 imu_msg.linear_acceleration_covariance = (-1., )*9
00106 pub_imu = False
00107 gps_msg = NavSatFix()
00108 xgps_msg = GPSFix()
00109 pub_gps = False
00110 vel_msg = TwistStamped()
00111 pub_vel = False
00112 mag_msg = Vector3Stamped()
00113 pub_mag = False
00114 temp_msg = Float32()
00115 pub_temp = False
00116 press_msg = Float32()
00117 pub_press = False
00118 anin1_msg = UInt16()
00119 pub_anin1 = False
00120 anin2_msg = UInt16()
00121 pub_anin2 = False
00122 pub_diag = False
00123
00124 def fill_from_raw(raw_data):
00125 '''Fill messages with information from 'raw' MTData block.'''
00126
00127
00128 pass
00129
00130 def fill_from_rawgps(rawgps_data):
00131 '''Fill messages with information from 'rawgps' MTData block.'''
00132 global pub_hps, xgps_msg, gps_msg
00133 if rawgps_data['bGPS']<self.old_bGPS:
00134 pub_gps = True
00135
00136 xgps_msg.latitude = gps_msg.latitude = rawgps_data['LAT']*1e-7
00137 xgps_msg.longitude = gps_msg.longitude = rawgps_data['LON']*1e-7
00138 xgps_msg.altitude = gps_msg.altitude = rawgps_data['ALT']*1e-3
00139
00140
00141
00142 xgps_msg.err_horz = 2*rawgps_data['Hacc']*1e-3
00143 xgps_msg.err_vert = 2*rawgps_data['Vacc']*1e-3
00144 self.old_bGPS = rawgps_data['bGPS']
00145
00146 def fill_from_Temp(temp):
00147 '''Fill messages with information from 'temperature' MTData block.'''
00148 global pub_temp, temp_msg
00149 pub_temp = True
00150 temp_msg.data = temp
00151
00152 def fill_from_Calib(imu_data):
00153 '''Fill messages with information from 'calibrated' MTData block.'''
00154 global pub_imu, imu_msg, pub_vel, vel_msg, pub_mag, mag_msg
00155 try:
00156 pub_imu = True
00157 imu_msg.angular_velocity.x = imu_data['gyrX']
00158 imu_msg.angular_velocity.y = imu_data['gyrY']
00159 imu_msg.angular_velocity.z = imu_data['gyrZ']
00160 imu_msg.angular_velocity_covariance = (radians(0.025), 0., 0., 0.,
00161 radians(0.025), 0., 0., 0., radians(0.025))
00162 pub_vel = True
00163 vel_msg.twist.angular.x = imu_data['gyrX']
00164 vel_msg.twist.angular.y = imu_data['gyrY']
00165 vel_msg.twist.angular.z = imu_data['gyrZ']
00166 except KeyError:
00167 pass
00168 try:
00169 pub_imu = True
00170 imu_msg.linear_acceleration.x = imu_data['accX']
00171 imu_msg.linear_acceleration.y = imu_data['accY']
00172 imu_msg.linear_acceleration.z = imu_data['accZ']
00173 imu_msg.linear_acceleration_covariance = (0.0004, 0., 0., 0.,
00174 0.0004, 0., 0., 0., 0.0004)
00175 except KeyError:
00176 pass
00177 try:
00178 pub_mag = True
00179 mag_msg.vector.x = imu_data['magX']
00180 mag_msg.vector.y = imu_data['magY']
00181 mag_msg.vector.z = imu_data['magZ']
00182 except KeyError:
00183 pass
00184
00185 def fill_from_Vel(velocity_data):
00186 '''Fill messages with information from 'velocity' MTData block.'''
00187 global pub_vel, vel_msg
00188 pub_vel = True
00189 vel_msg.twist.linear.x = velocity_data['Vel_X']
00190 vel_msg.twist.linear.y = velocity_data['Vel_Y']
00191 vel_msg.twist.linear.z = velocity_data['Vel_Z']
00192
00193 def fill_from_Orient(orient_data):
00194 '''Fill messages with information from 'orientation' MTData block.'''
00195 global pub_imu, imu_msg
00196 pub_imu = True
00197 if orient.has_key('quaternion'):
00198 w, x, y, z = orient['quaternion']
00199 elif orient.has_key('roll'):
00200
00201 x, y, z, w = quaternion_from_euler(radians(orient['roll']),
00202 radians(orient['pitch']), radians(orient['yaw']))
00203 elif orient.has_key('matrix'):
00204 m = identity_matrix()
00205 m[:3,:3] = orient['matrix']
00206 x, y, z, w = quaternion_from_matrix(m)
00207 imu_msg.orientation.x = x
00208 imu_msg.orientation.y = y
00209 imu_msg.orientation.z = z
00210 imu_msg.orientation.w = w
00211 imu_msg.orientation_covariance = (radians(1.), 0., 0., 0.,
00212 radians(1.), 0., 0., 0., radians(9.))
00213
00214 def fill_from_Pos(position_data):
00215 '''Fill messages with information from 'position' MTData block.'''
00216 global pub_gps, xgps_msg, gps_msg
00217 pub_gps = True
00218 xgps_msg.latitude = gps_msg.latitude = position_data['Lat']
00219 xgps_msg.longitude = gps_msg.longitude = position_data['Lon']
00220 xgps_msg.altitude = gps_msg.altitude = position_data['Alt']
00221
00222 def fill_from_Stat(status):
00223 '''Fill messages with information from 'status' MTData block.'''
00224 global pub_diag, pub_gps, gps_msg, xgps_msg
00225 pub_diag = True
00226 if status & 0b0001:
00227 self.stest_stat.level = DiagnosticStatus.OK
00228 self.stest_stat.message = "Ok"
00229 else:
00230 self.stest_stat.level = DiagnosticStatus.ERROR
00231 self.stest_stat.message = "Failed"
00232 if status & 0b0010:
00233 self.xkf_stat.level = DiagnosticStatus.OK
00234 self.xkf_stat.message = "Valid"
00235 else:
00236 self.xkf_stat.level = DiagnosticStatus.WARN
00237 self.xkf_stat.message = "Invalid"
00238 if status & 0b0100:
00239 self.gps_stat.level = DiagnosticStatus.OK
00240 self.gps_stat.message = "Ok"
00241 gps_msg.status.status = NavSatStatus.STATUS_FIX
00242 xgps_msg.status.status = GPSStatus.STATUS_FIX
00243 gps_msg.status.service = NavSatStatus.SERVICE_GPS
00244 xgps_msg.status.position_source = 0b01101001
00245 xgps_msg.status.motion_source = 0b01101010
00246 xgps_msg.status.orientation_source = 0b01101010
00247 else:
00248 self.gps_stat.level = DiagnosticStatus.WARN
00249 self.gps_stat.message = "No fix"
00250 gps_msg.status.status = NavSatStatus.STATUS_NO_FIX
00251 xgps_msg.status.status = GPSStatus.STATUS_NO_FIX
00252 gps_msg.status.service = 0
00253 xgps_msg.status.position_source = 0b01101000
00254 xgps_msg.status.motion_source = 0b01101000
00255 xgps_msg.status.orientation_source = 0b01101000
00256
00257 def fill_from_Auxiliary(aux_data):
00258 '''Fill messages with information from 'Auxiliary' MTData block.'''
00259 global pub_anin1, pub_anin2, anin1_msg, anin2_msg
00260 try:
00261 anin1_msg.data = o['Ain_1']
00262 pub_anin1 = True
00263 except KeyError:
00264 pass
00265 try:
00266 anin2_msg.data = o['Ain_2']
00267 pub_anin2 = True
00268 except KeyError:
00269 pass
00270
00271 def fill_from_Temperature(o):
00272 '''Fill messages with information from 'Temperature' MTData2 block.'''
00273 global pub_temp, temp_msg
00274 pub_temp = True
00275 temp_msg.data = o['Temp']
00276
00277 def fill_from_Timestamp(o):
00278 '''Fill messages with information from 'Timestamp' MTData2 block.'''
00279 global h
00280 try:
00281
00282 y, m, d, hr, mi, s, ns, f = o['Year'], o['Month'], o['Day'],\
00283 o['Hour'], o['Minute'], o['Second'], o['ns'], o['Flags']
00284 if f&0x4:
00285 secs = time.mktime((y, m, d, hr, mi, s, 0, 0, 0))
00286 h.stamp.secs = secs
00287 h.stamp.nsecs = ns
00288 except KeyError:
00289 pass
00290
00291 pass
00292
00293 def fill_from_Orientation_Data(o):
00294 '''Fill messages with information from 'Orientation Data' MTData2 block.'''
00295 global pub_imu, imu_msg
00296 pub_imu = True
00297 try:
00298 x, y, z, w = o['Q1'], o['Q2'], o['Q3'], o['Q0']
00299 except KeyError:
00300 pass
00301 try:
00302
00303 x, y, z, w = quaternion_from_euler(radians(o['Roll']),
00304 radians(o['Pitch']), radians(o['Yaw']))
00305 except KeyError:
00306 pass
00307 try:
00308 a, b, c, d, e, f, g, h, i = o['a'], o['b'], o['c'], o['d'],\
00309 o['e'], o['f'], o['g'], o['h'], o['i']
00310 m = identity_matrix()
00311 m[:3,:3] = ((a, b, c), (d, e, f), (g, h, i))
00312 x, y, z, w = quaternion_from_matrix(m)
00313 except KeyError:
00314 pass
00315 imu_msg.orientation.x = x
00316 imu_msg.orientation.y = y
00317 imu_msg.orientation.z = z
00318 imu_msg.orientation.w = w
00319 imu_msg.orientation_covariance = (radians(1.), 0., 0., 0.,
00320 radians(1.), 0., 0., 0., radians(9.))
00321
00322 def fill_from_Pressure(o):
00323 '''Fill messages with information from 'Pressure' MTData2 block.'''
00324 global pub_press, press_msg
00325 press_msg.data = o['Pressure']
00326
00327 def fill_from_Acceleration(o):
00328 '''Fill messages with information from 'Acceleration' MTData2 block.'''
00329 global pub_imu, imu_msg
00330 pub_imu = True
00331
00332 try:
00333 x, y, z = o['Delta v.x'], o['Delta v.y'], o['Delta v.z']
00334 except KeyError:
00335 pass
00336 try:
00337 x, y, z = o['freeAccX'], o['freeAccY'], o['freeAccZ']
00338 except KeyError:
00339 pass
00340 try:
00341 x, y, z = o['accX'], o['accY'], o['accZ']
00342 except KeyError:
00343 pass
00344 imu_msg.linear_acceleration.x = x
00345 imu_msg.linear_acceleration.y = y
00346 imu_msg.linear_acceleration.z = z
00347 imu_msg.linear_acceleration_covariance = (0.0004, 0., 0., 0.,
00348 0.0004, 0., 0., 0., 0.0004)
00349
00350 def fill_from_Position(o):
00351 '''Fill messages with information from 'Position' MTData2 block.'''
00352 global pub_gps, xgps_msg, gps_msg
00353
00354 try:
00355 xgps_msg.latitude = gps_msg.latitude = o['lat']
00356 xgps_msg.longitude = gps_msg.longitude = o['lon']
00357 pub_gps = True
00358 alt = o.get('altMsl', o.get('altEllipsoid', 0))
00359 xgps_msg.altitude = gps_msg.altitude = alt
00360 except KeyError:
00361 pass
00362
00363 def fill_from_Angular_Velocity(o):
00364 '''Fill messages with information from 'Angular Velocity' MTData2 block.'''
00365 global pub_imu, imu_msg, pub_vel, vel_msg
00366 try:
00367 imu_msg.angular_velocity.x = o['gyrX']
00368 imu_msg.angular_velocity.y = o['gyrY']
00369 imu_msg.angular_velocity.z = o['gyrZ']
00370 imu_msg.angular_velocity_covariance = (radians(0.025), 0., 0., 0.,
00371 radians(0.025), 0., 0., 0., radians(0.025))
00372 pub_imu = True
00373 vel_msg.twist.angular.x = o['gyrX']
00374 vel_msg.twist.angular.y = o['gyrY']
00375 vel_msg.twist.angular.z = o['gyrZ']
00376 pub_vel = True
00377 except KeyError:
00378 pass
00379
00380
00381 def fill_from_GPS(o):
00382 '''Fill messages with information from 'GPS' MTData2 block.'''
00383 global pub_gps, h, xgps_msg
00384 try:
00385 xgps_msg.gdop = o['gDOP']
00386 xgps_msg.pdop = o['pDOP']
00387 xgps_msg.hdop = o['hDOP']
00388 xgps_msg.vdop = o['vDOP']
00389 xgps_msg.tdop = o['tDOP']
00390 pub_gps = True
00391 except KeyError:
00392 pass
00393 try:
00394 y, m, d, hr, mi, s, ns, f = o['year'], o['month'], o['day'],\
00395 o['hour'], o['min'], o['sec'], o['nano'], o['valid']
00396 if f&0x4:
00397 secs = time.mktime((y, m, d, hr, mi, s, 0, 0, 0))
00398 h.stamp.secs = secs
00399 h.stamp.nsecs = ns
00400 except KeyError:
00401 pass
00402
00403
00404 def fill_from_SCR(o):
00405 '''Fill messages with information from 'SCR' MTData2 block.'''
00406
00407 pass
00408
00409 def fill_from_Analog_In(o):
00410 '''Fill messages with information from 'Analog In' MTData2 block.'''
00411 global pub_anin1, pub_anin2, anin1_msg, anin2_msg
00412 try:
00413 anin1_msg.data = o['analogIn1']
00414 pub_anin1 = True
00415 except KeyError:
00416 pass
00417 try:
00418 anin2_msg.data = o['analogIn2']
00419 pub_anin2 = True
00420 except KeyError:
00421 pass
00422
00423 def fill_from_Magnetic(o):
00424 '''Fill messages with information from 'Magnetic' MTData2 block.'''
00425 global pub_mag, mag_msg
00426 mag_msg.vector.x = o['magX']
00427 mag_msg.vector.y = o['magY']
00428 mag_msg.vector.z = o['magZ']
00429 pub_mag = True
00430
00431 def fill_from_Velocity(o):
00432 '''Fill messages with information from 'Velocity' MTData2 block.'''
00433 global pub_vel, vel_msg
00434 vel_msg.twist.linear.x = o['velX']
00435 vel_msg.twist.linear.y = o['velY']
00436 vel_msg.twist.linear.z = o['velZ']
00437 pub_vel = True
00438
00439 def fill_from_Status(o):
00440 '''Fill messages with information from 'Status' MTData2 block.'''
00441 try:
00442 status = o['StatusByte']
00443 fill_from_Stat(status)
00444 except KeyError:
00445 pass
00446 try:
00447 status = o['StatusWord']
00448 fill_from_Stat(status)
00449 except KeyError:
00450 pass
00451
00452
00453 def find_handler_name(name):
00454 return "fill_from_%s"%(name.replace(" ", "_"))
00455
00456
00457 data = self.mt.read_measurement()
00458
00459 for n, o in data:
00460 try:
00461 locals()[find_handler_name(n)](o)
00462 except KeyError:
00463 rospy.logwarn("Unknown MTi data packet: '%s', ignoring."%n)
00464
00465
00466 if pub_imu:
00467 imu_msg.header = h
00468 self.imu_pub.publish(imu_msg)
00469 if pub_gps:
00470 xgps_msg.header = gps_msg.header = h
00471 self.gps_pub.publish(gps_msg)
00472 self.xgps_pub.publish(xgps_msg)
00473 if pub_vel:
00474 vel_msg.header = h
00475 self.vel_pub.publish(vel_msg)
00476 if pub_mag:
00477 mag_msg.header = h
00478 self.mag_pub.publish(mag_msg)
00479 if pub_temp:
00480 self.temp_pub.publish(temp_msg)
00481 if pub_press:
00482 self.press_pub.publish(press_msg)
00483 if pub_anin1:
00484 self.analog_in1_pub.publish(anin1_msg)
00485 if pub_anin2:
00486 self.analog_in2_pub.publish(anin2_msg)
00487 if pub_diag:
00488 self.diag_msg.header = h
00489 self.diag_pub.publish(self.diag_msg)
00490
00491 self.str_pub.publish(str(data))
00492
00493
00494
00495 def main():
00496 '''Create a ROS node and instantiate the class.'''
00497 rospy.init_node('xsens_driver')
00498 driver = XSensDriver()
00499 driver.spin()
00500
00501
00502 if __name__== '__main__':
00503 main()
00504
00505