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