ethzasl_xsens_driver: mtnode.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
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
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 
00014 
00015 # transform Euler angles or matrix into quaternions
00016 from math import pi, radians
00017 from tf.transformations import quaternion_from_matrix, quaternion_from_euler, identity_matrix
00018 
00019 import numpy
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         ENU = numpy.identity(3)
00034         NED = numpy.array([[0, 1, 0], [ 1, 0, 0], [0, 0, -1]])
00035         NWU = numpy.array([[0, 1, 0], [-1, 0, 0], [0, 0,  1]])
00036 
00037         def __init__(self):
00038 
00039                 device = get_param('~device', 'auto')
00040                 baudrate = get_param('~baudrate', 0)
00041                 if device=='auto':
00042                         devs = mtdevice.find_devices()
00043                         if devs:
00044                                 device, baudrate = devs[0]
00045                                 rospy.loginfo("Detected MT device on port %s @ %d bps"%(device,
00046                                                 baudrate))
00047                         else:
00048                                 rospy.logerr("Fatal: could not find proper MT device.")
00049                                 rospy.signal_shutdown("Could not find proper MT device.")
00050                                 return
00051                 if not baudrate:
00052                         baudrate = mtdevice.find_baudrate(device)
00053                 if not baudrate:
00054                         rospy.logerr("Fatal: could not find proper baudrate.")
00055                         rospy.signal_shutdown("Could not find proper baudrate.")
00056                         return
00057 
00058                 rospy.loginfo("MT node interface: %s at %d bd."%(device, baudrate))
00059                 self.mt = mtdevice.MTDevice(device, baudrate)
00060 
00061                 self.frame_id = get_param('~frame_id', '/base_imu')
00062 
00063                 frame_local     = get_param('~frame_local'    , 'ENU')
00064                 frame_local_imu = get_param('~frame_local_imu', 'ENU')
00065 
00066                 if   frame_local == 'ENU':
00067                         R = XSensDriver.ENU
00068                 elif frame_local == 'NED':
00069                         R = XSensDriver.NED
00070                 elif frame_local == 'NWU':
00071                         R = XSensDriver.NWU
00072 
00073                 if   frame_local_imu == 'ENU':
00074                         R_IMU = XSensDriver.ENU
00075                 elif frame_local_imu == 'NED':
00076                         R_IMU = XSensDriver.NED
00077                 elif frame_local_imu == 'NWU':
00078                         R_IMU = XSensDriver.NWU
00079 
00080                 self.R = R.dot(R_IMU.transpose())
00081 
00082                 self.diag_pub = rospy.Publisher('/diagnostics', DiagnosticArray)
00083                 self.diag_msg = DiagnosticArray()
00084                 self.stest_stat = DiagnosticStatus(name='mtnode: Self Test', level=1,
00085                                 message='No status information')
00086                 self.xkf_stat = DiagnosticStatus(name='mtnode: XKF Valid', level=1,
00087                                 message='No status information')
00088                 self.gps_stat = DiagnosticStatus(name='mtnode: GPS Fix', level=1,
00089                                 message='No status information')
00090                 self.diag_msg.status = [self.stest_stat, self.xkf_stat, self.gps_stat]
00091 
00092                 self.imu_pub = rospy.Publisher('imu/data', Imu)
00093                 self.gps_pub = rospy.Publisher('fix', NavSatFix)
00094                 self.xgps_pub = rospy.Publisher('fix_extended', GPSFix)
00095                 self.vel_pub = rospy.Publisher('velocity', TwistStamped)
00096                 self.mag_pub = rospy.Publisher('magnetic', Vector3Stamped)
00097                 self.temp_pub = rospy.Publisher('temperature', Float32) # decide type
00098                 # TODO pressure, ITOW from raw GPS?
00099                 self.old_bGPS = 256     # publish GPS only if new
00100 
00101 
00102 
00103         def spin(self):
00104                 try:
00105                         while not rospy.is_shutdown():
00106                                 self.spin_once()
00107                 # Ctrl-C signal interferes with select with the ROS signal handler
00108                 # should be OSError in python 3.?
00109                 except select.error:
00110                         pass
00111 
00112         def spin_once(self):
00113 
00114                 def quat_from_orient(orient):
00115                         '''Build a quaternion from orientation data.'''
00116                         try:
00117                                 w, x, y, z = orient['quaternion']
00118                                 return (x, y, z, w)
00119                         except KeyError:
00120                                 pass
00121                         try:
00122                                 return quaternion_from_euler(pi*orient['roll']/180.,
00123                                                 pi*orient['pitch']/180, pi*orient['yaw']/180.)
00124                         except KeyError:
00125                                 pass
00126                         try:
00127                                 m = identity_matrix()
00128                                 m[:3,:3] = orient['matrix']
00129                                 return quaternion_from_matrix(m)
00130                         except KeyError:
00131                                 pass
00132 
00133                 # get data
00134                 data = self.mt.read_measurement()
00135                 # common header
00136                 h = Header()
00137                 h.stamp = rospy.Time.now()
00138                 h.frame_id = self.frame_id
00139 
00140                 # get data (None if not present)
00141                 temp = data.get('Temp') # float
00142                 raw_data = data.get('RAW')
00143                 imu_data = data.get('Calib')
00144                 orient_data = data.get('Orient')
00145                 velocity_data = data.get('Vel')
00146                 position_data = data.get('Pos')
00147                 rawgps_data = data.get('RAWGPS')
00148                 status = data.get('Stat')       # int
00149 
00150                 # create messages and default values
00151                 imu_msg = Imu()
00152                 imu_msg.orientation_covariance = (-1., )*9
00153                 imu_msg.angular_velocity_covariance = (-1., )*9
00154                 imu_msg.linear_acceleration_covariance = (-1., )*9
00155                 pub_imu = False
00156                 gps_msg = NavSatFix()
00157                 xgps_msg = GPSFix()
00158                 pub_gps = False
00159                 vel_msg = TwistStamped()
00160                 pub_vel = False
00161                 mag_msg = Vector3Stamped()
00162                 pub_mag = False
00163                 temp_msg = Float32()
00164                 pub_temp = False
00165 
00166                 # fill information where it's due
00167                 # start by raw information that can be overriden
00168                 if raw_data: # TODO warn about data not calibrated
00169                         pub_imu = True
00170                         pub_vel = True
00171                         pub_mag = True
00172                         pub_temp = True
00173                         # acceleration
00174                         imu_msg.linear_acceleration.x = raw_data['accX']
00175                         imu_msg.linear_acceleration.y = raw_data['accY']
00176                         imu_msg.linear_acceleration.z = raw_data['accZ']
00177                         imu_msg.linear_acceleration_covariance = (0., )*9
00178                         # gyroscopes
00179                         imu_msg.angular_velocity.x = raw_data['gyrX']
00180                         imu_msg.angular_velocity.y = raw_data['gyrY']
00181                         imu_msg.angular_velocity.z = raw_data['gyrZ']
00182                         imu_msg.angular_velocity_covariance = (0., )*9
00183                         vel_msg.twist.angular.x = raw_data['gyrX']
00184                         vel_msg.twist.angular.y = raw_data['gyrY']
00185                         vel_msg.twist.angular.z = raw_data['gyrZ']
00186                         # magnetometer
00187                         mag_msg.vector.x = raw_data['magX']
00188                         mag_msg.vector.y = raw_data['magY']
00189                         mag_msg.vector.z = raw_data['magZ']
00190                         # temperature
00191                         # 2-complement decoding and 1/256 resolution
00192                         x = raw_data['temp']
00193                         if x&0x8000:
00194                                 temp_msg.data = (x - 1<<16)/256.
00195                         else:
00196                                 temp_msg.data = x/256.
00197                 if rawgps_data:
00198                         if rawgps_data['bGPS']<self.old_bGPS:
00199                                 pub_gps = True
00200                                 # LLA
00201                                 xgps_msg.latitude = gps_msg.latitude = rawgps_data['LAT']*1e-7
00202                                 xgps_msg.longitude = gps_msg.longitude = rawgps_data['LON']*1e-7
00203                                 xgps_msg.altitude = gps_msg.altitude = rawgps_data['ALT']*1e-3
00204                                 # NED vel # TODO?
00205                                 # Accuracy
00206                                 # 2 is there to go from std_dev to 95% interval
00207                                 xgps_msg.err_horz = 2*rawgps_data['Hacc']*1e-3
00208                                 xgps_msg.err_vert = 2*rawgps_data['Vacc']*1e-3
00209                         self.old_bGPS = rawgps_data['bGPS']
00210                 if temp is not None:
00211                         pub_temp = True
00212                         temp_msg.data = temp
00213                 if imu_data:
00214                         try:
00215                                 x = imu_data['gyrX']
00216                                 y = imu_data['gyrY']
00217                                 z = imu_data['gyrZ']
00218 
00219                                 v = numpy.array([x, y, z])
00220                                 v = v.dot(self.R)
00221 
00222                                 imu_msg.angular_velocity.x = v[0]
00223                                 imu_msg.angular_velocity.y = v[1]
00224                                 imu_msg.angular_velocity.z = v[2]
00225                                 imu_msg.angular_velocity_covariance = (radians(0.025), 0., 0., 0.,
00226                                                 radians(0.025), 0., 0., 0., radians(0.025))
00227                                 pub_imu = True
00228                                 vel_msg.twist.angular.x = v[0]
00229                                 vel_msg.twist.angular.y = v[1]
00230                                 vel_msg.twist.angular.z = v[2]
00231                                 pub_vel = True
00232                         except KeyError:
00233                                 pass
00234                         try:
00235                                 x = imu_data['accX']
00236                                 y = imu_data['accY']
00237                                 z = imu_data['accZ']
00238 
00239                                 v = numpy.array([x, y, z])
00240                                 v = v.dot(self.R)
00241 
00242                                 imu_msg.linear_acceleration.x = v[0]
00243                                 imu_msg.linear_acceleration.y = v[1]
00244                                 imu_msg.linear_acceleration.z = v[2]
00245                                 imu_msg.linear_acceleration_covariance = (0.0004, 0., 0., 0.,
00246                                                 0.0004, 0., 0., 0., 0.0004)
00247                                 pub_imu = True
00248                         except KeyError:
00249                                 pass
00250                         try:
00251                                 x = imu_data['magX']
00252                                 y = imu_data['magY']
00253                                 z = imu_data['magZ']
00254 
00255                                 v = numpy.array([x, y, z])
00256                                 v = v.dot(self.R)
00257 
00258                                 mag_msg.vector.x = v[0]
00259                                 mag_msg.vector.y = v[1]
00260                                 mag_msg.vector.z = v[2]
00261                                 pub_mag = True
00262                         except KeyError:
00263                                 pass
00264                 if velocity_data:
00265                         pub_vel = True
00266                         vel_msg.twist.linear.x = velocity_data['Vel_X']
00267                         vel_msg.twist.linear.y = velocity_data['Vel_Y']
00268                         vel_msg.twist.linear.z = velocity_data['Vel_Z']
00269                 if orient_data:
00270                         pub_imu = True
00271                         orient_quat = quat_from_orient(orient_data)
00272                         imu_msg.orientation.x = orient_quat[0]
00273                         imu_msg.orientation.y = orient_quat[1]
00274                         imu_msg.orientation.z = orient_quat[2]
00275                         imu_msg.orientation.w = orient_quat[3]
00276                         imu_msg.orientation_covariance = (radians(1.), 0., 0., 0.,
00277                                         radians(1.), 0., 0., 0., radians(9.))
00278                 if position_data:
00279                         pub_gps = True
00280                         xgps_msg.latitude = gps_msg.latitude = position_data['Lat']
00281                         xgps_msg.longitude = gps_msg.longitude = position_data['Lon']
00282                         xgps_msg.altitude = gps_msg.altitude = position_data['Alt']
00283                 if status is not None:
00284                         if status & 0b0001:
00285                                 self.stest_stat.level = DiagnosticStatus.OK
00286                                 self.stest_stat.message = "Ok"
00287                         else:
00288                                 self.stest_stat.level = DiagnosticStatus.ERROR
00289                                 self.stest_stat.message = "Failed"
00290                         if status & 0b0010:
00291                                 self.xkf_stat.level = DiagnosticStatus.OK
00292                                 self.xkf_stat.message = "Valid"
00293                         else:
00294                                 self.xkf_stat.level = DiagnosticStatus.WARN
00295                                 self.xkf_stat.message = "Invalid"
00296                         if status & 0b0100:
00297                                 self.gps_stat.level = DiagnosticStatus.OK
00298                                 self.gps_stat.message = "Ok"
00299                         else:
00300                                 self.gps_stat.level = DiagnosticStatus.WARN
00301                                 self.gps_stat.message = "No fix"
00302                         self.diag_msg.header = h
00303                         self.diag_pub.publish(self.diag_msg)
00304 
00305                         if pub_gps:
00306                                 if status & 0b0100:
00307                                         gps_msg.status.status = NavSatStatus.STATUS_FIX
00308                                         xgps_msg.status.status = GPSStatus.STATUS_FIX
00309                                         gps_msg.status.service = NavSatStatus.SERVICE_GPS
00310                                         xgps_msg.status.position_source = 0b01101001
00311                                         xgps_msg.status.motion_source = 0b01101010
00312                                         xgps_msg.status.orientation_source = 0b01101010
00313                                 else:
00314                                         gps_msg.status.status = NavSatStatus.STATUS_NO_FIX
00315                                         xgps_msg.status.status = GPSStatus.STATUS_NO_FIX
00316                                         gps_msg.status.service = 0
00317                                         xgps_msg.status.position_source = 0b01101000
00318                                         xgps_msg.status.motion_source = 0b01101000
00319                                         xgps_msg.status.orientation_source = 0b01101000
00320                 # publish available information
00321                 if pub_imu:
00322                         imu_msg.header = h
00323                         self.imu_pub.publish(imu_msg)
00324                 if pub_gps:
00325                         xgps_msg.header = gps_msg.header = h
00326                         self.gps_pub.publish(gps_msg)
00327                         self.xgps_pub.publish(xgps_msg)
00328                 if pub_vel:
00329                         vel_msg.header = h
00330                         self.vel_pub.publish(vel_msg)
00331                 if pub_mag:
00332                         mag_msg.header = h
00333                         self.mag_pub.publish(mag_msg)
00334                 if pub_temp:
00335                         self.temp_pub.publish(temp_msg)
00336 
00337 
00338 
00339 def main():
00340         '''Create a ROS node and instantiate the class.'''
00341         rospy.init_node('xsens_driver')
00342         driver = XSensDriver()
00343         driver.spin()
00344 
00345 
00346 if __name__== '__main__':
00347         main()
00348 
00349