示例#1
0
    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)
示例#2
0
    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)
示例#3
0
    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
示例#6
0
    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."
示例#7
0
    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
示例#8
0
    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
示例#9
0
	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
示例#10
0
    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
示例#12
0
    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
示例#13
0
    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
示例#14
0
    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)
示例#15
0
    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)
示例#16
0
    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
示例#17
0
	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