def __init__(self):
device = get_param('~device', 'auto')
baudrate = get_param('~baudrate', 0)
timeout = get_param('~timeout', 0.002)
if device == 'auto':
devs = mtdevice.find_devices()
if devs:
device, baudrate = devs[0]
rospy.loginfo("Detected MT device on port %s @ %d bps" %
(device, baudrate))
else:
rospy.logerr("Fatal: could not find proper MT device.")
rospy.signal_shutdown("Could not find proper MT device.")
return
if not baudrate:
baudrate = mtdevice.find_baudrate(device)
if not baudrate:
rospy.logerr("Fatal: could not find proper baudrate.")
rospy.signal_shutdown("Could not find proper baudrate.")
return
rospy.loginfo("MT node interface: %s at %d bd." % (device, baudrate))
self.mt = mtdevice.MTDevice(device, baudrate, timeout)
self.frame_id = get_param('~frame_id', '/base_imu')
self.frame_local = get_param('~frame_local', 'ENU')
self.diag_msg = DiagnosticArray()
self.stest_stat = DiagnosticStatus(name='mtnode: Self Test',
level=1,
message='No status information')
self.xkf_stat = DiagnosticStatus(name='mtnode: XKF Valid',
level=1,
message='No status information')
self.gps_stat = DiagnosticStatus(name='mtnode: GPS Fix',
level=1,
message='No status information')
self.diag_msg.status = [self.stest_stat, self.xkf_stat, self.gps_stat]
# publishers created at first use to reduce topic clutter
self.diag_pub = None
self.imu_pub = None
self.gps_pub = None
self.vel_pub = None
self.mag_pub = None
self.temp_pub = None
self.press_pub = None
self.analog_in1_pub = None # decide type+header
self.analog_in2_pub = None # decide type+header
self.ecef_pub = None
self.time_ref_pub = None
# TODO pressure, ITOW from raw GPS?
self.old_bGPS = 256 # publish GPS only if new
# publish a string version of all data; to be parsed by clients
self.str_pub = rospy.Publisher('imu_data_str', String, queue_size=10)
def __init__(self):
device = get_param('~device', 'auto')
baudrate = get_param('~baudrate', 0)
if device == 'auto':
devs = mtdevice.find_devices()
if devs:
device, baudrate = devs[0]
rospy.loginfo("Detected MT device on port %s @ %d bps" %
(device, baudrate))
else:
rospy.logerr("Fatal: could not find proper MT device.")
rospy.signal_shutdown("Could not find proper MT device.")
return
if not baudrate:
baudrate = mtdevice.find_baudrate(device)
if not baudrate:
rospy.logerr("Fatal: could not find proper baudrate.")
rospy.signal_shutdown("Could not find proper baudrate.")
return
rospy.loginfo("MT node interface: %s at %d bd." % (device, baudrate))
self.mt = mtdevice.MTDevice(device, baudrate)
self.frame_id = get_param('~frame_id', '/base_imu')
self.diag_pub = rospy.Publisher('/diagnostics', DiagnosticArray)
self.diag_msg = DiagnosticArray()
self.stest_stat = DiagnosticStatus(name='mtnode: Self Test',
level=1,
message='No status information')
self.xkf_stat = DiagnosticStatus(name='mtnode: XKF Valid',
level=1,
message='No status information')
self.gps_stat = DiagnosticStatus(name='mtnode: GPS Fix',
level=1,
message='No status information')
self.diag_msg.status = [self.stest_stat, self.xkf_stat, self.gps_stat]
self.imu_pub = rospy.Publisher('imu/data', Imu)
self.gps_pub = rospy.Publisher('fix', NavSatFix)
self.xgps_pub = rospy.Publisher('fix_extended', GPSFix)
self.vel_pub = rospy.Publisher('velocity', TwistStamped)
self.mag_pub = rospy.Publisher('magnetic', Vector3Stamped)
self.temp_pub = rospy.Publisher('temperature',
Float32) # decide type+header
self.press_pub = rospy.Publisher('pressure',
Float32) # decide type+header
self.analog_in1_pub = rospy.Publisher('analog_in1',
UInt16) # decide type+header
self.analog_in2_pub = rospy.Publisher('analog_in2',
UInt16) # decide type+header
# TODO pressure, ITOW from raw GPS?
self.old_bGPS = 256 # publish GPS only if new
# publish a string version of all data; to be parsed by clients
self.str_pub = rospy.Publisher('imu_data_str', String)
def __init__(self):
device = get_param('~device', 'auto')
baudrate = get_param('~baudrate', 0)
timeout = get_param('~timeout', 0.002)
if device == 'auto':
devs = mtdevice.find_devices()
if devs:
device, baudrate = devs[0]
rospy.loginfo("Detected MT device on port %s @ %d bps"
% (device, baudrate))
else:
rospy.logerr("Fatal: could not find proper MT device.")
rospy.signal_shutdown("Could not find proper MT device.")
return
if not baudrate:
baudrate = mtdevice.find_baudrate(device)
if not baudrate:
rospy.logerr("Fatal: could not find proper baudrate.")
rospy.signal_shutdown("Could not find proper baudrate.")
return
rospy.loginfo("MT node interface: %s at %d bd." % (device, baudrate))
self.mt = mtdevice.MTDevice(device, baudrate, timeout)
self.frame_id = get_param('~frame_id', '/base_imu')
self.frame_local = get_param('~frame_local', 'ENU')
self.diag_msg = DiagnosticArray()
self.stest_stat = DiagnosticStatus(name='mtnode: Self Test', level=1,
message='No status information')
self.xkf_stat = DiagnosticStatus(name='mtnode: XKF Valid', level=1,
message='No status information')
self.gps_stat = DiagnosticStatus(name='mtnode: GPS Fix', level=1,
message='No status information')
self.diag_msg.status = [self.stest_stat, self.xkf_stat, self.gps_stat]
# publishers created at first use to reduce topic clutter
self.diag_pub = None
self.imu_pub = None
self.gps_pub = None
self.vel_pub = None
self.mag_pub = None
self.temp_pub = None
self.press_pub = None
self.analog_in1_pub = None # decide type+header
self.analog_in2_pub = None # decide type+header
self.ecef_pub = None
self.time_ref_pub = None
# TODO pressure, ITOW from raw GPS?
self.old_bGPS = 256 # publish GPS only if new
# publish a string version of all data; to be parsed by clients
self.str_pub = rospy.Publisher('imu_data_str', String, queue_size=10)
def __init__(self):
device = get_param('~device', 'auto')
baudrate = get_param('~baudrate', 0)
fName = get_param('~fname','ins.csv')
#fName = "/home/user1/Projects/xsenslog.txt"
if device=='auto':
devs = mtdevice.find_devices()
if devs:
device, baudrate = devs[0]
rospy.loginfo("Detected MT device on port %s @ %d bps"%(device,
baudrate))
else:
rospy.logerr("Fatal: could not find proper MT device.")
rospy.signal_shutdown("Could not find proper MT device.")
return
if not baudrate:
baudrate = mtdevice.find_baudrate(device)
if not baudrate:
rospy.logerr("Fatal: could not find proper baudrate.")
rospy.signal_shutdown("Could not find proper baudrate.")
return
rospy.loginfo("MT node interface: %s at %d bd."%(device, baudrate))
self.mt = mtdevice.MTDevice(device, baudrate)
self.frame_id = get_param('~frame_id', '/base_imu')
self.diag_pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=10)
self.diag_msg = DiagnosticArray()
self.stest_stat = DiagnosticStatus(name='mtnode: Self Test', level=1,
message='No status information')
self.xkf_stat = DiagnosticStatus(name='mtnode: XKF Valid', level=1,
message='No status information')
self.gps_stat = DiagnosticStatus(name='mtnode: GPS Fix', level=1,
message='No status information')
self.diag_msg.status = [self.stest_stat, self.xkf_stat, self.gps_stat]
self.imu_pub = rospy.Publisher('imu/data', Imu, queue_size=10)
self.gps_pub = rospy.Publisher('fix', NavSatFix, queue_size=10)
self.xgps_pub = rospy.Publisher('fix_extended', GPSFix, queue_size=10)
self.vel_pub = rospy.Publisher('velocity', TwistStamped, queue_size=10)
self.mag_pub = rospy.Publisher('magnetic', Vector3Stamped, queue_size=10)
self.temp_pub = rospy.Publisher('temperature', Float32, queue_size=10) # decide type+header
self.press_pub = rospy.Publisher('pressure', Float32, queue_size=10) # decide type+header
self.analog_in1_pub = rospy.Publisher('analog_in1', UInt16, queue_size=10) # decide type+header
self.analog_in2_pub = rospy.Publisher('analog_in2', UInt16, queue_size=10) # decide type+header
# TODO pressure, ITOW from raw GPS?
self.old_bGPS = 256 # publish GPS only if new
# publish a string version of all data; to be parsed by clients
self.str_pub = rospy.Publisher('imu_data_str', String, queue_size=10)
self.outFile = open(fName, "w")
def __init__(self):
device = get_param('~device', 'auto')
baudrate = get_param('~baudrate', 0)
if device=='auto':
devs = mtdevice.find_devices()
if devs:
device, baudrate = devs[0]
rospy.loginfo("Detected MT device on port %s @ %d bps"%(device,
baudrate))
else:
rospy.logerr("Fatal: could not find proper MT device.")
rospy.signal_shutdown("Could not find proper MT device.")
return
if not baudrate:
baudrate = mtdevice.find_baudrate(device)
if not baudrate:
rospy.logerr("Fatal: could not find proper baudrate.")
rospy.signal_shutdown("Could not find proper baudrate.")
return
rospy.loginfo("MT node interface: %s at %d bd."%(device, baudrate))
self.mt = mtdevice.MTDevice(device, baudrate)
self.mt.auto_config()
self.frame_id = get_param('~frame_id', '/base_imu')
self.diag_pub = rospy.Publisher('/diagnostics', DiagnosticArray)
self.diag_msg = DiagnosticArray()
self.stest_stat = DiagnosticStatus(name='mtnode: Self Test', level=1,
message='No status information')
self.xkf_stat = DiagnosticStatus(name='mtnode: XKF Valid', level=1,
message='No status information')
self.gps_stat = DiagnosticStatus(name='mtnode: GPS Fix', level=1,
message='No status information')
self.diag_msg.status = [self.stest_stat, self.xkf_stat, self.gps_stat]
self.imu_pub = rospy.Publisher('imu/data', Imu)
self.gps_pub = rospy.Publisher('fix', NavSatFix)
self.xgps_pub = rospy.Publisher('fix_extended', GPSFix)
self.vel_pub = rospy.Publisher('velocity', TwistStamped)
self.mag_pub = rospy.Publisher('magnetic', Vector3Stamped)
self.temp_pub = rospy.Publisher('temperature', Float32) # decide type
# TODO pressure, ITOW from raw GPS?
self.old_bGPS = 256 # publish GPS only if new
def __init__(self):
device = 'auto'
baudrate = 115200
timeout = 0.05
if device == 'auto':
devs = mtdevice.find_devices()
if devs:
device, baudrate = devs[0]
print("Detected MT device on port %s @ %d bps" %
(device, baudrate))
else:
print("Fatal: could not find proper MT device.")
print("Could not find proper MT device.")
return
if not baudrate:
baudrate = mtdevice.find_baudrate(device)
if not baudrate:
print("Fatal: could not find proper baudrate.")
print("Could not find proper baudrate.")
return
print("MT node interface: %s at %d bd." % (device, baudrate))
# Enable config mode
self.mt = mtdevice.MTDevice(device, baudrate, timeout, True, True,
False)
# Configure (see bottom of mtdevice.py)
# mf100fe = magnetometer at 100hz
# wr2000de = angular vel at 2000hz
# aa2000de = linear acceleration 2000hz
# oq400fe = orientation at 400hz
# pl400fe = position (lat long) 400hz
output_config = mtdevice.get_output_config(
'mf100fe,wr2000de,aa2000de,oq400de,pl400fe')
print "Changing output configuration",
self.mt.SetOutputConfiguration(output_config)
print "System is Ok, Ready to Record."
def __init__(self):
device = get_param('~device', 'auto')
baudrate = get_param('~baudrate', 0)
if device == 'auto':
devs = mtdevice.find_devices()
if devs:
device, baudrate = devs[0]
rospy.loginfo("Detected MT device on port %s @ %d bps" %
(device, baudrate))
else:
rospy.logerr("Fatal: could not find proper MT device.")
rospy.signal_shutdown("Could not find proper MT device.")
return
if not baudrate:
baudrate = mtdevice.find_baudrate(device)
if not baudrate:
rospy.logerr("Fatal: could not find proper baudrate.")
rospy.signal_shutdown("Could not find proper baudrate.")
return
rospy.loginfo("MT node interface: %s at %d bd." % (device, baudrate))
self.mt = mtdevice.MTDevice(device, baudrate)
self.frame_id = get_param('~frame_id', 'mti/data')
frame_local = get_param('~frame_local', 'ENU')
frame_local_imu = get_param('~frame_local_imu', 'ENU')
if frame_local == 'ENU':
R = XSensDriver.ENU
elif frame_local == 'NED':
R = XSensDriver.NED
elif frame_local == 'NWU':
R = XSensDriver.NWU
if frame_local_imu == 'ENU':
R_IMU = XSensDriver.ENU
elif frame_local_imu == 'NED':
R_IMU = XSensDriver.NED
elif frame_local_imu == 'NWU':
R_IMU = XSensDriver.NWU
self.R = R.dot(R_IMU.transpose())
self.diag_pub = rospy.Publisher('/diagnostics',
DiagnosticArray,
queue_size=10)
self.diag_msg = DiagnosticArray()
self.stest_stat = DiagnosticStatus(name='mtnode: Self Test',
level=1,
message='No status information')
self.xkf_stat = DiagnosticStatus(name='mtnode: XKF Valid',
level=1,
message='No status information')
self.gps_stat = DiagnosticStatus(name='mtnode: GPS Fix',
level=1,
message='No status information')
self.diag_msg.status = [self.stest_stat, self.xkf_stat, self.gps_stat]
# self.imu_pub = rospy.Publisher('mti/sensor/imu', Imu, queue_size=10) #IMU message
self.imu_pub = rospy.Publisher('imu_raw', Imu,
queue_size=10) #IMU message
self.ss_pub = rospy.Publisher('mti/sensor/sample',
sensorSample,
queue_size=10) # sensorSample
self.mag_pub = rospy.Publisher('mti/sensor/magnetic',
Vector3Stamped,
queue_size=10) # magnetic
self.baro_pub = rospy.Publisher('mti/sensor/pressure',
baroSample,
queue_size=10) # baro
self.gnssPvt_pub = rospy.Publisher('mti/sensor/gnssPvt',
gnssSample,
queue_size=10) # GNSS PVT
#self.gnssSatinfo_pub = rospy.Publisher('mti/sensor/gnssStatus', GPSStatus, queue_size=10) # GNSS SATINFO
self.ori_pub = rospy.Publisher('mti/filter/orientation',
orientationEstimate,
queue_size=10) # XKF/XEE orientation
self.vel_pub = rospy.Publisher('mti/filter/velocity',
velocityEstimate,
queue_size=10) # XKF/XEE velocity
self.pos_pub = rospy.Publisher('mti/filter/position',
positionEstimate,
queue_size=10) # XKF/XEE position
self.temp_pub = rospy.Publisher('temperature', Float32,
queue_size=10) # decide type
def __init__(self):
device = get_param('~device', 'auto')
baudrate = get_param('~baudrate', 0)
timeout = get_param('~timeout', 0.002)
initial_wait = get_param('~initial_wait', 0.1)
if device == 'auto':
devs = mtdevice.find_devices(timeout=timeout,
initial_wait=initial_wait)
if devs:
device, baudrate = devs[0]
rospy.loginfo("Detected MT device on port %s @ %d bps" %
(device, baudrate))
else:
rospy.logerr("Fatal: could not find proper MT device.")
rospy.signal_shutdown("Could not find proper MT device.")
return
if not baudrate:
baudrate = mtdevice.find_baudrate(device,
timeout=timeout,
initial_wait=initial_wait)
if not baudrate:
rospy.logerr("Fatal: could not find proper baudrate.")
rospy.signal_shutdown("Could not find proper baudrate.")
return
rospy.loginfo("MT node interface: %s at %d bd." % (device, baudrate))
self.mt = mtdevice.MTDevice(device,
baudrate,
timeout,
initial_wait=initial_wait)
# optional no rotation procedure for internal calibration of biases
# (only mark iv devices)
no_rotation_duration = get_param('~no_rotation_duration', 0)
if no_rotation_duration:
rospy.loginfo(
"Starting the no-rotation procedure to estimate the "
"gyroscope biases for %d s. Please don't move the IMU"
" during this time." % no_rotation_duration)
self.mt.SetNoRotation(no_rotation_duration)
self.frame_id = get_param('~frame_id', '/base_imu')
self.frame_local = get_param('~frame_local', 'ENU')
self.angular_velocity_covariance = matrix_from_diagonal(
get_param_list('~angular_velocity_covariance_diagonal',
[radians(0.025)] * 3))
self.linear_acceleration_covariance = matrix_from_diagonal(
get_param_list('~linear_acceleration_covariance_diagonal',
[0.0004] * 3))
self.orientation_covariance = matrix_from_diagonal(
get_param_list("~orientation_covariance_diagonal",
[radians(1.), radians(1.),
radians(9.)]))
self.diag_msg = DiagnosticArray()
self.stest_stat = DiagnosticStatus(name='mtnode: Self Test',
level=1,
message='No status information')
self.xkf_stat = DiagnosticStatus(name='mtnode: XKF Valid',
level=1,
message='No status information')
self.gps_stat = DiagnosticStatus(name='mtnode: GPS Fix',
level=1,
message='No status information')
self.diag_msg.status = [self.stest_stat, self.xkf_stat, self.gps_stat]
# publishers created at first use to reduce topic clutter
self.diag_pub = None
self.imu_pub = None
self.raw_gps_pub = None
self.vel_pub = None
self.mag_pub = None
self.pos_gps_pub = None
self.temp_pub = None
self.press_pub = None
self.analog_in1_pub = None # decide type+header
self.analog_in2_pub = None # decide type+header
self.ecef_pub = None
self.time_ref_pub = None
# TODO pressure, ITOW from raw GPS?
self.old_bGPS = 256 # publish GPS only if new
# publish a string version of all data; to be parsed by clients
self.str_pub = rospy.Publisher('imu_data_str', String, queue_size=10)
self.last_delta_q_time = None
self.delta_q_rate = None
def __init__(self):
device = get_param('~device', 'auto')
baudrate = get_param('~baudrate', 0)
if device=='auto':
devs = mtdevice.find_devices()
if devs:
device, baudrate = devs[0]
rospy.loginfo("Detected MT device on port %s @ %d bps"%(device,
baudrate))
else:
rospy.logerr("Fatal: could not find proper MT device.")
rospy.signal_shutdown("Could not find proper MT device.")
return
if not baudrate:
baudrate = mtdevice.find_baudrate(device)
if not baudrate:
rospy.logerr("Fatal: could not find proper baudrate.")
rospy.signal_shutdown("Could not find proper baudrate.")
return
rospy.loginfo("MT node interface: %s at %d bd."%(device, baudrate))
self.mt = mtdevice.MTDevice(device, baudrate)
self.frame_id = get_param('~frame_id', '/mti/data')
frame_local = get_param('~frame_local' , 'ENU')
frame_local_imu = get_param('~frame_local_imu', 'ENU')
if frame_local == 'ENU':
R = XSensDriver.ENU
elif frame_local == 'NED':
R = XSensDriver.NED
elif frame_local == 'NWU':
R = XSensDriver.NWU
if frame_local_imu == 'ENU':
R_IMU = XSensDriver.ENU
elif frame_local_imu == 'NED':
R_IMU = XSensDriver.NED
elif frame_local_imu == 'NWU':
R_IMU = XSensDriver.NWU
self.R = R.dot(R_IMU.transpose())
self.diag_pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=10)
self.diag_msg = DiagnosticArray()
self.stest_stat = DiagnosticStatus(name='mtnode: Self Test', level=1,
message='No status information')
self.xkf_stat = DiagnosticStatus(name='mtnode: XKF Valid', level=1,
message='No status information')
self.gps_stat = DiagnosticStatus(name='mtnode: GPS Fix', level=1,
message='No status information')
self.diag_msg.status = [self.stest_stat, self.xkf_stat, self.gps_stat]
self.imu_pub = rospy.Publisher('mti/sensor/imu', Imu, queue_size=10) #IMU message
self.ss_pub = rospy.Publisher('mti/sensor/sample', sensorSample, queue_size=10) # sensorSample
self.mag_pub = rospy.Publisher('mti/sensor/magnetic', Vector3Stamped, queue_size=10) # magnetic
self.baro_pub = rospy.Publisher('mti/sensor/pressure', baroSample, queue_size=10) # baro
self.gnssPvt_pub = rospy.Publisher('mti/sensor/gnssPvt', gnssSample, queue_size=10) # GNSS PVT
#self.gnssSatinfo_pub = rospy.Publisher('mti/sensor/gnssStatus', GPSStatus, queue_size=10) # GNSS SATINFO
self.ori_pub = rospy.Publisher('mti/filter/orientation', orientationEstimate, queue_size=10) # XKF/XEE orientation
self.vel_pub = rospy.Publisher('mti/filter/velocity', velocityEstimate, queue_size=10) # XKF/XEE velocity
self.pos_pub = rospy.Publisher('mti/filter/position', positionEstimate, queue_size=10) # XKF/XEE position
self.temp_pub = rospy.Publisher('temperature', Float32, queue_size=10) # decide type
def __init__(self):
device = get_param('~device', 'auto')
baudrate = get_param('~baudrate', 0)
if device == 'auto':
devs = mtdevice.find_devices()
if devs:
device, baudrate = devs[0]
rospy.loginfo("Detected MT device on port %s @ %d bps" %
(device, baudrate))
else:
rospy.logerr("Fatal: could not find proper MT device.")
rospy.signal_shutdown("Could not find proper MT device.")
return
if not baudrate:
baudrate = mtdevice.find_baudrate(device)
if not baudrate:
rospy.logerr("Fatal: could not find proper baudrate.")
rospy.signal_shutdown("Could not find proper baudrate.")
return
# Create our device
rospy.loginfo("MT node interface: %s at %d bd." % (device, baudrate))
self.mt = mtdevice.MTDevice(device, baudrate)
# Configure the device (1=all, 2=rate, 3=filter)
#self.mt.configureMti(200,1)
# Get config values from ROS
self.frame_id = get_param('~frame_id', 'mti/data')
self.use_rostime = get_param('~use_rostime', True)
self.frame_local = get_param('~frame_local', 'ENU')
self.frame_local_imu = get_param('~frame_local_imu', 'ENU')
if self.frame_local == 'ENU':
R = XSensDriver.ENU
elif self.frame_local == 'NED':
R = XSensDriver.NED
elif self.frame_local == 'NWU':
R = XSensDriver.NWU
if self.frame_local_imu == 'ENU':
R_IMU = XSensDriver.ENU
elif self.frame_local_imu == 'NED':
R_IMU = XSensDriver.NED
elif self.frame_local_imu == 'NWU':
R_IMU = XSensDriver.NWU
self.R = R.dot(R_IMU.transpose())
# self.diag_pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=10)
# self.diag_msg = DiagnosticArray()
# self.stest_stat = DiagnosticStatus(name='mtnode: Self Test', level=1, message='No status information')
# self.xkf_stat = DiagnosticStatus(name='mtnode: XKF Valid', level=1, message='No status information')
# self.gps_stat = DiagnosticStatus(name='mtnode: GPS Fix', level=1, message='No status information')
# self.diag_msg.status = [self.stest_stat, self.xkf_stat, self.gps_stat]
# custom message types
self.ss_pub = rospy.Publisher('custom/sample',
sensorSample,
queue_size=10) # sensorSample
self.gnssPvt_pub = rospy.Publisher('custom/gnssPvt',
gnssSample,
queue_size=10) # GNSS PVT
self.height_pub = rospy.Publisher(
'custom/height', baroSample, queue_size=10) # baro (height meters)
# normal message types
self.imuraw_pub = rospy.Publisher('sensor/imu_raw', Imu,
queue_size=10) #IMU message
self.imuins_pub = rospy.Publisher('sensor/imu_ins', Imu,
queue_size=10) #IMU message
self.mag_pub = rospy.Publisher('sensor/magnetic',
Vector3Stamped,
queue_size=10) # magnetic
self.baro_pub = rospy.Publisher('sensor/pressure',
FluidPressure,
queue_size=10) # baro (fluid pressure)
self.gps1_pub = rospy.Publisher('sensor/fix_navsat',
NavSatFix,
queue_size=10) # GNSS PVT
#self.gps2_pub = rospy.Publisher('mti/sensor/fix_gpscommon', NavSatFix, queue_size=10) # GNSS PVT
#self.gnssSatinfo_pub = rospy.Publisher('mti/sensor/fix_status', GPSStatus, queue_size=10) # GNSS SATINFO
self.temp_pub = rospy.Publisher('sensor/temperature',
Temperature,
queue_size=10) # decide type
# xsens filters
self.ori_pub = rospy.Publisher('filter/orientation',
orientationEstimate,
queue_size=10) # XKF/XEE orientation
self.vel_pub = rospy.Publisher('filter/velocity',
velocityEstimate,
queue_size=10) # XKF/XEE velocity
self.pos_pub = rospy.Publisher('filter/position',
positionEstimate,
queue_size=10) # XKF/XEE position
def __init__(self):
device = get_param('~device', 'auto')
baudrate = get_param('~baudrate', 0)
timeout = get_param('~timeout', 0.02)
if device == 'auto':
devs = mtdevice.find_devices()
if devs:
device, baudrate = devs[0]
rospy.loginfo("Detected MT device on port %s @ %d bps"
% (device, baudrate))
else:
rospy.logerr("Fatal: could not find proper MT device.")
rospy.signal_shutdown("Could not find proper MT device.")
return
if not baudrate:
baudrate = mtdevice.find_baudrate(device)
if not baudrate:
rospy.logerr("Fatal: could not find proper baudrate.")
rospy.signal_shutdown("Could not find proper baudrate.")
return
rospy.loginfo("MT node interface: %s at %d bd." % (device, baudrate))
self.mt = mtdevice.MTDevice(device, baudrate, timeout)
# optional no rotation procedure for internal calibration of biases
# (only mark iv devices)
no_rotation_duration = get_param('~no_rotation_duration', 0)
if no_rotation_duration:
rospy.loginfo("Starting the no-rotation procedure to estimate the "
"gyroscope biases for %d s. Please don't move the IMU"
" during this time." % no_rotation_duration)
self.mt.SetNoRotation(no_rotation_duration)
self.frame_id = get_param('~frame_id', '/base_imu')
self.frame_local = get_param('~frame_local', 'ENU')
self.diag_msg = DiagnosticArray()
self.stest_stat = DiagnosticStatus(name='mtnode: Self Test', level=1,
message='No status information')
self.xkf_stat = DiagnosticStatus(name='mtnode: XKF Valid', level=1,
message='No status information')
self.gps_stat = DiagnosticStatus(name='mtnode: GPS Fix', level=1,
message='No status information')
self.diag_msg.status = [self.stest_stat, self.xkf_stat, self.gps_stat]
# publishers created at first use to reduce topic clutter
self.diag_pub = None
self.imu_pub = None
self.pos_pub = None
self.gps_pub = None
self.vel_pub = None
self.mag_pub = None
self.temp_pub = None
self.press_pub = None
self.analog_in1_pub = None # decide type+header
self.analog_in2_pub = None # decide type+header
self.ecef_pub = None
self.time_ref_pub = None
# TODO pressure, ITOW from raw GPS?
self.old_bGPS = 256 # publish GPS only if new
# publish a string version of all data; to be parsed by clients
self.str_pub = rospy.Publisher('imu/imu_data_str', String, queue_size=10)
# predefinition of used variables
self.imu_msg = Imu()
self.imu_msg_old = Imu()
self.pos_msg = NavSatFix()
self.pos_msg_old = NavSatFix()
self.gps_msg = NavSatFix()
self.gps_msg_old = NavSatFix()
self.vel_msg = TwistStamped()
self.vel_msg_old = TwistStamped()
self.mag_msg = MagneticField()
self.mag_msg_old = MagneticField()
self.temp_msg = Temperature()
self.temp_msg_old = Temperature()
self.press_msg = FluidPressure()
self.press_msg_old = FluidPressure()
self.anin1_msg = UInt16()
self.anin1_msg_old = UInt16()
self.anin2_msg = UInt16()
self.anin2_msg_old = UInt16()
self.ecef_msg = PointStamped()
self.ecef_msg_old = PointStamped()
# triggers for new msg to publish
self.pub_imu = False
self.pub_pos = False
self.pub_gps = False
self.pub_vel = False
self.pub_mag = False
self.pub_temp = False
self.pub_press = False
self.pub_anin1 = False
self.pub_anin2 = False
self.pub_ecef = False
self.pub_diag = False
def __init__(self):
device = get_param('~device', 'auto')
baudrate = get_param('~baudrate', 0)
if device == 'auto':
devs = mtdevice.find_devices()
if devs:
device, baudrate = devs[0]
rospy.loginfo("Detected MT device on port %s @ %d bps" %
(device, baudrate))
else:
rospy.logerr("Fatal: could not find proper MT device.")
rospy.signal_shutdown("Could not find proper MT device.")
return
if not baudrate:
baudrate = mtdevice.find_baudrate(device)
if not baudrate:
rospy.logerr("Fatal: could not find proper baudrate.")
rospy.signal_shutdown("Could not find proper baudrate.")
return
rospy.loginfo("MT node interface: %s at %d bd." % (device, baudrate))
self.mt = mtdevice.MTDevice(device, baudrate)
self.frame_id = get_param('~frame_id', '/base_imu')
frame_local = get_param('~frame_local', 'ENU')
frame_local_imu = get_param('~frame_local_imu', 'ENU')
if frame_local == 'ENU':
R = XSensDriver.ENU
elif frame_local == 'NED':
R = XSensDriver.NED
elif frame_local == 'NWU':
R = XSensDriver.NWU
if frame_local_imu == 'ENU':
R_IMU = XSensDriver.ENU
elif frame_local_imu == 'NED':
R_IMU = XSensDriver.NED
elif frame_local_imu == 'NWU':
R_IMU = XSensDriver.NWU
self.R = R.dot(R_IMU.transpose())
self.diag_pub = rospy.Publisher('/diagnostics',
DiagnosticArray,
queue_size=10)
self.diag_msg = DiagnosticArray()
self.stest_stat = DiagnosticStatus(name='mtnode: Self Test',
level=1,
message='No status information')
self.xkf_stat = DiagnosticStatus(name='mtnode: XKF Valid',
level=1,
message='No status information')
self.gps_stat = DiagnosticStatus(name='mtnode: GPS Fix',
level=1,
message='No status information')
self.diag_msg.status = [self.stest_stat, self.xkf_stat, self.gps_stat]
self.imu_pub = rospy.Publisher('imu/data', Imu, queue_size=10)
self.gps_pub = rospy.Publisher('fix', NavSatFix, queue_size=10)
self.xgps_pub = rospy.Publisher('fix_extended', GPSFix, queue_size=10)
self.vel_pub = rospy.Publisher('velocity', TwistStamped, queue_size=10)
self.mag_pub = rospy.Publisher('magnetic',
Vector3Stamped,
queue_size=10)
self.temp_pub = rospy.Publisher('temperature', Float32,
queue_size=10) # decide type
# TODO pressure, ITOW from raw GPS?
self.old_bGPS = 256 # publish GPS only if new
def __init__(self):
device = get_param('~device', 'auto')
baudrate = get_param('~baudrate', 0)
if device == 'auto':
devs = mtdevice.find_devices()
if devs:
device, baudrate = devs[0]
rospy.loginfo("Detected MT device on port %s @ %d bps" %
(device, baudrate))
else:
rospy.logerr("Fatal: could not find proper MT device.")
rospy.signal_shutdown("Could not find proper MT device.")
return
if not baudrate:
baudrate = mtdevice.find_baudrate(device)
if not baudrate:
rospy.logerr("Fatal: could not find proper baudrate.")
rospy.signal_shutdown("Could not find proper baudrate.")
return
rospy.loginfo("MT node interface: %s at %d bd." % (device, baudrate))
self.mt = mtdevice.MTDevice(device, baudrate)
configure_on_startup = get_param('~configure_on_startup', False)
odr = get_param('~odr', 40)
output_mode = get_param('~output_mode', 2)
xkf_scenario = get_param('~xkf_scenario', 43)
self.use_rostime = get_param('~use_rostime', False)
if configure_on_startup:
rospy.loginfo('Setting ODR (%d) and output mode (%d)' %
(odr, output_mode))
if odr not in [1, 2, 5, 10, 20, 40, 50, 80, 100, 200, 400]:
raise Exception('Invalid ODR configuraton requested')
if output_mode not in [1, 2, 3]:
raise Exception('Invalid output mode requested')
self.mt.configureMti(odr, output_mode)
self.mt.ChangeBaudrate(baudrate)
self.mt.SetCurrentScenario(xkf_scenario)
self.mt.GoToMeasurement()
else:
rospy.loginfo('Using saved odr and output configuration')
self.frame_id = get_param('~frame_id', 'mti/data')
frame_local = get_param('~frame_local', 'ENU')
frame_local_imu = get_param('~frame_local_imu', 'ENU')
if frame_local == 'ENU':
R = XSensDriver.ENU
elif frame_local == 'NED':
R = XSensDriver.NED
elif frame_local == 'NWU':
R = XSensDriver.NWU
if frame_local_imu == 'ENU':
R_IMU = XSensDriver.ENU
elif frame_local_imu == 'NED':
R_IMU = XSensDriver.NED
elif frame_local_imu == 'NWU':
R_IMU = XSensDriver.NWU
self.R = R.dot(R_IMU.transpose())
self.diag_pub = rospy.Publisher('/diagnostics',
DiagnosticArray,
queue_size=10)
self.diag_msg = DiagnosticArray()
self.stest_stat = DiagnosticStatus(name='mtnode: Self Test',
level=1,
message='No status information')
self.xkf_stat = DiagnosticStatus(name='mtnode: XKF Valid',
level=1,
message='No status information')
self.gps_stat = DiagnosticStatus(name='mtnode: GPS Fix',
level=1,
message='No status information')
self.diag_msg.status = [self.stest_stat, self.xkf_stat, self.gps_stat]
self.imu_pub = rospy.Publisher('mti/sensor/imu', Imu,
queue_size=10) #IMU message
self.imu_free_pub = rospy.Publisher(
'mti/sensor/imu_free', Imu,
queue_size=10) #IMU message free Acceleration --- added by Harold
self.ss_pub = rospy.Publisher('mti/sensor/sample',
sensorSample,
queue_size=10) # sensorSample
self.mag_pub = rospy.Publisher('mti/sensor/magnetic',
Vector3Stamped,
queue_size=10) # magnetic
self.baro_pub = rospy.Publisher('mti/sensor/pressure',
baroSample,
queue_size=10) # baro
self.gnssPvt_pub = rospy.Publisher('mti/sensor/gnssPvt',
gnssSample,
queue_size=10) # GNSS PVT
#self.gnssSatinfo_pub = rospy.Publisher('mti/sensor/gnssStatus', GPSStatus, queue_size=10) # GNSS SATINFO
self.ori_pub = rospy.Publisher('mti/filter/orientation',
orientationEstimate,
queue_size=10) # XKF/XEE orientation
self.vel_pub = rospy.Publisher('mti/filter/velocity',
velocityEstimate,
queue_size=10) # XKF/XEE velocity
self.pos_pub = rospy.Publisher('mti/filter/position',
positionEstimate,
queue_size=10) # XKF/XEE position
self.temp_pub = rospy.Publisher('mti/temperature',
Float32,
queue_size=10) # decide type
def start(self):
self.log_info("XSenseDriver starting by time %f" % self.app.clock.time)
self.freq_div = self.get_param("freq_div", 1)
base_freq = self.get_param("base_freq", 500)
device = self.get_param('device', 'auto')
baudrate = self.get_param('baudrate', 0)
timeout = self.get_param('timeout', 0.002)
initial_wait = self.get_param('initial_wait', 0.1)
if device == 'auto':
devs = mtdevice.find_devices(timeout=timeout,
initial_wait=initial_wait)
if devs:
device, baudrate = devs[0]
self.log_info("Detected MT device on port %s @ %d bps" %
(device, baudrate))
else:
self.log_critical("Could not find proper MT device.")
return
if not baudrate:
baudrate = mtdevice.find_baudrate(device,
timeout=timeout,
initial_wait=initial_wait)
if not baudrate:
self.log_critical("Could not find proper baudrate.")
return
self.log_info("MT node interface: %s at %d bd." % (device, baudrate))
self.mt = mtdevice.MTDevice(device,
baudrate,
timeout,
initial_wait=initial_wait)
# optional no rotation procedure for internal calibration of biases
# (only mark iv devices)
no_rotation_duration = self.get_param('no_rotation_duration', 0)
if no_rotation_duration:
self.log_info(
"Starting the no-rotation procedure to estimate the "
"gyroscope biases for %d s. Please don't move the IMU"
" during this time." % no_rotation_duration)
self.mt.SetNoRotation(no_rotation_duration)
self.frame_id = self.get_param('frame_id', '/base_imu')
self.base_frame_id = self.get_param('base_frame_id', '/base_link')
self.frame_local = self.get_param('frame_local', 'ENU')
# Provides XSense pose to Isaac app pose tree.
q = self.get_param("rotate_from_base_frame", [0, 0, 0])
t = self.get_param("trans_from_base_frame", [0, 0, 0])
self.app.atlas.set_pose(
self.frame_id, # lhs (str): left-hand-side frame name
self.base_frame_id, # rhs (str): right-hand-side frame name
self.app.clock.time, # app_time (float): Isaac app time in seconds
# pose (List): tuple of (numpy.quaternion, numpy.array(3)) for rotation and translation accordingly.
(quaternion.from_euler_angles(q), numpy.array(t)))
# Covariances... (not used in Isaac SDK)
self.angular_velocity_covariance = self.matrix_from_diagonal(
self.get_param('angular_velocity_covariance_diagonal',
[radians(0.025)] * 3))
self.linear_acceleration_covariance = self.matrix_from_diagonal(
self.get_param('linear_acceleration_covariance_diagonal',
[0.0004] * 3))
self.orientation_covariance = self.matrix_from_diagonal(
self.get_param("orientation_covariance_diagonal",
[radians(1.), radians(1.),
radians(9.)]))
"""
self.diag_msg = DiagnosticArray()
self.stest_stat = DiagnosticStatus(name='mtnode: Self Test', level=1,
message='No status information')
self.xkf_stat = DiagnosticStatus(name='mtnode: XKF Valid', level=1,
message='No status information')
self.gps_stat = DiagnosticStatus(name='mtnode: GPS Fix', level=1,
message='No status information')
self.diag_msg.status = [self.stest_stat, self.xkf_stat, self.gps_stat]
"""
# publishers created at first use to reduce topic clutter
self.diag_pub = self.isaac_proto_tx("JsonProto", "diagnostics")
self.imu_pub = self.isaac_proto_tx("ImuProto", "imu_raw")
#self.raw_gps_pub = None
#self.vel_pub = None
#self.mag_pub = None
#self.pos_gps_pub = None
#self.temp_pub = self.isaac_proto_tx("Float64", "temperature")
#self.press_pub = None
#self.analog_in1_pub = None # decide type+header
#self.analog_in2_pub = None # decide type+header
#self.ecef_pub = None
#self.time_ref_pub = None
# TODO pressure, ITOW from raw GPS?
#self.old_bGPS = 256 # publish GPS only if new
# publish a string version of all data; to be parsed by clients
#self.str_pub = rospy.Publisher('imu_data_str', String, queue_size=10)
self.last_delta_q_time = None
self.delta_q_rate = None
# This part will be run once in the beginning of the program
# We can tick periodically, on every message, or blocking. See documentation for details.
#self.tick_blocking()
self.tick_periodically(1 / base_freq)
def __init__(self):
device = get_param('~device', 'auto')
baudrate = get_param('~baudrate', 0)
timeout = get_param('~timeout', 0.002)
if device == 'auto':
devs = mtdevice.find_devices()
if devs:
device, baudrate = devs[0]
rospy.loginfo("Detected MT device on port %s @ %d bps" %
(device, baudrate))
else:
rospy.logerr("Fatal: could not find proper MT device.")
rospy.signal_shutdown("Could not find proper MT device.")
return
if not baudrate:
baudrate = mtdevice.find_baudrate(device)
if not baudrate:
rospy.logerr("Fatal: could not find proper baudrate.")
rospy.signal_shutdown("Could not find proper baudrate.")
return
rospy.loginfo("MT node interface: %s at %d bd." % (device, baudrate))
self.mt = mtdevice.MTDevice(device, baudrate, timeout)
# optional no rotation procedure for internal calibration of biases
# (only mark iv devices)
no_rotation_duration = get_param('~no_rotation_duration', 0)
if no_rotation_duration:
rospy.loginfo(
"Starting the no-rotation procedure to estimate the "
"gyroscope biases for %d s. Please don't move the IMU"
" during this time." % no_rotation_duration)
self.mt.SetNoRotation(no_rotation_duration)
self.frame_id = get_param('~frame_id', '/base_imu')
self.frame_local = get_param('~frame_local', 'ENU')
self.stest_stat = DiagnosticStatus(name='mtnode: Self Test',
level=1,
message='No status information')
self.xkf_stat = DiagnosticStatus(name='mtnode: XKF Valid',
level=1,
message='No status information')
self.gps_stat = DiagnosticStatus(name='mtnode: GPS Fix',
level=1,
message='No status information')
self.device_info = {
'product_code': self.mt.GetProductCode(),
'device_id': self.mt.GetDeviceID(),
'firmware_rev': self.mt.GetFirmwareRev()
}
self.device_info.update(self.mt.GetConfiguration())
self.updater = Updater()
self.updater.setHardwareID(str(self.mt.GetDeviceID()))
#self.updater.add("Self Test", self.diagnostic_self_test)
#self.updater.add("XKF", self.diagnostic_xkf)
#self.updater.add("GPS Fix", self.diagnostic_gps)
self.updater.add("Device Info", self.diagnostic_device)
self.imu_freq = TopicDiagnostic(
"imu/data", self.updater,
FrequencyStatusParam({
'min': 0,
'max': 100
}, 0.1, 10), TimeStampStatusParam())
self.mag_freq = TopicDiagnostic(
"imu/mag", self.updater,
FrequencyStatusParam({
'min': 0,
'max': 100
}, 0.1, 10), TimeStampStatusParam())
# publishers created at first use to reduce topic clutter
self.imu_pub = None
self.gps_pub = None
self.vel_pub = None
self.mag_pub = None
self.temp_pub = None
self.press_pub = None
self.analog_in1_pub = None # decide type+header
self.analog_in2_pub = None # decide type+header
self.ecef_pub = None
self.time_ref_pub = None
# TODO pressure, ITOW from raw GPS?
self.old_bGPS = 256 # publish GPS only if new
# publish a string version of all data; to be parsed by clients
self.str_pub = rospy.Publisher('imu_data_str', String, queue_size=10)
def __init__(self):
device = get_param('~device', 'auto')
baudrate = get_param('~baudrate', 0)
timeout = get_param('~timeout', 0.002)
initial_wait = get_param('~initial_wait', 0.1)
if device == 'auto':
devs = mtdevice.find_devices(timeout=timeout,
initial_wait=initial_wait)
if devs:
device, baudrate = devs[0]
rospy.loginfo("Detected MT device on port %s @ %d bps"
% (device, baudrate))
else:
rospy.logerr("Fatal: could not find proper MT device.")
rospy.signal_shutdown("Could not find proper MT device.")
return
if not baudrate:
baudrate = mtdevice.find_baudrate(device, timeout=timeout,
initial_wait=initial_wait)
if not baudrate:
rospy.logerr("Fatal: could not find proper baudrate.")
rospy.signal_shutdown("Could not find proper baudrate.")
return
rospy.loginfo("MT node interface: %s at %d bd." % (device, baudrate))
self.mt = mtdevice.MTDevice(device, baudrate, timeout,
initial_wait=initial_wait)
# optional no rotation procedure for internal calibration of biases
# (only mark iv devices)
no_rotation_duration = get_param('~no_rotation_duration', 0)
if no_rotation_duration:
rospy.loginfo("Starting the no-rotation procedure to estimate the "
"gyroscope biases for %d s. Please don't move the IMU"
" during this time." % no_rotation_duration)
self.mt.SetNoRotation(no_rotation_duration)
self.frame_id = get_param('~frame_id', '/base_imu')
self.frame_local = get_param('~frame_local', 'ENU')
self.angular_velocity_covariance = matrix_from_diagonal(
get_param_list('~angular_velocity_covariance_diagonal', [radians(0.025)] * 3)
)
self.linear_acceleration_covariance = matrix_from_diagonal(
get_param_list('~linear_acceleration_covariance_diagonal', [0.0004] * 3)
)
self.orientation_covariance = matrix_from_diagonal(
get_param_list("~orientation_covariance_diagonal", [radians(1.), radians(1.), radians(9.)])
)
self.diag_msg = DiagnosticArray()
self.stest_stat = DiagnosticStatus(name='mtnode: Self Test', level=1,
message='No status information')
self.xkf_stat = DiagnosticStatus(name='mtnode: XKF Valid', level=1,
message='No status information')
self.gps_stat = DiagnosticStatus(name='mtnode: GPS Fix', level=1,
message='No status information')
self.diag_msg.status = [self.stest_stat, self.xkf_stat, self.gps_stat]
# publishers created at first use to reduce topic clutter
self.diag_pub = None
self.imu_pub = None
self.raw_gps_pub = None
self.vel_pub = None
self.mag_pub = None
self.pos_gps_pub = None
self.temp_pub = None
self.press_pub = None
self.analog_in1_pub = None # decide type+header
self.analog_in2_pub = None # decide type+header
self.ecef_pub = None
self.time_ref_pub = None
# TODO pressure, ITOW from raw GPS?
self.old_bGPS = 256 # publish GPS only if new
# publish a string version of all data; to be parsed by clients
self.str_pub = rospy.Publisher('imu_data_str', String, queue_size=10)
self.last_delta_q_time = None
self.delta_q_rate = None
def __init__(self):
device = get_param('~device', 'auto')
baudrate = get_param('~baudrate', 0)
timeout = get_param('~timeout', 0.002)
if device=='auto':
devs = mtdevice.find_devices()
if devs:
device, baudrate = devs[0]
rospy.loginfo("Detected MT device on port %s @ %d bps"%(device,
baudrate))
else:
rospy.logerr("Fatal: could not find proper MT device.")
rospy.signal_shutdown("Could not find proper MT device.")
return
if not baudrate:
baudrate = mtdevice.find_baudrate(device)
if not baudrate:
rospy.logerr("Fatal: could not find proper baudrate.")
rospy.signal_shutdown("Could not find proper baudrate.")
return
rospy.loginfo("MT node interface: %s at %d bd."%(device, baudrate))
self.mt = mtdevice.MTDevice(device, baudrate, timeout)
self.frame_id = get_param('~frame_id', '/base_imu')
frame_local = get_param('~frame_local' , 'ENU')
frame_local_imu = get_param('~frame_local_imu', 'ENU')
if frame_local == 'ENU':
R = XSensDriver.ENU
elif frame_local == 'NED':
R = XSensDriver.NED
elif frame_local == 'NWU':
R = XSensDriver.NWU
if frame_local_imu == 'ENU':
R_IMU = XSensDriver.ENU
elif frame_local_imu == 'NED':
R_IMU = XSensDriver.NED
elif frame_local_imu == 'NWU':
R_IMU = XSensDriver.NWU
self.R = R.dot(R_IMU.transpose())
self.diag_pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=10)
self.diag_msg = DiagnosticArray()
self.stest_stat = DiagnosticStatus(name='mtnode: Self Test', level=1,
message='No status information')
self.xkf_stat = DiagnosticStatus(name='mtnode: XKF Valid', level=1,
message='No status information')
self.gps_stat = DiagnosticStatus(name='mtnode: GPS Fix', level=1,
message='No status information')
self.diag_msg.status = [self.stest_stat, self.xkf_stat, self.gps_stat]
self.imu_pub = rospy.Publisher('imu/data', Imu, queue_size=10)
self.gps_pub = rospy.Publisher('fix', NavSatFix, queue_size=10)
self.xgps_pub = rospy.Publisher('fix_extended', GPSFix, queue_size=10)
self.vel_pub = rospy.Publisher('velocity', TwistStamped, queue_size=10)
self.mag_pub = rospy.Publisher('magnetic', Vector3Stamped, queue_size=10)
self.temp_pub = rospy.Publisher('temperature', Float32, queue_size=10) # decide type
# TODO pressure, ITOW from raw GPS?
self.old_bGPS = 256 # publish GPS only if new
