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