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

xsens_driver
Author(s):
autogenerated on Tue May 2 2023 02:05:55