def Publish(self):
        imu_data = self._hal_proxy.GetImu()

        imu_msg = Imu()
        h = Header()
        h.stamp = rospy.Time.now()
        h.frame_id = self._frame_id

        imu_msg.header = h
        imu_msg.orientation_covariance = (-1., )*9
        imu_msg.linear_acceleration_covariance = (-1., )*9
        imu_msg.angular_velocity_covariance = (-1., )*9

        imu_msg.orientation.w = imu_data['orientation']['w']
        imu_msg.orientation.x = imu_data['orientation']['x']
        imu_msg.orientation.y = imu_data['orientation']['y']
        imu_msg.orientation.z = imu_data['orientation']['z']

        imu_msg.linear_acceleration.x = imu_data['linear_accel']['x']
        imu_msg.linear_acceleration.y = imu_data['linear_accel']['y']
        imu_msg.linear_acceleration.z = imu_data['linear_accel']['z']

        imu_msg.angular_velocity.x = imu_data['angular_velocity']['x']
        imu_msg.angular_velocity.y = imu_data['angular_velocity']['y']
        imu_msg.angular_velocity.z = imu_data['angular_velocity']['z']

        # Read the x, y, z and heading
        self._publisher.publish(imu_msg)

def sensor_interface():
	rospy.init_node('imu_razor9dof', anonymous = False)
	interface.flushInput()
	
	# Perform a loop checking on the enabled status of the drivers
	while not rospy.is_shutdown():
		# Fetch a JSON message from the controller and process (or atleast attempt to)
		try:
			# Get the object and create some messages
			_object = json.loads(interface.readline())
			_imu = Imu();
			_heading = Float32()
			
			# Get the imu data (fuck quaternions!!)
			_imu.header = rospy.Header()
			roll = _object['angles'][0]
			pitch = _object['angles'][1]
			yaw = _object['angles'][2]
			#_imu.orientation = tf::createQuaternionFromRPY(roll, pitch, yaw)
			_imu.linear_acceleration.x = _object['accel'][0]
			_imu.linear_acceleration.y = _object['accel'][1]
			_imu.linear_acceleration.z = _object['accel'][2]

			# Get the heading data
			#_heading.data = _object['heading']
			_heading.data = _object['angles'][2]
			# Publish the data
			imuPublisher.publish(_imu);
			cmpPublisher.publish(_heading);
		except json.decoder.JSONDecodeError:
			rospy.logwarn("Invalid message received")
		except: pass

 def publish_imu(self):
     imu_msg = Imu()
     imu_msg.header.stamp = self.time
     imu_msg.header.frame_id = 'imu_odom'
     #quat = tf_math.quaternion_from_euler(self.kalman_state[0,0],self.kalman_state[1,0],self.kalman_state[2,0], axes='sxyz')
     a = self.kalman_state[3,0]
     b = self.kalman_state[4,0]
     c = self.kalman_state[5,0]
     d = self.kalman_state[6,0]
     q = math.sqrt(math.pow(a,2)+math.pow(b,2)+math.pow(c,2)+math.pow(d,2))
     #angles = tf_math.euler_from_quaternion(self.kalman_state[3:7,0])
     imu_msg.orientation.x = a/q#angles[0]
     imu_msg.orientation.y = b/q
     imu_msg.orientation.z = c/q
     imu_msg.orientation.w = d/q
     imu_msg.orientation_covariance = list(self.kalman_covariance[3:6,3:6].flatten())
     imu_msg.angular_velocity.x = self.kalman_state[0,0]
     imu_msg.angular_velocity.y = self.kalman_state[1,0]
     imu_msg.angular_velocity.z = self.kalman_state[2,0]
     imu_msg.angular_velocity_covariance = list(self.kalman_covariance[0:3,0:3].flatten())
     imu_msg.linear_acceleration.x = self.kalman_state[10,0]
     imu_msg.linear_acceleration.y = self.kalman_state[11,0]
     imu_msg.linear_acceleration.z = self.kalman_state[12,0]
     imu_msg.linear_acceleration_covariance = list(self.kalman_covariance[10:13,10:13].flatten())
     self.pub.publish(imu_msg)

    def publish(self, event):
        compass_accel = self.compass_accel.read()
        compass = compass_accel[0:3]
        accel = compass_accel[3:6]
        gyro = self.gyro.read()
        
        # Put together an IMU message
	imu_msg = Imu()
	imu_msg.header.stamp = rospy.Time.now()
        imu_msg.orientation_covariance[0] = -1
        imu_msg.angular_velocity = gyro[0] * DEG_TO_RAD
        imu_msg.angular_velocity = gyro[1] * DEG_TO_RAD
        imu_msg.angular_velocity = gyro[2] * DEG_TO_RAD
	imu_msg.linear_acceleration.x = accel[0] * G
	imu_msg.linear_acceleration.y = accel[1] * G
	imu_msg.linear_acceleration.z = accel[2] * G
    
	self.pub_imu.publish(imu_msg)

        # Put together a magnetometer message
	mag_msg = MagneticField()
	mag_msg.header.stamp = rospy.Time.now()
        mag_msg.magnetic_field.x = compass[0]
        mag_msg.magnetic_field.y = compass[1]
        mag_msg.magnetic_field.z = compass[2]
    
	self.pub_mag.publish(mag_msg)

def rospyrtimulib():
    imuData = Imu()
    print("Using settings file " + SETTINGS_FILE + ".ini")
    if not os.path.exists(SETTINGS_FILE + ".ini"):
        print("Settings file does not exist, will be created")

    s = RTIMU.Settings(SETTINGS_FILE)
    imu = RTIMU.RTIMU(s)

    print("IMU Name: " + imu.IMUName())

    if (not imu.IMUInit()):
        print("IMU Init Failed")
        sys.exit(1)
    else:
        print("IMU Init Succeeded")

    # this is a good time to set any fusion parameters

    imu.setSlerpPower(0.02)
    imu.setGyroEnable(True)
    imu.setAccelEnable(True)
    imu.setCompassEnable(True)

    pub = rospy.Publisher('imu', Imu, queue_size=10)
    rospy.init_node('rospyrtimulib', anonymous=True)
    rate = rospy.Rate(1000 / imu.IMUGetPollInterval())

    while not rospy.is_shutdown():
        if imu.IMURead():
            # collect the data
            data = imu.getIMUData()

            fusionQPose = data["fusionQPose"]
            imuData.orientation.x = fusionQPose[1]
            imuData.orientation.y = fusionQPose[2]
            imuData.orientation.z = fusionQPose[3]
            imuData.orientation.w = fusionQPose[0]
 
            gyro = data["gyro"]
            imuData.angular_velocity.x = gyro[0]
            imuData.angular_velocity.y = gyro[1]
            imuData.angular_velocity.z = gyro[2]

            accel = data["accel"]
            imuData.linear_acceleration.x = accel[0] * 9.81
            imuData.linear_acceleration.y = accel[1] * 9.81
            imuData.linear_acceleration.z = accel[2] * 9.81
            imuData.header.stamp = rospy.Time.now()

            # no covariance
            imuData.orientation_covariance[0] = -1
            imuData.angular_velocity_covariance[0] = -1
            imuData.linear_acceleration_covariance[0] = -1

            imuData.header.frame_id = 'map'

            # publish it
            pub.publish(imuData)
        rate.sleep()

def talker():
    pub = rospy.Publisher('imu', Imu, queue_size=10)
    rospy.init_node('imusensor', anonymous=True)
    r = rospy.Rate(500)
    while not rospy.is_shutdown():
        #str = "hello world %s"%print_gyro().__str__()
        arr = print_gyro()
        i = Imu()

        i.header = std_msgs.msg.Header()
        i.header.frame_id="my_frame"
        i.header.stamp = rospy.Time.now() # Note you need to call rospy.init_node() before this will work
        i.linear_acceleration.x=arr[0][0]
        i.linear_acceleration.y=arr[0][1]
        i.linear_acceleration.z=arr[0][2]
        i.angular_velocity.x=arr[1][0]
        i.angular_velocity.y=arr[1][1] 
        i.angular_velocity.z=arr[1][2] 
        
        i.orientation.x=0
        i.orientation.y=0
        i.orientation.z=0
        i.orientation.w=0

        #rospy.loginfo(str)
        pub.publish(i)
        r.sleep()

def post_imu(self, component_instance):
    """ Publish the data of the IMU sensor as a custom ROS Imu message
    """
    parent = component_instance.robot_parent
    parent_name = parent.blender_obj.name
    current_time = rospy.Time.now()

    imu = Imu()
    imu.header.seq = self._seq
    imu.header.stamp = current_time
    imu.header.frame_id = "/imu"
    
    imu.orientation = self.orientation.to_quaternion()

    imu.angular_velocity.x = component_instance.local_data['angular_velocity'][0]
    imu.angular_velocity.y = component_instance.local_data['angular_velocity'][1]
    imu.angular_velocity.z = component_instance.local_data['angular_velocity'][2]

    imu.linear_acceleration.x = component_instance.local_data['linear_acceleration'][0]
    imu.linear_acceleration.y = component_instance.local_data['linear_acceleration'][1]
    imu.linear_acceleration.z = component_instance.local_data['linear_acceleration'][2]



    for topic in self._topics:
        # publish the message on the correct w
        if str(topic.name) == str("/" + parent_name + "/" + component_instance.blender_obj.name):
            topic.publish(imu)

    self._seq += 1

	def unpackIMU(self, data):
		imu_msg = Imu()
		#Unpack Header
		imu_msg.header = self.unpackIMUHeader(data[0:19])
		#Unpack Orientation Message
		quat = self.bytestr_to_array(data[19:19 + (4*8)])
		imu_msg.orientation.x = quat[0]
		imu_msg.orientation.y = quat[1]
		imu_msg.orientation.z = quat[2]
		imu_msg.orientation.w = quat[3]
		#Unpack Orientation Covariance
		imu_msg.orientation_covariance = list(self.bytestr_to_array(data[55:(55 + (9*8))]))
		#Unpack Angular Velocity
		ang = self.bytestr_to_array(data[127: 127 + (3*8)])
		imu_msg.angular_velocity.x = ang[0]
		imu_msg.angular_velocity.y = ang[1]
		imu_msg.angular_velocity.z = ang[2]
		#Unpack Angular Velocity Covariance
		imu_msg.angular_velocity_covariance = list(self.bytestr_to_array(data[155:(155 + (9*8))]))
		#Unpack Linear Acceleration
		lin = self.bytestr_to_array(data[227: 227 + (3*8)])
		imu_msg.linear_acceleration.x = lin[0]
		imu_msg.linear_acceleration.y = lin[1]
		imu_msg.linear_acceleration.z = lin[2]
		#Unpack Linear Acceleration Covariance
		imu_msg.linear_acceleration_covariance = list(self.bytestr_to_array(data[255:(255 + (9*8))]))
		return imu_msg	

    def loop(self):
        while not rospy.is_shutdown():
            line = self.port.readline()
            chunks = line.split(":")
            if chunks[0] == "!QUAT":
                readings = chunks[1].split(",")
                if len(readings) == 10:
		    imu_msg = Imu()
		    imu_msg.header.stamp = rospy.Time.now()
		    imu_msg.header.frame_id = 'imu'
		    imu_msg.orientation.x = float(readings[0])
		    imu_msg.orientation.y = float(readings[1])
		    imu_msg.orientation.z = float(readings[2])
		    imu_msg.orientation.w = float(readings[3])
		    imu_msg.orientation_covariance = list(zeros((3,3)).flatten())
		    imu_msg.angular_velocity.x = float(readings[4])
		    imu_msg.angular_velocity.y = float(readings[5])
		    imu_msg.angular_velocity.z = float(readings[6])
		    imu_msg.angular_velocity_covariance = list(0.1*diagflat(ones((3,1))).flatten())
		    imu_msg.linear_acceleration.x = float(readings[7])
		    imu_msg.linear_acceleration.y = float(readings[8])
		    imu_msg.linear_acceleration.z = float(readings[9])
		    imu_msg.linear_acceleration_covariance = list(0.1*diagflat(ones((3,1))).flatten())
		    self.pub.publish(imu_msg)
		    quaternion = (imu_msg.orientation.x, imu_msg.orientation.y, imu_msg.orientation.z, imu_msg.orientation.w)
		    tf_br.sendTransform(translation = (0,0, 0), rotation = quaternion,time = rospy.Time.now(),child = 'imu',parent = 'world')
		else:
		    rospy.logerr("Did not get a valid IMU packet, got %s", line)
	    else:
		rospy.loginfo("Did not receive IMU data, instead got %s", line)

  def run(self):
    while not rospy.is_shutdown():
      self.lock.acquire()
      self.zumy.cmd(*self.cmd)
      imu_data = self.zumy.read_imu()
      enc_data = self.zumy.read_enc()
      self.lock.release()
      
      imu_msg = Imu()
      imu_msg.header = Header(self.imu_count,rospy.Time.now(),self.name)
      imu_msg.linear_acceleration.x = 9.81 * imu_data[0]
      imu_msg.linear_acceleration.y = 9.81 * imu_data[1]
      imu_msg.linear_acceleration.z = 9.81 * imu_data[2]
      imu_msg.angular_velocity.x = 3.14 / 180.0 * imu_data[3]
      imu_msg.angular_velocity.y = 3.14 / 180.0 * imu_data[4]
      imu_msg.angular_velocity.z = 3.14 / 180.0 * imu_data[5]
      self.imu_pub.publish(imu_msg)
      
      enc_msg = Int32()
      enc_msg.data = enc_data[0]
      self.r_enc_pub.publish(enc_msg)
      enc_msg.data = enc_data[1]
      self.l_enc_pub.publish(enc_msg)

      v_bat = self.zumy.read_voltage()
      self.batt_pub.publish(v_bat)
      self.heartBeat.publish("I am alive")
      self.rate.sleep()

    # If shutdown, turn off motors
    self.zumy.cmd(0,0)

    def publish(self, data):
        q = data.getOrientation()
        roll, pitch, yaw = euler_from_quaternion([q.w, q.x, q.y, q.z])

        # print "Before ", "Yaw: " + str(yaw * 180 / pi), "Roll: " + str(roll * 180 / pi), "pitch: " + str(pitch * 180 / pi), "\n\n"

        array = quaternion_from_euler(roll, pitch, yaw + (self._angle * pi / 180))

        res = Quaternion()
        res.w = array[0]
        res.x = array[1]
        res.y = array[2]
        res.z = array[3]

        roll, pitch, yaw = euler_from_quaternion([q.w, q.x, q.y, q.z])

        # print "after ", "Yaw: " + str(yaw * 180 / pi), "Roll: " + str(roll * 180 / pi), "pitch: " + str(pitch * 180 / pi), "\n\n"

        msg = Imu()
        msg.header.frame_id = self._frameId
        msg.header.stamp = rospy.get_rostime()
        msg.orientation = res
        msg.linear_acceleration = data.getAcceleration()
        msg.angular_velocity = data.getVelocity()

        magMsg = MagneticField()
        magMsg.header.frame_id = self._frameId
        magMsg.header.stamp = rospy.get_rostime()
        magMsg.magnetic_field = data.getMagnetometer()

        self._pub.publish(msg)
        self._pubMag.publish(magMsg)

    def publish(self, data):
        if not self._calib and data.getImuMsgId() == PUB_ID:
            q = data.getOrientation()
            roll, pitch, yaw = euler_from_quaternion([q.w, q.x, q.y, q.z])
            array = quaternion_from_euler(roll, pitch, yaw + (self._angle * pi / 180))

            res = Quaternion()
            res.w = array[0]
            res.x = array[1]
            res.y = array[2]
            res.z = array[3]

            msg = Imu()
            msg.header.frame_id = self._frameId
            msg.header.stamp = rospy.get_rostime()
            msg.orientation = res
            msg.linear_acceleration = data.getAcceleration()
            msg.angular_velocity = data.getVelocity()

            magMsg = MagneticField()
            magMsg.header.frame_id = self._frameId
            magMsg.header.stamp = rospy.get_rostime()
            magMsg.magnetic_field = data.getMagnetometer()

            self._pub.publish(msg)
            self._pubMag.publish(magMsg)

        elif data.getImuMsgId() == CALIB_ID:
            x, y, z = data.getValues()
            msg = imuCalib()

            if x > self._xMax:
                self._xMax = x
            if x < self._xMin:
                self._xMin = x

            if y > self._yMax:
                self._yMax = y
            if y < self._yMin:
                self._yMin = y

            if z > self._zMax:
                self._zMax = z
            if z < self._zMin:
                self._zMin = z


            msg.x.data = x
            msg.x.max = self._xMax
            msg.x.min = self._xMin

            msg.y.data = y
            msg.y.max = self._yMax
            msg.y.min = self._yMin

            msg.z.data = z
            msg.z.max = self._zMax
            msg.z.min = self._zMin

            self._pubCalib.publish(msg)

def fake_imu():
    pub = rospy.Publisher('IMU_ned', Imu, queue_size=10)
    rospy.init_node('test_convertimu', anonymous=True)
    rate = rospy.Rate(0.5) # 10hz
    
    while not rospy.is_shutdown():
        imu_message=Imu()

        imu_message.header.stamp=rospy.Time.now()
        imu_message.header.frame_id="IMU"

        imu_message.orientation.x=1
        imu_message.orientation.y=2
        imu_message.orientation.z=3
        imu_message.orientation.w=4
        imu_message.orientation_covariance=[-1,0,0,0,-1,0,0,0,-1]

        imu_message.linear_acceleration.x=6
        imu_message.linear_acceleration.y=7
        imu_message.linear_acceleration.z=8
        imu_message.linear_acceleration_covariance=[1e6, 0,0,0,1e6,0,0,0,1e6]

        imu_message.angular_velocity.x=10
        imu_message.angular_velocity.y=11
        imu_message.angular_velocity.z=12
        imu_message.angular_velocity_covariance=[1e6, 0,0,0,1e6,0,0,0,1e6]

        pub.publish(imu_message)
        rate.sleep()

def publish_imu_data():
    global imu, imu_publisher

    imu_data = Imu()

    imu_data.header.frame_id = "imu"
    imu_data.header.stamp = rospy.Time.from_sec(imu.last_update_time)

    # imu 3dmgx1 reports using the North-East-Down (NED) convention
    # so we need to convert to East-North-Up (ENU) coordinates according to ROS REP103
    # see http://answers.ros.org/question/626/quaternion-from-imu-interpreted-incorrectly-by-ros
    q = imu.orientation
    imu_data.orientation.w = q[0]
    imu_data.orientation.x = q[2]
    imu_data.orientation.y = q[1]
    imu_data.orientation.z = -q[3]
    imu_data.orientation_covariance = Config.get_orientation_covariance()

    av = imu.angular_velocity
    imu_data.angular_velocity.x = av[1]
    imu_data.angular_velocity.y = av[0]
    imu_data.angular_velocity.z = -av[2]
    imu_data.angular_velocity_covariance = Config.get_angular_velocity_covariance()

    la = imu.linear_acceleration
    imu_data.linear_acceleration.x = la[1]
    imu_data.linear_acceleration.y = la[0]
    imu_data.linear_acceleration.z = - la[2]
    imu_data.linear_acceleration_covariance = Config.get_linear_acceleration_covariance()

    imu_publisher.publish(imu_data)

    def run(self):
        while not rospy.is_shutdown():
            time_now = time.time()

            if time_now - self.timestamp > 0.5:
                self.cmd = (0, 0)

            self.lock.acquire()
            self.zumy.cmd(*self.cmd)
            imu_data = self.zumy.read_imu()
            self.lock.release()

            imu_msg = Imu()
            imu_msg.header = Header(self.imu_count, rospy.Time.now(), self.name)
            imu_msg.linear_acceleration.x = 9.81 * imu_data[0]
            imu_msg.linear_acceleration.y = 9.81 * imu_data[1]
            imu_msg.linear_acceleration.z = 9.81 * imu_data[2]
            imu_msg.angular_velocity.x = 3.14 / 180.0 * imu_data[3]
            imu_msg.angular_velocity.y = 3.14 / 180.0 * imu_data[4]
            imu_msg.angular_velocity.z = 3.14 / 180.0 * imu_data[5]
            self.imu_pub.publish(imu_msg)

            self.heartBeat.publish("I am alive from Glew")
            if self.msg != None:
                self.publisher.publish(self.msg.linear.y)
            self.rate.sleep()

        # If shutdown, turn off motors
        self.zumy.cmd(0, 0)

 def run(self):
     """Loop that obtains the latest wiimote state, publishes the IMU data, and sleeps.
     
     The IMU message, if fully filled in, contains information on orientation,
     acceleration (in m/s^2), and angular rate (in radians/sec). For each of
     these quantities, the IMU message format also wants the corresponding
     covariance matrix.
     
     Wiimote only gives us acceleration and angular rate. So we ensure that the orientation
     data entry is marked invalid. We do this by setting the first
     entry of its associated covariance matrix to -1. The covariance
     matrices are the 3x3 matrix with the axes' variance in the 
     diagonal. We obtain the variance from the Wiimote instance.  
     """
     
     rospy.loginfo("Wiimote IMU publisher starting (topic /imu/data).")
     self.threadName = "IMU topic Publisher"
     try:
         while not rospy.is_shutdown():
             (canonicalAccel, canonicalNunchukAccel, canonicalAngleRate) = self.obtainWiimoteData()
             
             msg = Imu(header=None,
                       orientation=None,                                         # will default to [0.,0.,0.,0],
                       orientation_covariance=[-1.,0.,0.,0.,0.,0.,0.,0.,0.],     # -1 indicates that orientation is unknown
                       angular_velocity=None,
                       angular_velocity_covariance=self.angular_velocity_covariance,
                       linear_acceleration=None,
                       linear_acceleration_covariance=self.linear_acceleration_covariance)
                       
                 
             # If a gyro is plugged into the Wiimote, then note the 
             # angular velocity in the message, else indicate with
             # the special gyroAbsence_covariance matrix that angular
             # velocity is unavailable:      
             if self.wiistate.motionPlusPresent:
                 msg.angular_velocity.x = canonicalAngleRate[PHI]
                 msg.angular_velocity.y = canonicalAngleRate[THETA]
                 msg.angular_velocity.z = canonicalAngleRate[PSI]
             else:
                 msg.angular_velocity_covariance = self.gyroAbsence_covariance
             
             msg.linear_acceleration.x = canonicalAccel[X]
             msg.linear_acceleration.y = canonicalAccel[Y]
             msg.linear_acceleration.z = canonicalAccel[Z]
             
             measureTime = self.wiistate.time
             timeSecs = int(measureTime)
             timeNSecs = int(abs(timeSecs - measureTime) * 10**9)
             msg.header.stamp.secs = timeSecs
             msg.header.stamp.nsecs = timeNSecs
             
             self.pub.publish(msg)
             
             rospy.logdebug("IMU state:")
             rospy.logdebug("    IMU accel: " + str(canonicalAccel) + "\n    IMU angular rate: " + str(canonicalAngleRate))
             rospy.sleep(self.sleepDuration)
     except rospy.ROSInterruptException:
         rospy.loginfo("Shutdown request. Shutting down Imu sender.")
         exit(0)

    def parse_imu(self, data):
        '''
        Given data from jaguar imu, parse and return a standard IMU message.
        Return None when given bad data, or no complete message was found in data.
        '''
         # Use regular expressions to extract complete message
        # message format: $val,val,val,val,val,val,val,val,val,val#\n
        hit = re.search(r"\$-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*#", data)
        if not hit:
            # if there are no hits, return None
            return None
        else:
            imu_data = hit.group(0)
            try:
                # Try to format the data
                imu_data = imu_data[1:-1].split(",")
                #Format (from drrobot docs): seq, accelx, accely, accelz, gyroY, gyroZ, gyroX, magnetonX, magnetonY, magnetonZ
                seq = int(imu_data[0])

                accelx = float(imu_data[1])
                accely = float(imu_data[2])
                accelz = float(imu_data[3])

                gyroy = float(imu_data[4])
                gyroz = float(imu_data[5])
                gyrox = float(imu_data[6])

                magnetonx = float(imu_data[7])
                magnetony = float(imu_data[8])
                magnetonz = float(imu_data[9])

                rot_mat = [ [float(imu_data[10]), float(imu_data[11]), float(imu_data[12])],
                            [float(imu_data[13]), float(imu_data[14]), float(imu_data[15])],
                            [float(imu_data[16]), float(imu_data[17]), float(imu_data[18])] ]

            except:
                # bad data in match, pass
                return None
            else:
                # data formatted fine, build message and publish

                # if we didn't get a magnetometer update, set to current reading
                if magnetonz == 0:
                    magnetonx = self.current_mag[0]
                    magnetony = self.current_mag[1]
                    magnetonz = self.current_mag[2]
                # otherwise, update current magnetometer
                else:
                    self.current_mag = [magnetonx, magnetony, magnetonz]

                # Calculate quaternion from given rotation matrix
                quat = rot_mat_to_quat(rot_mat);
                # Build message 
                msg = Imu()
                msg.header = Header(stamp = rospy.Time.now(), frame_id = "imu")
                msg.linear_acceleration = Vector3(accelx, accely, accelz)
                msg.angular_velocity = Vector3(gyrox, gyroy, gyroz)
                if quat != None: msg.orientation = Quaternion(quat[0], quat[1], quat[2], quat[3])
                return msg

    def spin(self):
        self.prev_time = rospy.Time.now()

        while not rospy.is_shutdown():
            if self.calibrating:
                self.calibrate()
                self.calibrating = False
                self.prev_time = rospy.Time.now()

            acceldata = self.accelerometer.read6Reg(accel_x_low)
            compassdata = self.compass.read6Reg(compass_x_high)
            gyrodata = self.gyro.read6Reg(gyro_x_low)

            # prepare Imu frame
            imu = Imu()
            imu.header.frame_id = self.frame_id
            self.linear_acceleration = Vector3();

            # get line from device
            #str = self.ser.readline()

            # timestamp
            imu.header.stamp = rospy.Time.now()

            #nums = str.split()

            # check, if it was correct line
            #if (len(nums) != 5):
            #    continue

            self.linear_acceleration.x = self.twosComplement(acceldata[0], acceldata[1]) #/16384.0
            self.linear_acceleration.y = self.twosComplement(acceldata[2], acceldata[3]) #/16384.0
            self.linear_acceleration.z = self.twosComplement(acceldata[4], acceldata[5]) #/16384.0

            imu.orientation.x = self.twosComplement(compassdata[1], compassdata[0]) #/1055.0,
            imu.orientation.y = self.twosComplement(compassdata[3], compassdata[2]) #/1055.0,
            imu.orientation.z = self.twosComplement(compassdata[5], compassdata[4]) #/950.0

            imu.angular_velocity.x = self.twosComplement(gyrodata[0], gyrodata[1])
            imu.angular_velocity.y = self.twosComplement(gyrodata[2], gyrodata[3])
            imu.angular_velocity.z = self.twosComplement(gyrodata[4], gyrodata[5])

            #gyro = int(nums[2])
            #ref = int(nums[3])
            #temp = int(nums[4])

            #val = (ref-gyro - self.bias) * 1000 / 3 / 1024 * self.scale

            #imu.angular_velocity.x = 0
            #imu.angular_velocity.y = 0
            #imu.angular_velocity.z = val * math.pi / 180
            imu.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 1]
            imu.orientation_covariance = [0.001, 0, 0, 0, 0.001, 0, 0, 0, 0.1]

            self.orientation += imu.angular_velocity.z * (imu.header.stamp - self.prev_time).to_sec()
            self.prev_time = imu.header.stamp
            (imu.orientation.x, imu.orientation.y, imu.orientation.z, imu.orientation.w) = Rotation.RotZ(self.orientation).GetQuaternion()
            self.pub.publish(imu)

	def _HandleReceivedLine(self,  line):
		self._Counter = self._Counter + 1
		self._SerialPublisher.publish(String(str(self._Counter) + ", in:  " + line))

		if(len(line) > 0):

			lineParts = line.split('\t')
			try:
			
				if(lineParts[0] == 'quat'):

					self._qx = float(lineParts[1])
					self._qy = float(lineParts[2])
					self._qz = float(lineParts[3])
					self._qw = float(lineParts[4])

				if(lineParts[0] == 'ypr'):

          				self._ax = float(lineParts[1])
          				self._ay = float(lineParts[2])
          				self._az = float(lineParts[3])

				if(lineParts[0] == 'areal'):

				  	self._lx = float(lineParts[1])
				  	self._ly = float(lineParts[2])
				  	self._lz = float(lineParts[3])

					imu_msg = Imu()
					h = Header()
					h.stamp = rospy.Time.now()
					h.frame_id = self.frame_id

					imu_msg.header = h

					imu_msg.orientation_covariance = (-1., )*9	
					imu_msg.angular_velocity_covariance = (-1., )*9
					imu_msg.linear_acceleration_covariance = (-1., )*9

					imu_msg.orientation.x = self._qx
					imu_msg.orientation.y = self._qy
					imu_msg.orientation.z = self._qz
					imu_msg.orientation.w = self._qw

					imu_msg.angular_velocity.x = self._ax
					imu_msg.angular_velocity.y = self._ay
					imu_msg.angular_velocity.z = self._az

					imu_msg.linear_acceleration.x = self._lx
					imu_msg.linear_acceleration.y = self._ly
					imu_msg.linear_acceleration.z = self._lz
					
					self.imu_pub.publish(imu_msg)

			except:
				rospy.logwarn("Error in Sensor values")
				rospy.logwarn(lineParts)
				pass

def GetImuFromLine(line):
	(ts, ax, ay, az, gx, gy, gz) = [t(s) for t,s in zip((int,int, int, int, int, int, int),line.split())]
	imu = Imu()
	ts = float(ts)/1000000.0
	ImuStamp = rospy.rostime.Time.from_sec(ts)
	imu.angular_velocity = create_vector3(gyro_raw_to_rads(gx), gyro_raw_to_rads(gy), gyro_raw_to_rads(gz))
	imu.angular_velocity_covariance = create_diag_mat(gyro_raw_to_rads(1.0), gyro_raw_to_rads(1.0), gyro_raw_to_rads(1.0));
	imu.linear_acceleration = create_vector3(acc_raw_to_ms(ax), acc_raw_to_ms(ay), acc_raw_to_ms(az))
	imu.linear_acceleration_covariance = create_diag_mat(acc_raw_to_ms(1.0), acc_raw_to_ms(1.0), acc_raw_to_ms(1.0))
	return (ts, ImuStamp, imu)