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