2 import roslib; roslib.load_manifest(
'xsens_driver')
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
21 from math
import radians, sqrt, atan2
22 from tf.transformations
import quaternion_from_matrix, quaternion_from_euler,\
28 v = rospy.get_param(name)
29 rospy.loginfo(
"Found parameter: %s, value: %s" % (name, str(v)))
32 rospy.logwarn(
"Cannot find value for parameter: %s, assigning " 33 "default: %s" % (name, str(v)))
39 if len(default) != len(value):
40 rospy.logfatal(
"Parameter %s should be a list of size %d", name, len(default))
49 matrix[i*n + i] = diagonal[i]
59 initial_wait =
get_param(
'~initial_wait', 0.1)
62 initial_wait=initial_wait)
64 device, baudrate = devs[0]
65 rospy.loginfo(
"Detected MT device on port %s @ %d bps" 68 rospy.logerr(
"Fatal: could not find proper MT device.")
69 rospy.signal_shutdown(
"Could not find proper MT device.")
73 initial_wait=initial_wait)
75 rospy.logerr(
"Fatal: could not find proper baudrate.")
76 rospy.signal_shutdown(
"Could not find proper baudrate.")
79 rospy.loginfo(
"MT node interface: %s at %d bd." % (device, baudrate))
81 initial_wait=initial_wait)
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)
97 get_param_list(
'~angular_velocity_covariance_diagonal', [radians(0.025)] * 3)
100 get_param_list(
'~linear_acceleration_covariance_diagonal', [0.0004] * 3)
103 get_param_list(
"~orientation_covariance_diagonal", [radians(1.), radians(1.), radians(9.)])
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')
132 self.
str_pub = rospy.Publisher(
'imu_data_str', String, queue_size=10)
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
149 self.mag_msg.magnetic_field_covariance = (0, )*9
152 self.temp_msg.variance = 0.
155 self.press_msg.variance = 0.
167 while not rospy.is_shutdown():
172 except (select.error, OSError, serial.serialutil.SerialException):
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.""" 184 elif source ==
'NWU':
193 def convert_quat(q, source, dest=self.frame_local):
194 """Convert a quaternion between ENU, NED, and NWU.""" 196 """Quaternion multiplication.""" 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
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)
214 return q_mult(q_enu_ned, q)
216 return q_mult(q_enu_nwu, q)
217 elif source ==
'NED':
219 return q_mult(q_ned_enu, q)
223 return q_mult(q_ned_nwu, q)
224 elif source ==
'NWU':
226 return q_mult(q_nwu_enu, q)
228 return q_mult(q_nwu_ned, q)
232 def publish_time_ref(secs, nsecs, source):
233 """Publish a time reference.""" 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)
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.""" 249 stamp_day = datetime.datetime(y, m, d)
250 elif week
is not None:
251 epoch = datetime.datetime(1980, 1, 6)
252 stamp_day = epoch + datetime.timedelta(weeks=week)
254 today = datetime.date.today()
255 stamp_day = datetime.datetime(today.year, today.month,
257 iso_day = stamp_day.isoweekday()
259 start_of_week = stamp_day - datetime.timedelta(days=iso_day)
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
272 def fill_from_RAW(raw_data):
273 '''Fill messages with information from 'raw' MTData block.''' 276 rospy.loginfo(
"Got MTi data packet: 'RAW', ignored!")
278 def fill_from_RAWGPS(rawgps_data):
279 '''Fill messages with information from 'rawgps' MTData block.''' 280 if rawgps_data[
'bGPS'] < self.
old_bGPS:
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
289 def fill_from_Temp(temp):
290 '''Fill messages with information from 'temperature' MTData block. 293 self.temp_msg.temperature = temp
295 def fill_from_Calib(imu_data):
296 '''Fill messages with information from 'calibrated' MTData block. 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
306 self.vel_msg.twist.angular.x = x
307 self.vel_msg.twist.angular.y = y
308 self.vel_msg.twist.angular.z = z
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
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
329 def fill_from_Orient(orient_data):
330 '''Fill messages with information from 'orientation' MTData block. 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
351 def fill_from_Auxiliary(aux_data):
352 '''Fill messages with information from 'Auxiliary' MTData block.''' 354 self.anin1_msg.data = o[
'Ain_1']
359 self.anin2_msg.data = o[
'Ain_2']
364 def fill_from_Pos(position_data):
365 '''Fill messages with information from 'position' MTData block.''' 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']
371 def fill_from_Vel(velocity_data):
372 '''Fill messages with information from 'velocity' MTData block.''' 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
381 def fill_from_Stat(status):
382 '''Fill messages with information from 'status' MTData block.''' 385 self.stest_stat.level = DiagnosticStatus.OK
386 self.stest_stat.message =
"Ok" 388 self.stest_stat.level = DiagnosticStatus.ERROR
389 self.stest_stat.message =
"Failed" 391 self.xkf_stat.level = DiagnosticStatus.OK
392 self.xkf_stat.message =
"Valid" 394 self.xkf_stat.level = DiagnosticStatus.WARN
395 self.xkf_stat.message =
"Invalid" 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
402 self.pos_gps_msg.status.status = NavSatStatus.STATUS_FIX
403 self.pos_gps_msg.status.service = NavSatStatus.SERVICE_GPS
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
410 self.pos_gps_msg.status.status = NavSatStatus.STATUS_NO_FIX
411 self.pos_gps_msg.status.service = 0
413 def fill_from_Sample(ts):
414 '''Catch 'Sample' MTData blocks.''' 418 def fill_from_Temperature(o):
419 '''Fill messages with information from 'Temperature' MTData2 block. 422 self.temp_msg.temperature = o[
'Temp']
424 def fill_from_Timestamp(o):
425 '''Fill messages with information from 'Timestamp' MTData2 block. 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']
432 secs = calendar.timegm((y, m, d, hr, mi, s, 0, 0, 0))
433 publish_time_ref(secs, ns,
'UTC time')
437 itow = o[
'TimeOfWeek']
438 secs, nsecs = stamp_from_itow(itow)
439 publish_time_ref(secs, nsecs,
'integer time of week')
443 sample_time_fine = o[
'SampleTimeFine']
445 secs = int(sample_time_fine / 10000)
446 nsecs = 1e5 * (sample_time_fine % 10000)
447 publish_time_ref(secs, nsecs,
'sample time fine')
451 sample_time_coarse = o[
'SampleTimeCoarse']
452 publish_time_ref(sample_time_coarse, 0,
'sample time coarse')
458 def fill_from_Orientation_Data(o):
459 '''Fill messages with information from 'Orientation Data' MTData2 463 x, y, z, w = o[
'Q1'], o[
'Q2'], o[
'Q3'], o[
'Q0']
467 x, y, z, w = quaternion_from_euler(radians(o[
'Roll']),
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)
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
487 def fill_from_Pressure(o):
488 '''Fill messages with information from 'Pressure' MTData2 block.''' 489 self.press_msg.fluid_pressure = o[
'Pressure']
492 def fill_from_Acceleration(o):
493 '''Fill messages with information from 'Acceleration' MTData2 499 x, y, z = o[
'Delta v.x'], o[
'Delta v.y'], o[
'Delta v.z']
503 x, y, z = o[
'freeAccX'], o[
'freeAccY'], o[
'freeAccZ']
507 x, y, z = o[
'accX'], o[
'accY'], o[
'accZ']
510 self.imu_msg.linear_acceleration.x = x
511 self.imu_msg.linear_acceleration.y = y
512 self.imu_msg.linear_acceleration.z = z
515 def fill_from_Position(o):
516 '''Fill messages with information from 'Position' MTData2 block.''' 518 self.pos_gps_msg.latitude = o[
'lat']
519 self.pos_gps_msg.longitude = o[
'lon']
521 alt = o.get(
'altEllipsoid', o.get(
'altMsl', 0))
522 self.pos_gps_msg.altitude = alt
527 x, y, z = o[
'ecefX'], o[
'ecefY'], o[
'ecefZ']
529 self.ecef_msg.point.x = x
530 self.ecef_msg.point.y = y
531 self.ecef_msg.point.z = z
536 def fill_from_GNSS(o):
537 '''Fill messages with information from 'GNSS' MTData2 block.''' 540 itow, y, m, d, ns, f = o[
'itow'], o[
'year'], o[
'month'],\
541 o[
'day'], o[
'nano'], o[
'valid']
543 secs, nsecs = stamp_from_itow(itow, y, m, d, ns)
544 publish_time_ref(secs, nsecs,
'GNSS time UTC')
546 fixtype = o[
'fixtype']
549 self.raw_gps_msg.status.status = NavSatStatus.STATUS_NO_FIX
550 self.raw_gps_msg.status.service = 0
553 self.raw_gps_msg.status.status = NavSatStatus.STATUS_FIX
554 self.raw_gps_msg.status.service = NavSatStatus.SERVICE_GPS
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
568 def fill_from_Angular_Velocity(o):
569 '''Fill messages with information from 'Angular Velocity' MTData2 572 dqw, dqx, dqy, dqz = (o[
'Delta q0'], o[
'Delta q1'],
573 o[
'Delta q2'], o[
'Delta q3'])
574 now = rospy.Time.now()
591 ca_2, sa_2 = dqw, sqrt(dqx**2 + dqy**2 + dqz**2)
592 ca = ca_2**2 - sa_2**2
594 rotation_angle = atan2(sa, ca)
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
604 self.vel_msg.twist.angular.x = x
605 self.vel_msg.twist.angular.y = y
606 self.vel_msg.twist.angular.z = z
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
617 self.vel_msg.twist.angular.x = x
618 self.vel_msg.twist.angular.y = y
619 self.vel_msg.twist.angular.z = z
624 def fill_from_GPS(o):
625 '''Fill messages with information from 'GPS' MTData2 block.''' 628 x, y, z = o[
'ecefX'], o[
'ecefY'], o[
'ecefZ']
629 self.ecef_msg.point.x = x * 0.01
630 self.ecef_msg.point.y = y * 0.01
631 self.ecef_msg.point.z = z * 0.01
633 vx, vy, vz = o[
'ecefVX'], o[
'ecefVY'], o[
'ecefVZ']
634 self.vel_msg.twist.linear.x = vx * 0.01
635 self.vel_msg.twist.linear.y = vy * 0.01
636 self.vel_msg.twist.linear.z = vz * 0.01
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')
647 itow, y, m, d, ns, f = o[
'iTOW'], o[
'year'], o[
'month'],\
648 o[
'day'], o[
'nano'], o[
'valid']
650 secs, nsecs = stamp_from_itow(itow, y, m, d, ns)
651 publish_time_ref(secs, nsecs,
'GPS Time UTC')
656 def fill_from_SCR(o):
657 '''Fill messages with information from 'SCR' MTData2 block.''' 661 def fill_from_Analog_In(o):
662 '''Fill messages with information from 'Analog In' MTData2 block. 665 self.anin1_msg.data = o[
'analogIn1']
666 self.pub_anin1 =
True 670 self.anin2_msg.data = o[
'analogIn2']
671 self.pub_anin2 =
True 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
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'],
687 self.vel_msg.twist.linear.x = x
688 self.vel_msg.twist.linear.y = y
689 self.vel_msg.twist.linear.z = z
692 def fill_from_Status(o):
693 '''Fill messages with information from 'Status' MTData2 block.''' 695 status = o[
'StatusByte']
696 fill_from_Stat(status)
700 status = o[
'StatusWord']
701 fill_from_Stat(status)
705 def find_handler_name(name):
706 return "fill_from_%s" % (name.replace(
" ",
"_"))
710 data = self.mt.read_measurement()
716 self.h.stamp = rospy.Time.now()
723 for n, o
in data.items():
725 locals()[find_handler_name(n)](o)
727 rospy.logwarn(
"Unknown MTi data packet: '%s', ignoring." % n)
731 self.imu_msg.header = self.
h 733 self.
imu_pub = rospy.Publisher(
'imu/data', Imu, queue_size=10)
734 self.imu_pub.publish(self.
imu_msg)
736 self.raw_gps_msg.header = self.
h 738 self.
raw_gps_pub = rospy.Publisher(
'raw_fix', NavSatFix, queue_size=10)
741 self.pos_gps_msg.header = self.
h 743 self.
pos_gps_pub = rospy.Publisher(
'fix', NavSatFix, queue_size=10)
746 self.vel_msg.header = self.
h 748 self.
vel_pub = rospy.Publisher(
'velocity', TwistStamped,
750 self.vel_pub.publish(self.
vel_msg)
752 self.mag_msg.header = self.
h 754 self.
mag_pub = rospy.Publisher(
'imu/mag', MagneticField,
756 self.mag_pub.publish(self.
mag_msg)
758 self.temp_msg.header = self.
h 760 self.
temp_pub = rospy.Publisher(
'temperature', Temperature,
762 self.temp_pub.publish(self.
temp_msg)
764 self.press_msg.header = self.
h 766 self.
press_pub = rospy.Publisher(
'pressure', FluidPressure,
772 UInt16, queue_size=10)
773 self.analog_in1_pub.publish(self.
anin1_msg)
778 self.analog_in2_pub.publish(self.
anin2_msg)
780 self.ecef_msg.header = self.
h 782 self.
ecef_pub = rospy.Publisher(
'ecef', PointStamped,
784 self.ecef_pub.publish(self.
ecef_msg)
786 self.diag_msg.header = self.
h 788 self.
diag_pub = rospy.Publisher(
'/diagnostics',
789 DiagnosticArray, queue_size=10)
790 self.diag_pub.publish(self.
diag_msg)
792 self.str_pub.publish(str(data))
796 '''Create a ROS node and instantiate the class.''' 797 rospy.init_node(
'xsens_driver')
802 if __name__ ==
'__main__':
def get_param_list(name, default)
def find_baudrate(port, timeout=0.002, verbose=False, initial_wait=0.1)
Auto detect baudrate.
def matrix_from_diagonal(diagonal)
angular_velocity_covariance
def get_param(name, default)
def find_devices(timeout=0.002, verbose=False, initial_wait=0.1)
linear_acceleration_covariance