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 IMU"
89  " 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((w0, x0, y0, z0), (w1, x1, y1, z1)):
196  """Quaternion multiplication."""
197  w = w0*w1 - x0*x1 - y0*y1 - z0*z1
198  x = w0*x1 + x0*w1 + y0*z1 - z0*y1
199  y = w0*y1 - x0*z1 + y0*w1 + z0*x1
200  z = w0*z1 + x0*y1 - y0*x1 + z0*w1
201  return (w, x, y, z)
202  q_enu_ned = (0, 1./sqrt(2), 1./sqrt(2), 0)
203  q_enu_nwu = (1./sqrt(2), 0, 0, -1./sqrt(2))
204  q_ned_nwu = (0, -1, 0, 0)
205  q_ned_enu = (0, -1./sqrt(2), -1./sqrt(2), 0)
206  q_nwu_enu = (1./sqrt(2), 0, 0, 1./sqrt(2))
207  q_nwu_ned = (0, 1, 0, 0)
208  if source == 'ENU':
209  if dest == 'ENU':
210  return q
211  elif dest == 'NED':
212  return q_mult(q_enu_ned, q)
213  elif dest == 'NWU':
214  return q_mult(q_enu_nwu, q)
215  elif source == 'NED':
216  if dest == 'ENU':
217  return q_mult(q_ned_enu, q)
218  elif dest == 'NED':
219  return q
220  elif dest == 'NWU':
221  return q_mult(q_ned_nwu, q)
222  elif source == 'NWU':
223  if dest == 'ENU':
224  return q_mult(q_nwu_enu, q)
225  elif dest == 'NED':
226  return q_mult(q_nwu_ned, q)
227  elif dest == 'NWU':
228  return q
229 
230  def publish_time_ref(secs, nsecs, source):
231  """Publish a time reference."""
232  # Doesn't follow the standard publishing pattern since several time
233  # refs could be published simultaneously
234  if self.time_ref_pub is None:
235  self.time_ref_pub = rospy.Publisher(
236  'time_reference', TimeReference, queue_size=10)
237  time_ref_msg = TimeReference()
238  time_ref_msg.header = self.h
239  time_ref_msg.time_ref.secs = secs
240  time_ref_msg.time_ref.nsecs = nsecs
241  time_ref_msg.source = source
242  self.time_ref_pub.publish(time_ref_msg)
243 
244  def stamp_from_itow(itow, y=None, m=None, d=None, ns=0, week=None):
245  """Return (secs, nsecs) from GPS time of week ms information."""
246  if y is not None:
247  stamp_day = datetime.datetime(y, m, d)
248  elif week is not None:
249  epoch = datetime.datetime(1980, 1, 6) # GPS epoch
250  stamp_day = epoch + datetime.timedelta(weeks=week)
251  else:
252  today = datetime.date.today() # using today by default
253  stamp_day = datetime.datetime(today.year, today.month,
254  today.day)
255  iso_day = stamp_day.isoweekday() # 1 for Monday, 7 for Sunday
256  # stamp for the GPS start of the week (Sunday morning)
257  start_of_week = stamp_day - datetime.timedelta(days=iso_day)
258  # stamp at the millisecond precision
259  stamp_ms = start_of_week + datetime.timedelta(milliseconds=itow)
260  secs = calendar.timegm((stamp_ms.year, stamp_ms.month, stamp_ms.day,
261  stamp_ms.hour, stamp_ms.minute,
262  stamp_ms.second, 0, 0, -1))
263  nsecs = stamp_ms.microsecond * 1000 + ns
264  if nsecs < 0: # ns can be negative
265  secs -= 1
266  nsecs += 1e9
267  return (secs, nsecs)
268 
269  # MTData
270  def fill_from_RAW(raw_data):
271  '''Fill messages with information from 'raw' MTData block.'''
272  # don't publish raw imu data anymore
273  # TODO find what to do with that
274  rospy.loginfo("Got MTi data packet: 'RAW', ignored!")
275 
276  def fill_from_RAWGPS(rawgps_data):
277  '''Fill messages with information from 'rawgps' MTData block.'''
278  if rawgps_data['bGPS'] < self.old_bGPS:
279  self.pub_raw_gps = True
280  # LLA
281  self.raw_gps_msg.latitude = rawgps_data['LAT']*1e-7
282  self.raw_gps_msg.longitude = rawgps_data['LON']*1e-7
283  self.raw_gps_msg.altitude = rawgps_data['ALT']*1e-3
284  # NED vel # TODO?
285  self.old_bGPS = rawgps_data['bGPS']
286 
287  def fill_from_Temp(temp):
288  '''Fill messages with information from 'temperature' MTData block.
289  '''
290  self.pub_temp = True
291  self.temp_msg.temperature = temp
292 
293  def fill_from_Calib(imu_data):
294  '''Fill messages with information from 'calibrated' MTData block.'''
295  try:
296  self.pub_imu = True
297  x, y, z = imu_data['gyrX'], imu_data['gyrY'], imu_data['gyrZ']
298  self.imu_msg.angular_velocity.x = x
299  self.imu_msg.angular_velocity.y = y
300  self.imu_msg.angular_velocity.z = z
301  self.imu_msg.angular_velocity_covariance = self.angular_velocity_covariance
302  self.pub_vel = True
303  self.vel_msg.twist.angular.x = x
304  self.vel_msg.twist.angular.y = y
305  self.vel_msg.twist.angular.z = z
306  except KeyError:
307  pass
308  try:
309  self.pub_imu = True
310  x, y, z = imu_data['accX'], imu_data['accY'], imu_data['accZ']
311  self.imu_msg.linear_acceleration.x = x
312  self.imu_msg.linear_acceleration.y = y
313  self.imu_msg.linear_acceleration.z = z
314  self.imu_msg.linear_acceleration_covariance = self.linear_acceleration_covariance
315  except KeyError:
316  pass
317  try:
318  self.pub_mag = True
319  x, y, z = imu_data['magX'], imu_data['magY'], imu_data['magZ']
320  self.mag_msg.magnetic_field.x = x
321  self.mag_msg.magnetic_field.y = y
322  self.mag_msg.magnetic_field.z = z
323  except KeyError:
324  pass
325 
326  def fill_from_Orient(orient_data):
327  '''Fill messages with information from 'orientation' MTData block.
328  '''
329  self.pub_imu = True
330  if 'quaternion' in orient_data:
331  w, x, y, z = orient_data['quaternion']
332  elif 'roll' in orient_data:
333  x, y, z, w = quaternion_from_euler(
334  radians(orient_data['roll']), radians(orient_data['pitch']),
335  radians(orient_data['yaw']))
336  elif 'matrix' in orient_data:
337  m = identity_matrix()
338  m[:3, :3] = orient_data['matrix']
339  x, y, z, w = quaternion_from_matrix(m)
340  w, x, y, z = convert_quat((w, x, y, z), o['frame'])
341  self.imu_msg.orientation.x = x
342  self.imu_msg.orientation.y = y
343  self.imu_msg.orientation.z = z
344  self.imu_msg.orientation.w = w
345  self.imu_msg.orientation_covariance = self.orientation_covariance
346 
347  def fill_from_Auxiliary(aux_data):
348  '''Fill messages with information from 'Auxiliary' MTData block.'''
349  try:
350  self.anin1_msg.data = o['Ain_1']
351  self.pub_anin1 = True
352  except KeyError:
353  pass
354  try:
355  self.anin2_msg.data = o['Ain_2']
356  self.pub_anin2 = True
357  except KeyError:
358  pass
359 
360  def fill_from_Pos(position_data):
361  '''Fill messages with information from 'position' MTData block.'''
362  self.pub_pos_gps = True
363  self.pos_gps_msg.latitude = position_data['Lat']
364  self.pos_gps_msg.longitude = position_data['Lon']
365  self.pos_gps_msg.altitude = position_data['Alt']
366 
367  def fill_from_Vel(velocity_data):
368  '''Fill messages with information from 'velocity' MTData block.'''
369  self.pub_vel = True
370  x, y, z = convert_coords(
371  velocity_data['Vel_X'], velocity_data['Vel_Y'],
372  velocity_data['Vel_Z'], o['frame'])
373  self.vel_msg.twist.linear.x = x
374  self.vel_msg.twist.linear.y = y
375  self.vel_msg.twist.linear.z = z
376 
377  def fill_from_Stat(status):
378  '''Fill messages with information from 'status' MTData block.'''
379  self.pub_diag = True
380  if status & 0b0001:
381  self.stest_stat.level = DiagnosticStatus.OK
382  self.stest_stat.message = "Ok"
383  else:
384  self.stest_stat.level = DiagnosticStatus.ERROR
385  self.stest_stat.message = "Failed"
386  if status & 0b0010:
387  self.xkf_stat.level = DiagnosticStatus.OK
388  self.xkf_stat.message = "Valid"
389  else:
390  self.xkf_stat.level = DiagnosticStatus.WARN
391  self.xkf_stat.message = "Invalid"
392  if status & 0b0100:
393  self.gps_stat.level = DiagnosticStatus.OK
394  self.gps_stat.message = "Ok"
395  self.raw_gps_msg.status.status = NavSatStatus.STATUS_FIX
396  self.raw_gps_msg.status.service = NavSatStatus.SERVICE_GPS
397  # we borrow the status from the raw gps for pos_gps_msg
398  self.pos_gps_msg.status.status = NavSatStatus.STATUS_FIX
399  self.pos_gps_msg.status.service = NavSatStatus.SERVICE_GPS
400  else:
401  self.gps_stat.level = DiagnosticStatus.WARN
402  self.gps_stat.message = "No fix"
403  self.raw_gps_msg.status.status = NavSatStatus.STATUS_NO_FIX
404  self.raw_gps_msg.status.service = 0
405  # we borrow the status from the raw gps for pos_gps_msg
406  self.pos_gps_msg.status.status = NavSatStatus.STATUS_NO_FIX
407  self.pos_gps_msg.status.service = 0
408 
409  def fill_from_Sample(ts):
410  '''Catch 'Sample' MTData blocks.'''
411  self.h.seq = ts
412 
413  # MTData2
414  def fill_from_Temperature(o):
415  '''Fill messages with information from 'Temperature' MTData2 block.
416  '''
417  self.pub_temp = True
418  self.temp_msg.temperature = o['Temp']
419 
420  def fill_from_Timestamp(o):
421  '''Fill messages with information from 'Timestamp' MTData2 block.'''
422  try:
423  # put timestamp from gps UTC time if available
424  y, m, d, hr, mi, s, ns, f = o['Year'], o['Month'], o['Day'],\
425  o['Hour'], o['Minute'], o['Second'], o['ns'], o['Flags']
426  if f & 0x4:
427  secs = calendar.timegm((y, m, d, hr, mi, s, 0, 0, 0))
428  publish_time_ref(secs, ns, 'UTC time')
429  except KeyError:
430  pass
431  try:
432  itow = o['TimeOfWeek']
433  secs, nsecs = stamp_from_itow(itow)
434  publish_time_ref(secs, nsecs, 'integer time of week')
435  except KeyError:
436  pass
437  try:
438  sample_time_fine = o['SampleTimeFine']
439  # int in 10kHz ticks
440  secs = int(sample_time_fine / 10000)
441  nsecs = 1e5 * (sample_time_fine % 10000)
442  publish_time_ref(secs, nsecs, 'sample time fine')
443  except KeyError:
444  pass
445  try:
446  sample_time_coarse = o['SampleTimeCoarse']
447  publish_time_ref(sample_time_coarse, 0, 'sample time coarse')
448  except KeyError:
449  pass
450  # TODO find what to do with other kind of information
451  pass
452 
453  def fill_from_Orientation_Data(o):
454  '''Fill messages with information from 'Orientation Data' MTData2
455  block.'''
456  self.pub_imu = True
457  try:
458  x, y, z, w = o['Q1'], o['Q2'], o['Q3'], o['Q0']
459  except KeyError:
460  pass
461  try:
462  x, y, z, w = quaternion_from_euler(radians(o['Roll']),
463  radians(o['Pitch']),
464  radians(o['Yaw']))
465  except KeyError:
466  pass
467  try:
468  a, b, c, d, e, f, g, h, i = o['a'], o['b'], o['c'], o['d'],\
469  o['e'], o['f'], o['g'], o['h'], o['i']
470  m = identity_matrix()
471  m[:3, :3] = ((a, b, c), (d, e, f), (g, h, i))
472  x, y, z, w = quaternion_from_matrix(m)
473  except KeyError:
474  pass
475  w, x, y, z = convert_quat((w, x, y, z), o['frame'])
476  self.imu_msg.orientation.x = x
477  self.imu_msg.orientation.y = y
478  self.imu_msg.orientation.z = z
479  self.imu_msg.orientation.w = w
480  self.imu_msg.orientation_covariance = self.orientation_covariance
481 
482  def fill_from_Pressure(o):
483  '''Fill messages with information from 'Pressure' MTData2 block.'''
484  self.press_msg.fluid_pressure = o['Pressure']
485  self.pub_press = True
486 
487  def fill_from_Acceleration(o):
488  '''Fill messages with information from 'Acceleration' MTData2
489  block.'''
490  self.pub_imu = True
491 
492  # FIXME not sure we should treat all in that same way
493  try:
494  x, y, z = o['Delta v.x'], o['Delta v.y'], o['Delta v.z']
495  except KeyError:
496  pass
497  try:
498  x, y, z = o['freeAccX'], o['freeAccY'], o['freeAccZ']
499  except KeyError:
500  pass
501  try:
502  x, y, z = o['accX'], o['accY'], o['accZ']
503  except KeyError:
504  pass
505  self.imu_msg.linear_acceleration.x = x
506  self.imu_msg.linear_acceleration.y = y
507  self.imu_msg.linear_acceleration.z = z
508  self.imu_msg.linear_acceleration_covariance = self.linear_acceleration_covariance
509 
510  def fill_from_Position(o):
511  '''Fill messages with information from 'Position' MTData2 block.'''
512  try:
513  self.pos_gps_msg.latitude = o['lat']
514  self.pos_gps_msg.longitude = o['lon']
515  # altMsl is deprecated
516  alt = o.get('altEllipsoid', o.get('altMsl', 0))
517  self.pos_gps_msg.altitude = alt
518  self.pub_pos_gps = True
519  except KeyError:
520  pass
521  try:
522  x, y, z = o['ecefX'], o['ecefY'], o['ecefZ']
523  # TODO: ecef units not specified: might not be in meters!
524  self.ecef_msg.point.x = x
525  self.ecef_msg.point.y = y
526  self.ecef_msg.point.z = z
527  self.pub_ecef = True
528  except KeyError:
529  pass
530 
531  def fill_from_GNSS(o):
532  '''Fill messages with information from 'GNSS' MTData2 block.'''
533  try: # PVT
534  # time block
535  itow, y, m, d, ns, f = o['itow'], o['year'], o['month'],\
536  o['day'], o['nano'], o['valid']
537  if f & 0x4:
538  secs, nsecs = stamp_from_itow(itow, y, m, d, ns)
539  publish_time_ref(secs, nsecs, 'GNSS time UTC')
540  # flags
541  fixtype = o['fixtype']
542  if fixtype == 0x00:
543  self.raw_gps_msg.status.status = NavSatStatus.STATUS_NO_FIX # no fix
544  self.raw_gps_msg.status.service = 0
545  else:
546  self.raw_gps_msg.status.status = NavSatStatus.STATUS_FIX # unaugmented
547  self.raw_gps_msg.status.service = NavSatStatus.SERVICE_GPS
548  # lat lon alt
549  self.raw_gps_msg.latitude = o['lat']
550  self.raw_gps_msg.longitude = o['lon']
551  self.raw_gps_msg.altitude = o['height']/1e3
552  # TODO should we separate raw_gps and GNSS?
553  self.pub_raw_gps = True
554  # TODO velocity?
555  # TODO 2D heading?
556  # TODO DOP?
557  except KeyError:
558  pass
559  # TODO publish Sat Info
560 
561  def fill_from_Angular_Velocity(o):
562  '''Fill messages with information from 'Angular Velocity' MTData2
563  block.'''
564  try:
565  dqw, dqx, dqy, dqz = (o['Delta q0'], o['Delta q1'],
566  o['Delta q2'], o['Delta q3'])
567  now = rospy.Time.now()
568  if self.last_delta_q_time is None:
569  self.last_delta_q_time = now
570  else:
571  # update rate (filtering needed to account for lag variance)
572  delta_t = (now - self.last_delta_q_time).to_sec()
573  if self.delta_q_rate is None:
574  self.delta_q_rate = 1./delta_t
575  delta_t_filtered = .95/self.delta_q_rate + .05*delta_t
576  # rate in necessarily integer
577  self.delta_q_rate = round(1./delta_t_filtered)
578  self.last_delta_q_time = now
579  # relationship between \Delta q and velocity \bm{\omega}:
580  # \bm{w} = \Delta t . \bm{\omega}
581  # \theta = |\bm{w}|
582  # \Delta q = [cos{\theta/2}, sin{\theta/2)/\theta . \bm{\omega}
583  # extract rotation angle over delta_t
584  ca_2, sa_2 = dqw, sqrt(dqx**2 + dqy**2 + dqz**2)
585  ca = ca_2**2 - sa_2**2
586  sa = 2*ca_2*sa_2
587  rotation_angle = atan2(sa, ca)
588  # compute rotation velocity
589  rotation_speed = rotation_angle * self.delta_q_rate
590  f = rotation_speed / sa_2
591  x, y, z = f*dqx, f*dqy, f*dqz
592  self.imu_msg.angular_velocity.x = x
593  self.imu_msg.angular_velocity.y = y
594  self.imu_msg.angular_velocity.z = z
595  self.imu_msg.angular_velocity_covariance = self.angular_velocity_covariance
596  self.pub_imu = True
597  self.vel_msg.twist.angular.x = x
598  self.vel_msg.twist.angular.y = y
599  self.vel_msg.twist.angular.z = z
600  self.pub_vel = True
601  except KeyError:
602  pass
603  try:
604  x, y, z = o['gyrX'], o['gyrY'], o['gyrZ']
605  self.imu_msg.angular_velocity.x = x
606  self.imu_msg.angular_velocity.y = y
607  self.imu_msg.angular_velocity.z = z
608  self.imu_msg.angular_velocity_covariance = self.angular_velocity_covariance
609  self.pub_imu = True
610  self.vel_msg.twist.angular.x = x
611  self.vel_msg.twist.angular.y = y
612  self.vel_msg.twist.angular.z = z
613  self.pub_vel = True
614  except KeyError:
615  pass
616 
617  def fill_from_GPS(o):
618  '''Fill messages with information from 'GPS' MTData2 block.'''
619  # TODO DOP
620  try: # SOL
621  x, y, z = o['ecefX'], o['ecefY'], o['ecefZ']
622  self.ecef_msg.point.x = x * 0.01 # data is in cm
623  self.ecef_msg.point.y = y * 0.01
624  self.ecef_msg.point.z = z * 0.01
625  self.pub_ecef = True
626  vx, vy, vz = o['ecefVX'], o['ecefVY'], o['ecefVZ']
627  self.vel_msg.twist.linear.x = vx * 0.01 # data is in cm
628  self.vel_msg.twist.linear.y = vy * 0.01
629  self.vel_msg.twist.linear.z = vz * 0.01
630  self.pub_vel = True
631  itow, ns, week, f = o['iTOW'], o['fTOW'], o['Week'], o['Flags']
632  if (f & 0x0C) == 0xC:
633  secs, nsecs = stamp_from_itow(itow, ns=ns, week=week)
634  publish_time_ref(secs, nsecs, 'GPS Time')
635  # TODO there are other pieces of information that we could
636  # publish
637  except KeyError:
638  pass
639  try: # Time UTC
640  itow, y, m, d, ns, f = o['iTOW'], o['year'], o['month'],\
641  o['day'], o['nano'], o['valid']
642  if f & 0x4:
643  secs, nsecs = stamp_from_itow(itow, y, m, d, ns)
644  publish_time_ref(secs, nsecs, 'GPS Time UTC')
645  except KeyError:
646  pass
647  # TODO publish SV Info
648 
649  def fill_from_SCR(o):
650  '''Fill messages with information from 'SCR' MTData2 block.'''
651  # TODO that's raw information
652  pass
653 
654  def fill_from_Analog_In(o):
655  '''Fill messages with information from 'Analog In' MTData2 block.'''
656  try:
657  self.anin1_msg.data = o['analogIn1']
658  self.pub_anin1 = True
659  except KeyError:
660  pass
661  try:
662  self.anin2_msg.data = o['analogIn2']
663  self.pub_anin2 = True
664  except KeyError:
665  pass
666 
667  def fill_from_Magnetic(o):
668  '''Fill messages with information from 'Magnetic' MTData2 block.'''
669  x, y, z = o['magX'], o['magY'], o['magZ']
670  self.mag_msg.magnetic_field.x = x
671  self.mag_msg.magnetic_field.y = y
672  self.mag_msg.magnetic_field.z = z
673  self.pub_mag = True
674 
675  def fill_from_Velocity(o):
676  '''Fill messages with information from 'Velocity' MTData2 block.'''
677  x, y, z = convert_coords(o['velX'], o['velY'], o['velZ'],
678  o['frame'])
679  self.vel_msg.twist.linear.x = x
680  self.vel_msg.twist.linear.y = y
681  self.vel_msg.twist.linear.z = z
682  self.pub_vel = True
683 
684  def fill_from_Status(o):
685  '''Fill messages with information from 'Status' MTData2 block.'''
686  try:
687  status = o['StatusByte']
688  fill_from_Stat(status)
689  except KeyError:
690  pass
691  try:
692  status = o['StatusWord']
693  fill_from_Stat(status)
694  except KeyError:
695  pass
696 
697  def find_handler_name(name):
698  return "fill_from_%s" % (name.replace(" ", "_"))
699 
700  # get data
701  try:
702  data = self.mt.read_measurement()
703  except mtdef.MTTimeoutException:
704  time.sleep(0.1)
705  return
706  # common header
707  self.h = Header()
708  self.h.stamp = rospy.Time.now()
709  self.h.frame_id = self.frame_id
710 
711  # set default values
712  self.reset_vars()
713 
714  # fill messages based on available data fields
715  for n, o in data.items():
716  try:
717  locals()[find_handler_name(n)](o)
718  except KeyError:
719  rospy.logwarn("Unknown MTi data packet: '%s', ignoring." % n)
720 
721  # publish available information
722  if self.pub_imu:
723  self.imu_msg.header = self.h
724  if self.imu_pub is None:
725  self.imu_pub = rospy.Publisher('imu/data', Imu, queue_size=10)
726  self.imu_pub.publish(self.imu_msg)
727  if self.pub_raw_gps:
728  self.raw_gps_msg.header = self.h
729  if self.raw_gps_pub is None:
730  self.raw_gps_pub = rospy.Publisher('raw_fix', NavSatFix, queue_size=10)
731  self.raw_gps_pub.publish(self.raw_gps_msg)
732  if self.pub_pos_gps:
733  self.pos_gps_msg.header = self.h
734  if self.pos_gps_pub is None:
735  self.pos_gps_pub = rospy.Publisher('fix', NavSatFix, queue_size=10)
736  self.pos_gps_pub.publish(self.pos_gps_msg)
737  if self.pub_vel:
738  self.vel_msg.header = self.h
739  if self.vel_pub is None:
740  self.vel_pub = rospy.Publisher('velocity', TwistStamped,
741  queue_size=10)
742  self.vel_pub.publish(self.vel_msg)
743  if self.pub_mag:
744  self.mag_msg.header = self.h
745  if self.mag_pub is None:
746  self.mag_pub = rospy.Publisher('imu/mag', MagneticField,
747  queue_size=10)
748  self.mag_pub.publish(self.mag_msg)
749  if self.pub_temp:
750  self.temp_msg.header = self.h
751  if self.temp_pub is None:
752  self.temp_pub = rospy.Publisher('temperature', Temperature,
753  queue_size=10)
754  self.temp_pub.publish(self.temp_msg)
755  if self.pub_press:
756  self.press_msg.header = self.h
757  if self.press_pub is None:
758  self.press_pub = rospy.Publisher('pressure', FluidPressure,
759  queue_size=10)
760  self.press_pub.publish(self.press_msg)
761  if self.pub_anin1:
762  if self.analog_in1_pub is None:
763  self.analog_in1_pub = rospy.Publisher('analog_in1',
764  UInt16, queue_size=10)
765  self.analog_in1_pub.publish(self.anin1_msg)
766  if self.pub_anin2:
767  if self.analog_in2_pub is None:
768  self.analog_in2_pub = rospy.Publisher('analog_in2', UInt16,
769  queue_size=10)
770  self.analog_in2_pub.publish(self.anin2_msg)
771  if self.pub_ecef:
772  self.ecef_msg.header = self.h
773  if self.ecef_pub is None:
774  self.ecef_pub = rospy.Publisher('ecef', PointStamped,
775  queue_size=10)
776  self.ecef_pub.publish(self.ecef_msg)
777  if self.pub_diag:
778  self.diag_msg.header = self.h
779  if self.diag_pub is None:
780  self.diag_pub = rospy.Publisher('/diagnostics', DiagnosticArray,
781  queue_size=10)
782  self.diag_pub.publish(self.diag_msg)
783  # publish string representation
784  self.str_pub.publish(str(data))
785 
786 
787 def main():
788  '''Create a ROS node and instantiate the class.'''
789  rospy.init_node('xsens_driver')
790  driver = XSensDriver()
791  driver.spin()
792 
793 
794 if __name__ == '__main__':
795  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:1135
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:1118
mtnode.main
def main()
Definition: mtnode.py:787
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 Thu Jun 6 2019 19:13:05