mtnode.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 import roslib; roslib.load_manifest('xsens_driver')
3 import rospy
4 import select
5 import sys
6 
7 import mtdevice
8 import mtdef
9 
10 from std_msgs.msg import Header, String, UInt16
11 from sensor_msgs.msg import Imu, NavSatFix, NavSatStatus, MagneticField,\
12  FluidPressure, Temperature, TimeReference
13 from geometry_msgs.msg import TwistStamped, PointStamped
14 from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus
15 import time
16 import datetime
17 import calendar
18 import serial
19 
20 # transform Euler angles or matrix into quaternions
21 from math import radians, sqrt, atan2
22 from tf.transformations import quaternion_from_matrix, quaternion_from_euler,\
23  identity_matrix
24 
25 
26 def get_param(name, default):
27  try:
28  v = rospy.get_param(name)
29  rospy.loginfo("Found parameter: %s, value: %s" % (name, str(v)))
30  except KeyError:
31  v = default
32  rospy.logwarn("Cannot find value for parameter: %s, assigning "
33  "default: %s" % (name, str(v)))
34  return v
35 
36 
37 def get_param_list(name, default):
38  value = get_param(name, default)
39  if len(default) != len(value):
40  rospy.logfatal("Parameter %s should be a list of size %d", name, len(default))
41  sys.exit(1)
42  return value
43 
44 
45 def matrix_from_diagonal(diagonal):
46  n = len(diagonal)
47  matrix = [0] * n * n
48  for i in range(0, n):
49  matrix[i*n + i] = diagonal[i]
50  return tuple(matrix)
51 
52 
53 class XSensDriver(object):
54 
55  def __init__(self):
56  device = get_param('~device', 'auto')
57  baudrate = get_param('~baudrate', 0)
58  timeout = get_param('~timeout', 0.002)
59  initial_wait = get_param('~initial_wait', 0.1)
60  if device == 'auto':
61  devs = mtdevice.find_devices(timeout=timeout,
62  initial_wait=initial_wait)
63  if devs:
64  device, baudrate = devs[0]
65  rospy.loginfo("Detected MT device on port %s @ %d bps"
66  % (device, baudrate))
67  else:
68  rospy.logerr("Fatal: could not find proper MT device.")
69  rospy.signal_shutdown("Could not find proper MT device.")
70  return
71  if not baudrate:
72  baudrate = mtdevice.find_baudrate(device, timeout=timeout,
73  initial_wait=initial_wait)
74  if not baudrate:
75  rospy.logerr("Fatal: could not find proper baudrate.")
76  rospy.signal_shutdown("Could not find proper baudrate.")
77  return
78 
79  rospy.loginfo("MT node interface: %s at %d bd." % (device, baudrate))
80  self.mt = mtdevice.MTDevice(device, baudrate, timeout,
81  initial_wait=initial_wait)
82 
83  # optional no rotation procedure for internal calibration of biases
84  # (only mark iv devices)
85  no_rotation_duration = get_param('~no_rotation_duration', 0)
86  if no_rotation_duration:
87  rospy.loginfo("Starting the no-rotation procedure to estimate the "
88  "gyroscope biases for %d s. Please don't move the "
89  "IMU during this time." % no_rotation_duration)
90  self.mt.SetNoRotation(no_rotation_duration)
91 
92  self.frame_id = get_param('~frame_id', '/base_imu')
93 
94  self.frame_local = get_param('~frame_local', 'ENU')
95 
96  self.angular_velocity_covariance = matrix_from_diagonal(
97  get_param_list('~angular_velocity_covariance_diagonal', [radians(0.025)] * 3)
98  )
99  self.linear_acceleration_covariance = matrix_from_diagonal(
100  get_param_list('~linear_acceleration_covariance_diagonal', [0.0004] * 3)
101  )
102  self.orientation_covariance = matrix_from_diagonal(
103  get_param_list("~orientation_covariance_diagonal", [radians(1.), radians(1.), radians(9.)])
104  )
105 
106  self.diag_msg = DiagnosticArray()
107  self.stest_stat = DiagnosticStatus(name='mtnode: Self Test', level=1,
108  message='No status information')
109  self.xkf_stat = DiagnosticStatus(name='mtnode: XKF Valid', level=1,
110  message='No status information')
111  self.gps_stat = DiagnosticStatus(name='mtnode: GPS Fix', level=1,
112  message='No status information')
113  self.diag_msg.status = [self.stest_stat, self.xkf_stat, self.gps_stat]
114 
115  # publishers created at first use to reduce topic clutter
116  self.diag_pub = None
117  self.imu_pub = None
118  self.raw_gps_pub = None
119  self.vel_pub = None
120  self.mag_pub = None
121  self.pos_gps_pub = None
122  self.temp_pub = None
123  self.press_pub = None
124  self.analog_in1_pub = None # decide type+header
125  self.analog_in2_pub = None # decide type+header
126  self.ecef_pub = None
127  self.time_ref_pub = None
128  # TODO pressure, ITOW from raw GPS?
129  self.old_bGPS = 256 # publish GPS only if new
130 
131  # publish a string version of all data; to be parsed by clients
132  self.str_pub = rospy.Publisher('imu_data_str', String, queue_size=10)
133  self.last_delta_q_time = None
134  self.delta_q_rate = None
135 
136  def reset_vars(self):
137  self.imu_msg = Imu()
138  self.imu_msg.orientation_covariance = (-1., )*9
139  self.imu_msg.angular_velocity_covariance = (-1., )*9
140  self.imu_msg.linear_acceleration_covariance = (-1., )*9
141  self.pub_imu = False
142  self.raw_gps_msg = NavSatFix()
143  self.pub_raw_gps = False
144  self.pos_gps_msg = NavSatFix()
145  self.pub_pos_gps = False
146  self.vel_msg = TwistStamped()
147  self.pub_vel = False
148  self.mag_msg = MagneticField()
149  self.mag_msg.magnetic_field_covariance = (0, )*9
150  self.pub_mag = False
151  self.temp_msg = Temperature()
152  self.temp_msg.variance = 0.
153  self.pub_temp = False
154  self.press_msg = FluidPressure()
155  self.press_msg.variance = 0.
156  self.pub_press = False
157  self.anin1_msg = UInt16()
158  self.pub_anin1 = False
159  self.anin2_msg = UInt16()
160  self.pub_anin2 = False
161  self.ecef_msg = PointStamped()
162  self.pub_ecef = False
163  self.pub_diag = False
164 
165  def spin(self):
166  try:
167  while not rospy.is_shutdown():
168  self.spin_once()
169  self.reset_vars()
170  # Ctrl-C signal interferes with select with the ROS signal handler
171  # should be OSError in python 3.?
172  except (select.error, OSError, serial.serialutil.SerialException):
173  pass
174 
175  def spin_once(self):
176  '''Read data from device and publishes ROS messages.'''
177  def convert_coords(x, y, z, source, dest=self.frame_local):
178  """Convert the coordinates between ENU, NED, and NWU."""
179  if source == dest:
180  return x, y, z
181  # convert to ENU
182  if source == 'NED':
183  x, y, z = y, x, -z
184  elif source == 'NWU':
185  x, y, z = -y, x, z
186  # convert to desired
187  if dest == 'NED':
188  x, y, z = y, x, -z
189  elif dest == 'NWU':
190  x, y, z = y, -x, z
191  return x, y, z
192 
193  def convert_quat(q, source, dest=self.frame_local):
194  """Convert a quaternion between ENU, NED, and NWU."""
195  def q_mult(q0, q1):
196  """Quaternion multiplication."""
197  w0, x0, y0, z0 = q0
198  w1, x1, y1, z1 = q1
199  w = w0*w1 - x0*x1 - y0*y1 - z0*z1
200  x = w0*x1 + x0*w1 + y0*z1 - z0*y1
201  y = w0*y1 - x0*z1 + y0*w1 + z0*x1
202  z = w0*z1 + x0*y1 - y0*x1 + z0*w1
203  return (w, x, y, z)
204  q_enu_ned = (0, 1./sqrt(2), 1./sqrt(2), 0)
205  q_enu_nwu = (1./sqrt(2), 0, 0, -1./sqrt(2))
206  q_ned_nwu = (0, -1, 0, 0)
207  q_ned_enu = (0, -1./sqrt(2), -1./sqrt(2), 0)
208  q_nwu_enu = (1./sqrt(2), 0, 0, 1./sqrt(2))
209  q_nwu_ned = (0, 1, 0, 0)
210  if source == 'ENU':
211  if dest == 'ENU':
212  return q
213  elif dest == 'NED':
214  return q_mult(q_enu_ned, q)
215  elif dest == 'NWU':
216  return q_mult(q_enu_nwu, q)
217  elif source == 'NED':
218  if dest == 'ENU':
219  return q_mult(q_ned_enu, q)
220  elif dest == 'NED':
221  return q
222  elif dest == 'NWU':
223  return q_mult(q_ned_nwu, q)
224  elif source == 'NWU':
225  if dest == 'ENU':
226  return q_mult(q_nwu_enu, q)
227  elif dest == 'NED':
228  return q_mult(q_nwu_ned, q)
229  elif dest == 'NWU':
230  return q
231 
232  def publish_time_ref(secs, nsecs, source):
233  """Publish a time reference."""
234  # Doesn't follow the standard publishing pattern since several time
235  # refs could be published simultaneously
236  if self.time_ref_pub is None:
237  self.time_ref_pub = rospy.Publisher(
238  'time_reference', TimeReference, queue_size=10)
239  time_ref_msg = TimeReference()
240  time_ref_msg.header = self.h
241  time_ref_msg.time_ref.secs = secs
242  time_ref_msg.time_ref.nsecs = nsecs
243  time_ref_msg.source = source
244  self.time_ref_pub.publish(time_ref_msg)
245 
246  def stamp_from_itow(itow, y=None, m=None, d=None, ns=0, week=None):
247  """Return (secs, nsecs) from GPS time of week ms information."""
248  if y is not None:
249  stamp_day = datetime.datetime(y, m, d)
250  elif week is not None:
251  epoch = datetime.datetime(1980, 1, 6) # GPS epoch
252  stamp_day = epoch + datetime.timedelta(weeks=week)
253  else:
254  today = datetime.date.today() # using today by default
255  stamp_day = datetime.datetime(today.year, today.month,
256  today.day)
257  iso_day = stamp_day.isoweekday() # 1 for Monday, 7 for Sunday
258  # stamp for the GPS start of the week (Sunday morning)
259  start_of_week = stamp_day - datetime.timedelta(days=iso_day)
260  # stamp at the millisecond precision
261  stamp_ms = start_of_week + datetime.timedelta(milliseconds=itow)
262  secs = calendar.timegm((
263  stamp_ms.year, stamp_ms.month, stamp_ms.day, stamp_ms.hour,
264  stamp_ms.minute, stamp_ms.second, 0, 0, -1))
265  nsecs = stamp_ms.microsecond * 1000 + ns
266  if nsecs < 0: # ns can be negative
267  secs -= 1
268  nsecs += 1e9
269  return (secs, nsecs)
270 
271  # MTData
272  def fill_from_RAW(raw_data):
273  '''Fill messages with information from 'raw' MTData block.'''
274  # don't publish raw imu data anymore
275  # TODO find what to do with that
276  rospy.loginfo("Got MTi data packet: 'RAW', ignored!")
277 
278  def fill_from_RAWGPS(rawgps_data):
279  '''Fill messages with information from 'rawgps' MTData block.'''
280  if rawgps_data['bGPS'] < self.old_bGPS:
281  self.pub_raw_gps = True
282  # LLA
283  self.raw_gps_msg.latitude = rawgps_data['LAT']*1e-7
284  self.raw_gps_msg.longitude = rawgps_data['LON']*1e-7
285  self.raw_gps_msg.altitude = rawgps_data['ALT']*1e-3
286  # NED vel # TODO?
287  self.old_bGPS = rawgps_data['bGPS']
288 
289  def fill_from_Temp(temp):
290  '''Fill messages with information from 'temperature' MTData block.
291  '''
292  self.pub_temp = True
293  self.temp_msg.temperature = temp
294 
295  def fill_from_Calib(imu_data):
296  '''Fill messages with information from 'calibrated' MTData block.
297  '''
298  try:
299  self.pub_imu = True
300  x, y, z = imu_data['gyrX'], imu_data['gyrY'], imu_data['gyrZ']
301  self.imu_msg.angular_velocity.x = x
302  self.imu_msg.angular_velocity.y = y
303  self.imu_msg.angular_velocity.z = z
304  self.imu_msg.angular_velocity_covariance = self.angular_velocity_covariance
305  self.pub_vel = True
306  self.vel_msg.twist.angular.x = x
307  self.vel_msg.twist.angular.y = y
308  self.vel_msg.twist.angular.z = z
309  except KeyError:
310  pass
311  try:
312  self.pub_imu = True
313  x, y, z = imu_data['accX'], imu_data['accY'], imu_data['accZ']
314  self.imu_msg.linear_acceleration.x = x
315  self.imu_msg.linear_acceleration.y = y
316  self.imu_msg.linear_acceleration.z = z
317  self.imu_msg.linear_acceleration_covariance = self.linear_acceleration_covariance
318  except KeyError:
319  pass
320  try:
321  self.pub_mag = True
322  x, y, z = imu_data['magX'], imu_data['magY'], imu_data['magZ']
323  self.mag_msg.magnetic_field.x = x
324  self.mag_msg.magnetic_field.y = y
325  self.mag_msg.magnetic_field.z = z
326  except KeyError:
327  pass
328 
329  def fill_from_Orient(orient_data):
330  '''Fill messages with information from 'orientation' MTData block.
331  '''
332  self.pub_imu = True
333  if 'quaternion' in orient_data:
334  w, x, y, z = orient_data['quaternion']
335  elif 'roll' in orient_data:
336  x, y, z, w = quaternion_from_euler(
337  radians(orient_data['roll']),
338  radians(orient_data['pitch']),
339  radians(orient_data['yaw']))
340  elif 'matrix' in orient_data:
341  m = identity_matrix()
342  m[:3, :3] = orient_data['matrix']
343  x, y, z, w = quaternion_from_matrix(m)
344  w, x, y, z = convert_quat((w, x, y, z), o['frame'])
345  self.imu_msg.orientation.x = x
346  self.imu_msg.orientation.y = y
347  self.imu_msg.orientation.z = z
348  self.imu_msg.orientation.w = w
349  self.imu_msg.orientation_covariance = self.orientation_covariance
350 
351  def fill_from_Auxiliary(aux_data):
352  '''Fill messages with information from 'Auxiliary' MTData block.'''
353  try:
354  self.anin1_msg.data = o['Ain_1']
355  self.pub_anin1 = True
356  except KeyError:
357  pass
358  try:
359  self.anin2_msg.data = o['Ain_2']
360  self.pub_anin2 = True
361  except KeyError:
362  pass
363 
364  def fill_from_Pos(position_data):
365  '''Fill messages with information from 'position' MTData block.'''
366  self.pub_pos_gps = True
367  self.pos_gps_msg.latitude = position_data['Lat']
368  self.pos_gps_msg.longitude = position_data['Lon']
369  self.pos_gps_msg.altitude = position_data['Alt']
370 
371  def fill_from_Vel(velocity_data):
372  '''Fill messages with information from 'velocity' MTData block.'''
373  self.pub_vel = True
374  x, y, z = convert_coords(
375  velocity_data['Vel_X'], velocity_data['Vel_Y'],
376  velocity_data['Vel_Z'], o['frame'])
377  self.vel_msg.twist.linear.x = x
378  self.vel_msg.twist.linear.y = y
379  self.vel_msg.twist.linear.z = z
380 
381  def fill_from_Stat(status):
382  '''Fill messages with information from 'status' MTData block.'''
383  self.pub_diag = True
384  if status & 0b0001:
385  self.stest_stat.level = DiagnosticStatus.OK
386  self.stest_stat.message = "Ok"
387  else:
388  self.stest_stat.level = DiagnosticStatus.ERROR
389  self.stest_stat.message = "Failed"
390  if status & 0b0010:
391  self.xkf_stat.level = DiagnosticStatus.OK
392  self.xkf_stat.message = "Valid"
393  else:
394  self.xkf_stat.level = DiagnosticStatus.WARN
395  self.xkf_stat.message = "Invalid"
396  if status & 0b0100:
397  self.gps_stat.level = DiagnosticStatus.OK
398  self.gps_stat.message = "Ok"
399  self.raw_gps_msg.status.status = NavSatStatus.STATUS_FIX
400  self.raw_gps_msg.status.service = NavSatStatus.SERVICE_GPS
401  # we borrow the status from the raw gps for pos_gps_msg
402  self.pos_gps_msg.status.status = NavSatStatus.STATUS_FIX
403  self.pos_gps_msg.status.service = NavSatStatus.SERVICE_GPS
404  else:
405  self.gps_stat.level = DiagnosticStatus.WARN
406  self.gps_stat.message = "No fix"
407  self.raw_gps_msg.status.status = NavSatStatus.STATUS_NO_FIX
408  self.raw_gps_msg.status.service = 0
409  # we borrow the status from the raw gps for pos_gps_msg
410  self.pos_gps_msg.status.status = NavSatStatus.STATUS_NO_FIX
411  self.pos_gps_msg.status.service = 0
412 
413  def fill_from_Sample(ts):
414  '''Catch 'Sample' MTData blocks.'''
415  self.h.seq = ts
416 
417  # MTData2
418  def fill_from_Temperature(o):
419  '''Fill messages with information from 'Temperature' MTData2 block.
420  '''
421  self.pub_temp = True
422  self.temp_msg.temperature = o['Temp']
423 
424  def fill_from_Timestamp(o):
425  '''Fill messages with information from 'Timestamp' MTData2 block.
426  '''
427  try:
428  # put timestamp from gps UTC time if available
429  y, m, d, hr, mi, s, ns, f = o['Year'], o['Month'], o['Day'],\
430  o['Hour'], o['Minute'], o['Second'], o['ns'], o['Flags']
431  if f & 0x4:
432  secs = calendar.timegm((y, m, d, hr, mi, s, 0, 0, 0))
433  publish_time_ref(secs, ns, 'UTC time')
434  except KeyError:
435  pass
436  try:
437  itow = o['TimeOfWeek']
438  secs, nsecs = stamp_from_itow(itow)
439  publish_time_ref(secs, nsecs, 'integer time of week')
440  except KeyError:
441  pass
442  try:
443  sample_time_fine = o['SampleTimeFine']
444  # int in 10kHz ticks
445  secs = int(sample_time_fine / 10000)
446  nsecs = 1e5 * (sample_time_fine % 10000)
447  publish_time_ref(secs, nsecs, 'sample time fine')
448  except KeyError:
449  pass
450  try:
451  sample_time_coarse = o['SampleTimeCoarse']
452  publish_time_ref(sample_time_coarse, 0, 'sample time coarse')
453  except KeyError:
454  pass
455  # TODO find what to do with other kind of information
456  pass
457 
458  def fill_from_Orientation_Data(o):
459  '''Fill messages with information from 'Orientation Data' MTData2
460  block.'''
461  self.pub_imu = True
462  try:
463  x, y, z, w = o['Q1'], o['Q2'], o['Q3'], o['Q0']
464  except KeyError:
465  pass
466  try:
467  x, y, z, w = quaternion_from_euler(radians(o['Roll']),
468  radians(o['Pitch']),
469  radians(o['Yaw']))
470  except KeyError:
471  pass
472  try:
473  a, b, c, d, e, f, g, h, i = o['a'], o['b'], o['c'], o['d'],\
474  o['e'], o['f'], o['g'], o['h'], o['i']
475  m = identity_matrix()
476  m[:3, :3] = ((a, b, c), (d, e, f), (g, h, i))
477  x, y, z, w = quaternion_from_matrix(m)
478  except KeyError:
479  pass
480  w, x, y, z = convert_quat((w, x, y, z), o['frame'])
481  self.imu_msg.orientation.x = x
482  self.imu_msg.orientation.y = y
483  self.imu_msg.orientation.z = z
484  self.imu_msg.orientation.w = w
485  self.imu_msg.orientation_covariance = self.orientation_covariance
486 
487  def fill_from_Pressure(o):
488  '''Fill messages with information from 'Pressure' MTData2 block.'''
489  self.press_msg.fluid_pressure = o['Pressure']
490  self.pub_press = True
491 
492  def fill_from_Acceleration(o):
493  '''Fill messages with information from 'Acceleration' MTData2
494  block.'''
495  self.pub_imu = True
496 
497  # FIXME not sure we should treat all in that same way
498  try:
499  x, y, z = o['Delta v.x'], o['Delta v.y'], o['Delta v.z']
500  except KeyError:
501  pass
502  try:
503  x, y, z = o['freeAccX'], o['freeAccY'], o['freeAccZ']
504  except KeyError:
505  pass
506  try:
507  x, y, z = o['accX'], o['accY'], o['accZ']
508  except KeyError:
509  pass
510  self.imu_msg.linear_acceleration.x = x
511  self.imu_msg.linear_acceleration.y = y
512  self.imu_msg.linear_acceleration.z = z
513  self.imu_msg.linear_acceleration_covariance = self.linear_acceleration_covariance
514 
515  def fill_from_Position(o):
516  '''Fill messages with information from 'Position' MTData2 block.'''
517  try:
518  self.pos_gps_msg.latitude = o['lat']
519  self.pos_gps_msg.longitude = o['lon']
520  # altMsl is deprecated
521  alt = o.get('altEllipsoid', o.get('altMsl', 0))
522  self.pos_gps_msg.altitude = alt
523  self.pub_pos_gps = True
524  except KeyError:
525  pass
526  try:
527  x, y, z = o['ecefX'], o['ecefY'], o['ecefZ']
528  # TODO: ecef units not specified: might not be in meters!
529  self.ecef_msg.point.x = x
530  self.ecef_msg.point.y = y
531  self.ecef_msg.point.z = z
532  self.pub_ecef = True
533  except KeyError:
534  pass
535 
536  def fill_from_GNSS(o):
537  '''Fill messages with information from 'GNSS' MTData2 block.'''
538  try: # PVT
539  # time block
540  itow, y, m, d, ns, f = o['itow'], o['year'], o['month'],\
541  o['day'], o['nano'], o['valid']
542  if f & 0x4:
543  secs, nsecs = stamp_from_itow(itow, y, m, d, ns)
544  publish_time_ref(secs, nsecs, 'GNSS time UTC')
545  # flags
546  fixtype = o['fixtype']
547  if fixtype == 0x00:
548  # no fix
549  self.raw_gps_msg.status.status = NavSatStatus.STATUS_NO_FIX
550  self.raw_gps_msg.status.service = 0
551  else:
552  # unaugmented
553  self.raw_gps_msg.status.status = NavSatStatus.STATUS_FIX
554  self.raw_gps_msg.status.service = NavSatStatus.SERVICE_GPS
555  # lat lon alt
556  self.raw_gps_msg.latitude = o['lat']
557  self.raw_gps_msg.longitude = o['lon']
558  self.raw_gps_msg.altitude = o['height']/1e3
559  # TODO should we separate raw_gps and GNSS?
560  self.pub_raw_gps = True
561  # TODO velocity?
562  # TODO 2D heading?
563  # TODO DOP?
564  except KeyError:
565  pass
566  # TODO publish Sat Info
567 
568  def fill_from_Angular_Velocity(o):
569  '''Fill messages with information from 'Angular Velocity' MTData2
570  block.'''
571  try:
572  dqw, dqx, dqy, dqz = (o['Delta q0'], o['Delta q1'],
573  o['Delta q2'], o['Delta q3'])
574  now = rospy.Time.now()
575  if self.last_delta_q_time is None:
576  self.last_delta_q_time = now
577  else:
578  # update rate (filtering so as to account for lag variance)
579  delta_t = (now - self.last_delta_q_time).to_sec()
580  if self.delta_q_rate is None:
581  self.delta_q_rate = 1./delta_t
582  delta_t_filtered = .95/self.delta_q_rate + .05*delta_t
583  # rate in necessarily integer
584  self.delta_q_rate = round(1./delta_t_filtered)
585  self.last_delta_q_time = now
586  # relationship between \Delta q and velocity \bm{\omega}:
587  # \bm{w} = \Delta t . \bm{\omega}
588  # \theta = |\bm{w}|
589  # \Delta q = [cos{\theta/2}, sin{\theta/2)/\theta . \omega
590  # extract rotation angle over delta_t
591  ca_2, sa_2 = dqw, sqrt(dqx**2 + dqy**2 + dqz**2)
592  ca = ca_2**2 - sa_2**2
593  sa = 2*ca_2*sa_2
594  rotation_angle = atan2(sa, ca)
595  # compute rotation velocity
596  rotation_speed = rotation_angle * self.delta_q_rate
597  f = rotation_speed / sa_2
598  x, y, z = f*dqx, f*dqy, f*dqz
599  self.imu_msg.angular_velocity.x = x
600  self.imu_msg.angular_velocity.y = y
601  self.imu_msg.angular_velocity.z = z
602  self.imu_msg.angular_velocity_covariance = self.angular_velocity_covariance
603  self.pub_imu = True
604  self.vel_msg.twist.angular.x = x
605  self.vel_msg.twist.angular.y = y
606  self.vel_msg.twist.angular.z = z
607  self.pub_vel = True
608  except KeyError:
609  pass
610  try:
611  x, y, z = o['gyrX'], o['gyrY'], o['gyrZ']
612  self.imu_msg.angular_velocity.x = x
613  self.imu_msg.angular_velocity.y = y
614  self.imu_msg.angular_velocity.z = z
615  self.imu_msg.angular_velocity_covariance = self.angular_velocity_covariance
616  self.pub_imu = True
617  self.vel_msg.twist.angular.x = x
618  self.vel_msg.twist.angular.y = y
619  self.vel_msg.twist.angular.z = z
620  self.pub_vel = True
621  except KeyError:
622  pass
623 
624  def fill_from_GPS(o):
625  '''Fill messages with information from 'GPS' MTData2 block.'''
626  # TODO DOP
627  try: # SOL
628  x, y, z = o['ecefX'], o['ecefY'], o['ecefZ']
629  self.ecef_msg.point.x = x * 0.01 # data is in cm
630  self.ecef_msg.point.y = y * 0.01
631  self.ecef_msg.point.z = z * 0.01
632  self.pub_ecef = True
633  vx, vy, vz = o['ecefVX'], o['ecefVY'], o['ecefVZ']
634  self.vel_msg.twist.linear.x = vx * 0.01 # data is in cm
635  self.vel_msg.twist.linear.y = vy * 0.01
636  self.vel_msg.twist.linear.z = vz * 0.01
637  self.pub_vel = True
638  itow, ns, week, f = o['iTOW'], o['fTOW'], o['Week'], o['Flags']
639  if (f & 0x0C) == 0xC:
640  secs, nsecs = stamp_from_itow(itow, ns=ns, week=week)
641  publish_time_ref(secs, nsecs, 'GPS Time')
642  # TODO there are other pieces of information that we could
643  # publish
644  except KeyError:
645  pass
646  try: # Time UTC
647  itow, y, m, d, ns, f = o['iTOW'], o['year'], o['month'],\
648  o['day'], o['nano'], o['valid']
649  if f & 0x4:
650  secs, nsecs = stamp_from_itow(itow, y, m, d, ns)
651  publish_time_ref(secs, nsecs, 'GPS Time UTC')
652  except KeyError:
653  pass
654  # TODO publish SV Info
655 
656  def fill_from_SCR(o):
657  '''Fill messages with information from 'SCR' MTData2 block.'''
658  # TODO that's raw information
659  pass
660 
661  def fill_from_Analog_In(o):
662  '''Fill messages with information from 'Analog In' MTData2 block.
663  '''
664  try:
665  self.anin1_msg.data = o['analogIn1']
666  self.pub_anin1 = True
667  except KeyError:
668  pass
669  try:
670  self.anin2_msg.data = o['analogIn2']
671  self.pub_anin2 = True
672  except KeyError:
673  pass
674 
675  def fill_from_Magnetic(o):
676  '''Fill messages with information from 'Magnetic' MTData2 block.'''
677  x, y, z = o['magX'], o['magY'], o['magZ']
678  self.mag_msg.magnetic_field.x = x
679  self.mag_msg.magnetic_field.y = y
680  self.mag_msg.magnetic_field.z = z
681  self.pub_mag = True
682 
683  def fill_from_Velocity(o):
684  '''Fill messages with information from 'Velocity' MTData2 block.'''
685  x, y, z = convert_coords(o['velX'], o['velY'], o['velZ'],
686  o['frame'])
687  self.vel_msg.twist.linear.x = x
688  self.vel_msg.twist.linear.y = y
689  self.vel_msg.twist.linear.z = z
690  self.pub_vel = True
691 
692  def fill_from_Status(o):
693  '''Fill messages with information from 'Status' MTData2 block.'''
694  try:
695  status = o['StatusByte']
696  fill_from_Stat(status)
697  except KeyError:
698  pass
699  try:
700  status = o['StatusWord']
701  fill_from_Stat(status)
702  except KeyError:
703  pass
704 
705  def find_handler_name(name):
706  return "fill_from_%s" % (name.replace(" ", "_"))
707 
708  # get data
709  try:
710  data = self.mt.read_measurement()
711  except mtdef.MTTimeoutException:
712  time.sleep(0.1)
713  return
714  # common header
715  self.h = Header()
716  self.h.stamp = rospy.Time.now()
717  self.h.frame_id = self.frame_id
718 
719  # set default values
720  self.reset_vars()
721 
722  # fill messages based on available data fields
723  for n, o in data.items():
724  try:
725  locals()[find_handler_name(n)](o)
726  except KeyError:
727  rospy.logwarn("Unknown MTi data packet: '%s', ignoring." % n)
728 
729  # publish available information
730  if self.pub_imu:
731  self.imu_msg.header = self.h
732  if self.imu_pub is None:
733  self.imu_pub = rospy.Publisher('imu/data', Imu, queue_size=10)
734  self.imu_pub.publish(self.imu_msg)
735  if self.pub_raw_gps:
736  self.raw_gps_msg.header = self.h
737  if self.raw_gps_pub is None:
738  self.raw_gps_pub = rospy.Publisher('raw_fix', NavSatFix, queue_size=10)
739  self.raw_gps_pub.publish(self.raw_gps_msg)
740  if self.pub_pos_gps:
741  self.pos_gps_msg.header = self.h
742  if self.pos_gps_pub is None:
743  self.pos_gps_pub = rospy.Publisher('fix', NavSatFix, queue_size=10)
744  self.pos_gps_pub.publish(self.pos_gps_msg)
745  if self.pub_vel:
746  self.vel_msg.header = self.h
747  if self.vel_pub is None:
748  self.vel_pub = rospy.Publisher('velocity', TwistStamped,
749  queue_size=10)
750  self.vel_pub.publish(self.vel_msg)
751  if self.pub_mag:
752  self.mag_msg.header = self.h
753  if self.mag_pub is None:
754  self.mag_pub = rospy.Publisher('imu/mag', MagneticField,
755  queue_size=10)
756  self.mag_pub.publish(self.mag_msg)
757  if self.pub_temp:
758  self.temp_msg.header = self.h
759  if self.temp_pub is None:
760  self.temp_pub = rospy.Publisher('temperature', Temperature,
761  queue_size=10)
762  self.temp_pub.publish(self.temp_msg)
763  if self.pub_press:
764  self.press_msg.header = self.h
765  if self.press_pub is None:
766  self.press_pub = rospy.Publisher('pressure', FluidPressure,
767  queue_size=10)
768  self.press_pub.publish(self.press_msg)
769  if self.pub_anin1:
770  if self.analog_in1_pub is None:
771  self.analog_in1_pub = rospy.Publisher('analog_in1',
772  UInt16, queue_size=10)
773  self.analog_in1_pub.publish(self.anin1_msg)
774  if self.pub_anin2:
775  if self.analog_in2_pub is None:
776  self.analog_in2_pub = rospy.Publisher('analog_in2', UInt16,
777  queue_size=10)
778  self.analog_in2_pub.publish(self.anin2_msg)
779  if self.pub_ecef:
780  self.ecef_msg.header = self.h
781  if self.ecef_pub is None:
782  self.ecef_pub = rospy.Publisher('ecef', PointStamped,
783  queue_size=10)
784  self.ecef_pub.publish(self.ecef_msg)
785  if self.pub_diag:
786  self.diag_msg.header = self.h
787  if self.diag_pub is None:
788  self.diag_pub = rospy.Publisher('/diagnostics',
789  DiagnosticArray, queue_size=10)
790  self.diag_pub.publish(self.diag_msg)
791  # publish string representation
792  self.str_pub.publish(str(data))
793 
794 
795 def main():
796  '''Create a ROS node and instantiate the class.'''
797  rospy.init_node('xsens_driver')
798  driver = XSensDriver()
799  driver.spin()
800 
801 
802 if __name__ == '__main__':
803  main()
mtnode.get_param_list
def get_param_list(name, default)
Definition: mtnode.py:37
mtnode.XSensDriver.spin
def spin(self)
Definition: mtnode.py:165
mtnode.XSensDriver.reset_vars
def reset_vars(self)
Definition: mtnode.py:136
mtdevice.find_baudrate
def find_baudrate(port, timeout=0.002, verbose=False, initial_wait=0.1)
Auto detect baudrate.
Definition: mtdevice.py:1193
mtnode.matrix_from_diagonal
def matrix_from_diagonal(diagonal)
Definition: mtnode.py:45
mtnode.XSensDriver.angular_velocity_covariance
angular_velocity_covariance
Definition: mtnode.py:96
mtdevice.MTDevice
MTDevice class.
Definition: mtdevice.py:20
mtnode.get_param
def get_param(name, default)
Definition: mtnode.py:26
mtdevice.find_devices
def find_devices(timeout=0.002, verbose=False, initial_wait=0.1)
Definition: mtdevice.py:1176
mtnode.main
def main()
Definition: mtnode.py:795
mtnode.XSensDriver.linear_acceleration_covariance
linear_acceleration_covariance
Definition: mtnode.py:99
mtnode.XSensDriver.spin_once
def spin_once(self)
Definition: mtnode.py:175
mtnode.XSensDriver.__init__
def __init__(self)
Definition: mtnode.py:55

xsens_driver
Author(s):
autogenerated on Mon Sep 9 2019 03:44:33