Python Imu.angular_velocity_covariance[0]方法代码示例

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import angular_velocity_covariance[0] [as 别名]
    def read_from_imu(self):
        while True:
            try:
                imu_msg = Imu()
                imu_msg.header.stamp = rospy.Time.now()
                imu_msg.header.frame_id = 'imu'

                orientation = self.bno.read_quaternion()
                linear_acceleration = self.bno.read_linear_acceleration()
                angular_velocity = self.bno.read_gyroscope()

                imu_msg.orientation_covariance[0] = -1
                imu_msg.linear_acceleration_covariance[0] = -1
                imu_msg.angular_velocity_covariance[0] = -1

                imu_msg.orientation = Quaternion(orientation[1],
                                                 orientation[2],
                                                 orientation[3],
                                                 orientation[0])
                imu_msg.linear_acceleration = Vector3(linear_acceleration[0],
                                                      linear_acceleration[1],
                                                      linear_acceleration[2])
                imu_msg.angular_velocity = Vector3(np.deg2rad(angular_velocity[0]),
                                                   np.deg2rad(angular_velocity[1]),
                                                   np.deg2rad(angular_velocity[2]))
                return imu_msg
            except IOError:
                pass

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import angular_velocity_covariance[0] [as 别名]
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()

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import angular_velocity_covariance[0] [as 别名]
def callback(raw_data, pub):
    imu = Imu()
    imu.header = raw_data.header

    # set imu.orientation quaternion
    # Btw, robot_pose_ekf expects the Roll and Pitch angles to be
    # absolute, but the Yaw angle to be relative. Compute the relative
    # angle from the currently reported and previously reported absolute
    # angle
    r = raw_data.roll
    p = raw_data.pitch
    y = raw_data.yaw - callback.old_yaw
    callback.old_yaw = raw_data.yaw

    quat = quaternion_from_euler(r,p,y,"sxyz")

    imu.orientation.x = quat[0]
    imu.orientation.y = quat[1]
    imu.orientation.z = quat[2]
    imu.orientation.w = quat[3]

    #TODO: set imu.orientation_covariance
    #row major about x, y, z
    # According to OS5000 datasheet, accuracy
    # depends on tilt:
    # 0.5 deg RMS level heading,
    # 1 deg typical RMS accuracy < +-30deg tilt,
    # 1.5deg < +-60deg tilt
    # Roll and Pitch:
    # Typical 1deg accuracy for tilt < +-30 deg
    # Assume tilt < +- 30 deg, set all standard
    # deviations to 1 degree
    std_dev = from_degrees(1)
    imu.orientation_covariance[0] = std_dev**2
    imu.orientation_covariance[4] = std_dev**2
    # standard deviation on yaw is doubled since
    # it's computed as a difference of two measurements,
    # each with a std_dev of 1 degree
    imu.orientation_covariance[8] = (2*std_dev)**2

    # no angular velocity data is available, so set
    # element 0 of covariance matrix to -1
    # alternatively, set the covariance really high
    imu.angular_velocity = Vector3(0,0,0)
    imu.angular_velocity_covariance[0] = -1

    # Set linear acceleration
    imu.linear_acceleration = raw_data.acceleration
    # TODO: Set linear acceleration covariance
    imu.linear_acceleration_covariance[0] = -1

    pub.publish(imu)

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import angular_velocity_covariance[0] [as 别名]
    def publish(self,event):
        compass_accel = self.compass_accel.read()
        compass = compass_accel[1]
        accel = compass_accel[0]
        gyro = self.gyro.read()
        # Put together an IMU message
        imu_msg = Imu()
        imu_msg.header.stamp = rospy.Time.now()
	imu_msg.header.frame_id = 'map'
        # covariance matrix
        imu_msg.orientation_covariance[0] = -1
        imu_msg.angular_velocity_covariance[0] = -1
        imu_msg.linear_acceleration_covariance[0] = -1
        # angular velocity
        imu_msg.angular_velocity.x = gyro[0][0]*self.DEG2RAD
        imu_msg.angular_velocity.y = gyro[0][1]*self.DEG2RAD
        imu_msg.angular_velocity.z = gyro[0][2]*self.DEG2RAD
        # linear acceleration
        imu_msg.linear_acceleration.x = accel[0]*self.G
        imu_msg.linear_acceleration.y = accel[1]*self.G
        imu_msg.linear_acceleration.z = (accel[2])*self.G
        # calculate RPY
	# refers to Adafruit_9DOF.cpp in GitHub
        roll = math.atan2(accel[1], (accel[2]))
	pitch = math.atan2(-accel[0], (accel[1] * math.sin(roll) + accel[2] * math.cos(roll)))
	#yaw = math.atan2(compass[1],compass[0])
	yaw = 0
	#print "R=", math.degrees(roll)," P=", math.degrees(pitch), " Y=",math.degrees(yaw) # uncomment to print RPY
	# convert RPY to quaternion
	quaternion = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
        # orientation
        imu_msg.orientation.x = quaternion[0]
	imu_msg.orientation.y = quaternion[1]
	imu_msg.orientation.z = quaternion[2]
	imu_msg.orientation.w = quaternion[3]
        # publish
        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]
        # publish
        self.pub_mag.publish(mag_msg)
	
	RY_msg = Float64MultiArray()
	RY_msg.data = roll, pitch
	self.pub_RP.publish(RY_msg)
	'''self.br.sendTransform((0,0,0,),

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import angular_velocity_covariance[0] [as 别名]
    def publish(self,event):
        compass_accel=self.compass_accel.read()
        compass=compass_accel[0]
        accel=compass_accel[1]
        gyro=self.gyro.read()
        # Put together an IMU message
        imu_msg=Imu()
        imu_msg.header.stamp=rospy.Time.now()
	imu_msg.header.frame_id="sean"
        # covariance matrix
        imu_msg.orientation_covariance[0]=-1
        imu_msg.angular_velocity_covariance[0]=-1
        imu_msg.linear_acceleration_covariance[0]=-1
        # angular velocity
        imu_msg.angular_velocity.x=gyro[0][0]*self.DEG2RAD
        imu_msg.angular_velocity.y=gyro[0][1]*self.DEG2RAD
        imu_msg.angular_velocity.z=gyro[0][2]*self.DEG2RAD
        # linear acceleration
        imu_msg.linear_acceleration.x=accel[0]*self.G
        imu_msg.linear_acceleration.y=accel[1]*self.G
        imu_msg.linear_acceleration.z=accel[2]*self.G
        # pitch roll yaw
        roll=math.atan2(imu_msg.linear_acceleration.y,imu_msg.linear_acceleration.z)
	if (imu_msg.linear_acceleration.y * math.sin(roll) + imu_msg.linear_acceleration.z * math.cos(roll) == 0):
		#pitch = imu_msg.linear_acceleration.x > 0 ? (math.pi / 2) : (math.pi / 2)
		if	imu_msg.linear_acceleration.x > 0:
			pitch=math.pi/2
		else:
			pitch=-math.pi/2
	else:
		pitch = math.atan(-imu_msg.linear_acceleration.x / (imu_msg.linear_acceleration.y * math.sin(roll) + imu_msg.linear_acceleration.z * math.cos(roll)))
	yaw = math.atan2(compass[2] * math.sin(roll) - compass[1] * math.cos(roll), compass[0] * math.cos(pitch) + compass[1] * math.sin(pitch) * math.sin(roll) + compass[2] * math.sin(pitch) * math.cos(roll))
        # orientation
        imu_msg.orientation.x=roll*180/math.pi
        imu_msg.orientation.y=pitch*180/math.pi
        imu_msg.orientation.z=yaw*180/math.pi
        imu_msg.orientation.w=0
        # publish
        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]
        # publish
        self.pub_mag.publish(mag_msg)

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import angular_velocity_covariance[0] [as 别名]
	def imuCallback(self,imuData):
		imu = Imu()
		imu.header.stamp = rospy.Time.now()
		imu.header.frame_id = "imu_frame"#"imu_frame"
		imu.angular_velocity.x = -(imuData.gyro[0] - GYRO_X_OFFSET)/self.GYRO_SENSITIVITY*pi/180
		imu.angular_velocity.y = -(imuData.gyro[1] - GYRO_Y_OFFSET)/self.GYRO_SENSITIVITY*pi/180
		imu.angular_velocity.z = -(imuData.gyro[2] - GYRO_Z_OFFSET)/self.GYRO_SENSITIVITY*pi/180
		
		imu.orientation_covariance[0]=0.001
		imu.orientation_covariance[4]=0.001
		imu.orientation_covariance[8]=0.001

		imu.angular_velocity_covariance[0]=0.001
		imu.angular_velocity_covariance[4]=0.001
		imu.angular_velocity_covariance[8]=0.001


		x=-imuData.accel[0]
		y=-imuData.accel[1]
		z=-imuData.accel[2]
		effectiveG = sqrt(x**2 + y**2 + z**2)
		x = float(x)/effectiveG*g
		y = float(y)/effectiveG*g
		z = float(z)/effectiveG*g
	
		
		imu.linear_acceleration.x = x
		imu.linear_acceleration.y = y
		imu.linear_acceleration.z = z
		
		imu.linear_acceleration_covariance[0]=0.001
		imu.linear_acceleration_covariance[4]=0.001
		imu.linear_acceleration_covariance[8]=0.001
		
		mag = Vector3Stamped()
		mag.header.stamp = imu.header.stamp
		mag.header.frame_id = "imu_frame"
		mag.vector.x = imuData.magneto[0] - MAGNETO_X_OFFSET
		mag.vector.y = imuData.magneto[1] - MAGNETO_Y_OFFSET
		mag.vector.z = imuData.magneto[2] - MAGNETO_Z_OFFSET
		
		self.pubImu.publish(imu)
		self.pubMag.publish(mag)

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import angular_velocity_covariance[0] [as 别名]
    def cb_osc_accxyz(self, address_list, value_list, send_address):
        """
        Callback for when accel data is received from a device.
        
        Populates a sensor_msgs/Imu message, including the ip address of the
        sending client in the msg.header.frame_id field.
        
        @param address_list: A list with the OSC address parts in it.
        @type address_list: C{list}
        @param value_list: A list with the OSC value arguments in it.
        @type value_list: C{list}
        @param send_address: A tuple with the (ip, port) of the sender.
        @type send_address: C{tuple}
        """
        msg = Imu()
        msg.linear_acceleration.x = value_list[0] * 9.80665
        msg.linear_acceleration.y = value_list[1] * 9.80665
        msg.linear_acceleration.z = value_list[2] * 9.80665

        msg.header.frame_id = send_address[0]
        msg.header.stamp = rospy.Time.now()
        # Covariance was calculated from about 20 minutes of static data
        # Conditions:
        #    * Back down
        #    * Plugged In
        #    * Vibrate Off
        #    * Cell and Wifi On
        # Results:
        #          x                y                z
        # Mean:    0.2934510093    -0.2174349315    -9.8049353269
        # Stdev:   0.0197007054     0.0205649244     0.0259846818
        # Var:     0.0003881178     0.0004229161     0.0006752037
        var = 0.0008
        msg.linear_acceleration_covariance = [var, 0, 0, 0, var, 0, 0, 0, var]
        msg.angular_velocity_covariance = [0.0] * 9
        msg.angular_velocity_covariance[0] = -1.0
        msg.orientation_covariance = msg.angular_velocity_covariance
        self.accel_pub.publish(msg)

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import angular_velocity_covariance[0] [as 别名]
        imuData = Imu()
        imuData.orientation.x = fusionQPose[1]
        imuData.orientation.y = fusionQPose[2]
        imuData.orientation.z = fusionQPose[3]
        imuData.orientation.w = fusionQPose[0]
        imuData.angular_velocity.x = gyr[0]
        imuData.angular_velocity.y = gyr[1]
        imuData.angular_velocity.z = gyr[2]
        imuData.linear_acceleration.x = acc[0] * 9.81
        imuData.linear_acceleration.y = acc[1] * 9.81
        imuData.linear_acceleration.z = acc[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
        pubImu.publish(imuData)
        pubDepth.publish(depthData)
    # END OF IMU

  #        
  # At reduced rate display data (10Hz)
  # 
  if ((currentTimeS - previousDisplayTimeS) > 0.1):
    if(displayIMU):
      if (data["motion"]):

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import angular_velocity_covariance[0] [as 别名]
def talker():
	pub = rospy.Publisher('imu', Imu, queue_size=10)
	rospy.init_node('ros_erle_imu', anonymous=True)
	rate = rospy.Rate(10)

	imu = MPU9250()
	imu.initialize()
	
	msg = Imu()

	while not rospy.is_shutdown():

		imu.read_acc()
		
		m9a, m9g, m9m = imu.getMotion9()

		msg.header.stamp = rospy.get_rostime()

		#ORIENTATION calculation
		'''You can just integrate your angular velocity to get angular 
		position (as Euler angles), convert the Euler angles to Quaternion, 
		then multiply the Quaternion to accumulate the orientation.

		Suppose your input is given by a 3D vector of angular velocity: 
		omega = (alpha, beta, gamma), given by degrees per second. 
		To get the Euler angles, E, in degrees, multiply omega by the change 
		in time, which we can call dt '''

		dt = 0.1 #10Hz in seconds
		
		AngVelX = m9g[0]*57.29577951308*dt
		AngVelY = m9g[1]*57.29577951308*dt
		AngVelZ = m9g[2]*57.29577951308*dt

		w_or = math.cos(AngVelX/2) * math.cos(AngVelY/2) * math.cos(AngVelZ/2) + math.sin(AngVelX/2) * math.sin(AngVelY/2) * math.sin(AngVelZ/2)
		x_or = math.sin(AngVelX/2) * math.cos(AngVelY/2) * math.cos(AngVelZ/2) - math.cos(AngVelX/2) * math.sin(AngVelY/2) * math.sin(AngVelZ/2)
		y_or = math.cos(AngVelX/2) * math.sin(AngVelY/2) * math.cos(AngVelZ/2) + math.sin(AngVelX/2) * math.cos(AngVelY/2) * math.sin(AngVelZ/2)
		z_or = math.cos(AngVelX/2) * math.cos(AngVelY/2) * math.sin(AngVelZ/2) - math.sin(AngVelX/2) * math.sin(AngVelY/2) * math.cos(AngVelZ/2)

		msg.orientation.x = x_or
		msg.orientation.y = y_or
		msg.orientation.z = z_or
		msg.orientation.w = w_or
		msg.orientation_covariance[0] = x_or * x_or
		msg.orientation_covariance[0] = y_or * y_or
		msg.orientation_covariance[0] = z_or * z_or		

		msg.angular_velocity.x = m9g[0]
		msg.angular_velocity.y = m9g[1]
		msg.angular_velocity.z = m9g[2]
		msg.angular_velocity_covariance[0] = m9g[0] * m9g[0]
                msg.angular_velocity_covariance[4] = m9g[1] * m9g[1]
                msg.angular_velocity_covariance[8] = m9g[2] * m9g[2]
		
		
		msg.linear_acceleration.x = m9a[0]
		msg.linear_acceleration.y = m9a[1]
		msg.linear_acceleration.z = m9a[2]
		msg.linear_acceleration_covariance[0] = m9a[0] * m9a[0]
		msg.linear_acceleration_covariance[4] = m9a[1] * m9a[1]
		msg.linear_acceleration_covariance[8] = m9a[2] * m9a[2]
		
		pub.publish(msg)

		rate.sleep()

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import angular_velocity_covariance[0] [as 别名]
def smcimu():
    rospy.init_node('smcimu')
    pub = rospy.Publisher('~imu', Imu)

    # path to read the data from
    path = rospy.get_param('~path', None)
    if path == None:
        path = glob.iglob('/sys/devices/platform/applesmc.*/position').next()

    # path to read the calibration from
    calibrationPath = rospy.get_param('~calibration_file', None)
    if calibrationPath == None:
        calibrationPath = roslib.packages.resource_file('smcimu', 'info', 'calibration.yaml')

    # read calibration
    calib = None
    with open(calibrationPath, 'r') as f:
        calib = yaml.load(f.read())

    targetSamples = int(rospy.get_param('~samples', 5))
    targetRate = float(rospy.get_param('~rate', 30))
    rate = rospy.Rate(targetRate * targetSamples)

    # parse '(x,y,z)'
    parsePosition = re.compile('\((-?\d+),(-?\d+),(-?\d+)\)')

    while not rospy.is_shutdown():
        actualSamples = 0
        data = [0, 0, 0]
        # collect samples
        for i in range(targetSamples):
            try:
                with open(path) as f:
                    match = parsePosition.match(f.read())
                    data[0] += int(match.group(1))
                    data[1] += int(match.group(2))
                    data[2] += int(match.group(3))
                    actualSamples += 1
            except Exception as e:
                # this seems to happen quite a bit
                rospy.logwarn(rospy.get_name() + ': Error reading data: %s', str(e))
            rate.sleep()

        if actualSamples != 0:
            ix, iy, iz = [a / float(actualSamples) for a in data]
            iw = 1.0
            # transform the data
            x = ix * calib[0] + iy * calib[4] + iz * calib[8] + iw * calib[12]
            y = ix * calib[1] + iy * calib[5] + iz * calib[9] + iw * calib[13]
            z = ix * calib[2] + iy * calib[6] + iz * calib[10] + iw * calib[14]
            w = ix * calib[3] + iy * calib[7] + iz * calib[11] + iw * calib[15]
            x, y, z = [a / w for a in (x, y, z)]
            #w = 1.0
            # send out the Imu message
            imu = Imu()
            imu.header.stamp = rospy.Time.now()
            imu.linear_acceleration.x = x
            imu.linear_acceleration.y = y
            imu.linear_acceleration.z = z
            # I don't know the covariance
            # these values should be transformed by the calibration matrix
            # I just copied how kinect_aux does this
            # the sensor in my MBP seems more accurate than this
            imu.linear_acceleration_covariance[0] = imu.linear_acceleration_covariance[4] = imu.linear_acceleration_covariance[8] = 0.01
            # -1 in covariance[0] means we don't support that data
            imu.angular_velocity_covariance[0] = -1
            imu.orientation_covariance[0] = -1
            pub.publish(imu)
        else:
            rospy.logerr(rospy.get_name() + ' gathered 0 samples', data)

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import angular_velocity_covariance[0] [as 别名]
def imu_publisher(sock):
    host= "192.168.42.45" #IP OF PHONE, get using IFCONFIG. Set same IP in HyperIMU
    port=5555
    theta = 0
    gyro_x_offset = 0.0
    gyro_y_offset = 0.0
    gyro_z_offset = 0.0
    pub_freq = 10
    alpha = 0.9
    count = 0
    num_callibration_itrs = 60
    debug = False

    gyro_pub = rospy.Publisher('gyro', Vector3, queue_size=50)
    imu_pub = rospy.Publisher('imu', Imu, queue_size=50)
    rospy.init_node('imu_publisher', anonymous=True)
    rate = rospy.Rate(pub_freq)
    if rospy.has_param('~num_callibration_itrs'):
        num_callibration_itrs = rospy.get_param('~num_callibration_itrs')
    if rospy.has_param('~host'):
        host = rospy.get_param('~host')
    if rospy.has_param('~debug'):
        debug = rospy.get_param('~debug')

    sock.bind((host,port))

    current_time = rospy.Time.now()
    last_time = rospy.Time.now()

    rospy.loginfo("waiting for device...")
    while not rospy.is_shutdown():
        data,addr = sock.recvfrom(1024)
        line = data.split(',')

        if len(line) == 3:  #received complete packet TODO: 4 or 3?
            line[2] = line[2].replace("#","")
            current_time = rospy.Time.now()
            gyro_x = float(line[0])
            gyro_y = float(line[1])
            gyro_z = float(line[2])
            if count < num_callibration_itrs:
                gyro_x_offset += gyro_x
                gyro_y_offset += gyro_y
                gyro_z_offset += gyro_z
                count += 1
            elif count == num_callibration_itrs and num_callibration_itrs != 0:
                gyro_x_offset /= num_callibration_itrs
                gyro_y_offset /= num_callibration_itrs
                gyro_z_offset /= num_callibration_itrs
                rospy.loginfo("finished callibrating yaw")
                count += 1

            #publish ros Imu message
            else:
                gyro_x -= gyro_x_offset
                gyro_y -= gyro_y_offset
                gyro_z -= gyro_z_offset

                #discretize readings to round off noise
                #gyro_x = float(int(gyro_x*100))/100.0;
                #gyro_y = float(int(gyro_y*100))/100.0;
                #gyro_z = float(int(gyro_z*100))/100.0;
                if debug:
                    rospy.loginfo('x %s y %s z %s', gyro_x, gyro_y, gyro_z)
                gyro_msg = Vector3()
                gyro_msg.x = gyro_x
                gyro_msg.y = gyro_y
                gyro_msg.z = gyro_z
                gyro_pub.publish(gyro_msg)

                dt = current_time.to_sec() - last_time.to_sec()
                theta += dt*gyro_z
                imu_msg = Imu()
                imu_msg.header.stamp = rospy.Time.now()
                imu_msg.header.frame_id = '/base_link'
                q = tf.transformations.quaternion_from_euler(0.0, 0.0, theta)
                imu_msg.orientation.x = q[0]
                imu_msg.orientation.y = q[1]
                imu_msg.orientation.z = q[2]
                imu_msg.orientation.w = q[3]
                imu_msg.orientation_covariance = [1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6]
                imu_msg.angular_velocity_covariance[0] = -1
                imu_msg.linear_acceleration_covariance[0] = -1
                imu_pub.publish(imu_msg)
            last_time = current_time
            #rate.sleep()
        else:
            rospy.loginfo("received incomplete UDP packet from android IMU")
            continue

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import angular_velocity_covariance[0] [as 别名]

#.........这里部分代码省略.........

		m9a, m9g, m9m = imu.getMotion9()


		#----------update IMU
		ax = m9a[0]
		ay = m9a[1]
		az = m9a[2]
		gx = m9g[0]
		gy = m9g[1]
		gz = m9g[2]
		q0 = 0.0 #W
		q1 = 0.0 #X
		q2 = 0.0 #Y
		q3 = 0.0 #Z
		
		'''
		#----------Calculate delta time
		t = time()
		currenttime = 0
		previoustime = currenttime
		currenttime = 1000000 * t + t / 1000000
		dt = (currenttime - previoustime) / 1000000.0
		if (dt < (1/1300.0)) : 
			time.sleep((1/1300.0 - dt) * 1000000)
		t = time()
		currenttime = 1000000 * t + t / 1000000
		dt = (currenttime - previoustime) / 1000000.0
		print "Delta time: d = %f" % dt
		#Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
		if not ((ax == 0.0) and (ay == 0.0) and (az == 0.0)) :
			#Normalise accelerometer measurement
			recipNorm = (ax * ax + ay * ay + az * az)**-.5
			ax *= recipNorm
			ay *= recipNorm
			az *= recipNorm
			#Estimated direction of gravity and vector perpendicular to magnetic flux
			halfvx = q1 * q3 - q0 * q2
			halfvy = q0 * q1 + q2 * q3
			halfvz = q0 * q0 - 0.5 + q3 * q3
			#Error is sum of cross product between estimated and measured direction of gravity
			halfex = (ay * halfvz - az * halfvy)
			halfey = (az * halfvx - ax * halfvz)
			halfez = (ax * halfvy - ay * halfvx)
			#Compute and apply integral feedback (if enabled)
			integralFBx += twoKi * halfex * dt;
			integralFBy += twoKi * halfey * dt;
			integralFBz += twoKi * halfez * dt;
			gx += integralFBx
			gy += integralFBy
			gz += integralFBz
			#Apply proportional feedback
			gx += twoKp * halfex;
			gy += twoKp * halfey;
			gz += twoKp * halfez;
		#Integrate rate of change of quaternion
		gx *= (0.5 * dt)
		gy *= (0.5 * dt)
		gz *= (0.5 * dt)
		qa = q0
		qb = q1
		qc = q2
		q0 += (-qb * gx - qc * gy - q3 * gz)
		q1 += (qa * gx + qc * gz - q3 * gy)
		q2 += (qa * gy - qb * gz + q3 * gx)
		q3 += (qa * gz + qb * gy - qc * gx)
		#Normalise quaternion
		recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3)
		q0 *= recipNorm
		q1 *= recipNorm
		q2 *= recipNorm
		q3 *= recipNorm
		'''

		#Fill message
		msg.orientation.x = q1
		msg.orientation.y = q2
		msg.orientation.z = q3
		msg.orientation.w = q0
		msg.orientation_covariance[0] = q1 * q1
		msg.orientation_covariance[0] = q2 * q2
		msg.orientation_covariance[0] = q3 * q3		

		msg.angular_velocity.x = m9g[0]
		msg.angular_velocity.y = m9g[1]
		msg.angular_velocity.z = m9g[2]
		msg.angular_velocity_covariance[0] = m9g[0] * m9g[0]
        	msg.angular_velocity_covariance[4] = m9g[1] * m9g[1]
        	msg.angular_velocity_covariance[8] = m9g[2] * m9g[2]
		
		msg.linear_acceleration.x = m9a[0]
		msg.linear_acceleration.y = m9a[1]
		msg.linear_acceleration.z = m9a[2]
		msg.linear_acceleration_covariance[0] = m9a[0] * m9a[0]
		msg.linear_acceleration_covariance[4] = m9a[1] * m9a[1]
		msg.linear_acceleration_covariance[8] = m9a[2] * m9a[2]
		
		pub.publish(msg)

		rate.sleep()

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import angular_velocity_covariance[0] [as 别名]

#.........这里部分代码省略.........
            mag_z = float(line[12])

            if count == 0:
                rospy.loginfo("callibrating accelerometer and gyroscope readings for %s itrs...", num_callibration_itrs)
                for i in range(0, num_callibration_itrs):
                    gyro_x_offset += gyro_x
                    gyro_y_offset += gyro_y
                    gyro_z_offset += gyro_z
                gyro_x_offset /= num_callibration_itrs
                gyro_y_offset /= num_callibration_itrs
                gyro_z_offset /= num_callibration_itrs

            #filter accel and mag readings with low pass filter
            accel_x_filt += alpha * (accel_x - accel_x_filt)
            accel_y_filt += alpha * (accel_y - accel_y_filt)
            accel_z_filt += alpha * (accel_z - accel_z_filt)
            
            mag_x_filt += alpha * (mag_x - mag_x_filt)
            mag_y_filt += alpha * (mag_y - mag_y_filt)
            mag_z_filt += alpha * (mag_z - mag_z_filt)

            gyro_x_filt += alpha * (gyro_x - gyro_x_filt)
            gyro_y_filt += alpha * (gyro_y - gyro_y_filt)
            gyro_z_filt += alpha * (gyro_z - gyro_z_filt)

            #normalize accel and mag readings
            accel_norm = sqrt(pow(accel_x_filt,2)+pow(accel_y_filt,2)+pow(accel_z_filt,2))
            accel_x_filt /= accel_norm
            accel_y_filt /= accel_norm
            accel_z_filt /= accel_norm 

            mag_norm = sqrt(pow(mag_x_filt,2)+pow(mag_y_filt,2)+pow(mag_z_filt,2))
            mag_x_filt /= mag_norm
            mag_y_filt /= mag_norm
            mag_z_filt /= mag_norm

            #discretize gyro readings
            #gyro_z = float(int((gyro_z) * 100 / 20))/5
            #print gyro_z
            #calculate roll pitch yaw
            pitch   = atan2(accel_x_filt, sqrt(pow(accel_y_filt,2) + pow(accel_z_filt,2)))
            roll    = atan2(-accel_y_filt, sqrt(pow(accel_x_filt,2) + pow(accel_z_filt,2)))
            yaw_mag = atan2(-mag_y_filt*cos(roll)+mag_z_filt*sin(roll),
                             mag_x_filt*cos(pitch)+mag_z_filt*sin(pitch)*sin(roll)+mag_z_filt*sin(pitch)*cos(roll))
            yawU = atan2(mag_x_filt, mag_y_filt)
            dt = current_time.to_sec() - last_time.to_sec()
            yaw_cf = alpha*(yaw_cf + dt*(gyro_z-gyro_z_offset)) + (1-alpha)*yawU

            roll_gyro += dt*(gyro_x_filt - gyro_x_offset)
            pitch_gyro += dt*(gyro_y_filt - gyro_y_offset)
            yaw_gyro += dt*(gyro_z - gyro_z_offset)

            #callibrate roll pitch yaw
            #roll = roll_gyro
            #pitch = pitch_gyro
            yaw = yaw_gyro
            if count < num_callibration_itrs:
                roll_offset += roll
                pitch_offset += pitch
                yaw_offset += yaw
                count += 1
            elif count == num_callibration_itrs:
                roll_offset /= num_callibration_itrs
                pitch_offset /= num_callibration_itrs
                yaw_offset /= num_callibration_itrs
                rospy.loginfo("finished callibrating rpy")
                count += 1

            #publish ros Imu message
            else:
                roll -= roll_offset
                pitch -= pitch_offset
                yaw -= yaw_offset
                if roll < -pi:
                    roll += 2*pi
                if pitch < -pi:
                    pitch += 2*pi
                if yaw > pi:
                    yaw -= 2*pi
                if yaw < -pi:
                    yaw += 2*pi
                if debug:
                    rospy.loginfo('roll %s pitch %s yaw %s', int(roll*180/pi), int(pitch*180/pi), int(yaw*180/pi))
                imu_msg = Imu()
                imu_msg.header.stamp = rospy.Time.now()
                imu_msg.header.frame_id = '/base_link'
                q = tf.transformations.quaternion_from_euler(0.0, 0.0, yaw)
                imu_msg.orientation.x = q[0]
                imu_msg.orientation.y = q[1]
                imu_msg.orientation.z = q[2]
                imu_msg.orientation.w = q[3]
                imu_msg.orientation_covariance = [1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6]
                imu_msg.angular_velocity_covariance[0] = -1
                imu_msg.linear_acceleration_covariance[0] = -1
                pub.publish(imu_msg)
            last_time = current_time
            #rate.sleep()
        else:
            rospy.loginfo("received incomplete UDP packet from android IMU")
            continue

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import angular_velocity_covariance[0] [as 别名]
def imu_publisher(sock):
    host="192.168.42.185"
    port=5555
    yaw_offset = 0.0
    roll_offset = 0.0
    pitch_offset = 0.0
    pub_freq = 10
    alpha = 0.9
    count = 0
    num_callibration_itrs = 60
    debug = False

    pub = rospy.Publisher('imu', Imu, queue_size=50)
    rpy_pub = rospy.Publisher('rpy', Vector3, queue_size=50)

    rospy.init_node('imu_publisher', anonymous=True)
    rate = rospy.Rate(pub_freq)
    if rospy.has_param('~num_callibration_itrs'):
        num_callibration_itrs = rospy.get_param('~num_callibration_itrs')
    if rospy.has_param('~host'):
        host = rospy.get_param('~host')
    if rospy.has_param('~debug'):
        debug = rospy.get_param('~debug')

    sock.bind((host,port))

    current_time = rospy.Time.now()
    last_time = rospy.Time.now()

    rospy.loginfo("waiting for device...")
    while not rospy.is_shutdown():
        data,addr = sock.recvfrom(1024)
        line = data.split(',')
        if len(line) == 4:  #received complete packet
            current_time = rospy.Time.now()
            yaw = float(line[0])
            roll = float(line[1])
            pitch = float(line[2])
            if count < num_callibration_itrs:
                yaw_offset += yaw
                roll_offset += roll
                pitch_offset += pitch
                count += 1
            elif count == num_callibration_itrs:
                yaw_offset /= num_callibration_itrs
                roll_offset /= num_callibration_itrs
                pitch_offset /= num_callibration_itrs
                rospy.loginfo("finished callibrating yaw")
                count += 1

            #publish ros Imu message
            else:
                yaw -= yaw_offset
                roll -= roll_offset
                pitch -= pitch_offset
                if yaw > pi:
                    yaw -= 2*pi
                if yaw < -pi:
                    yaw += 2*pi
                if roll > pi:
                    roll -= 2*pi
                if roll < -pi:
                    roll += 2*pi
                if pitch > pi:
                    pitch -= 2*pi
                if pitch < -pi:
                    pitch += 2*pi
                if debug:
                    rospy.loginfo('roll %s pitch %s yaw %s', int(roll), int(pitch), int(yaw))
                imu_msg = Imu()
                imu_msg.header.stamp = rospy.Time.now()
                imu_msg.header.frame_id = '/base_link'
                q = tf.transformations.quaternion_from_euler(0.0, 0.0, yaw)
                imu_msg.orientation.x = q[0]
                imu_msg.orientation.y = q[1]
                imu_msg.orientation.z = q[2]
                imu_msg.orientation.w = q[3]
                #imu_msg.orientation_covariance = [1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6]
                imu_msg.angular_velocity_covariance[0] = -1
                imu_msg.linear_acceleration_covariance[0] = -1
                pub.publish(imu_msg)
                rpy_msg = Vector3()
                rpy_msg.x = roll
                rpy_msg.y = pitch
                rpy_msg.z = yaw
                rpy_pub.publish(rpy_msg)
            last_time = current_time
            #rate.sleep()
        else:
            rospy.loginfo("received incomplete UDP packet from android IMU")
            continue

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import angular_velocity_covariance[0] [as 别名]
    def publish(self, etherbotix):
        # Can't publish yet if don't know the version of our IMU
        if etherbotix.version > 0 and etherbotix.getImuVersion() == 0:
           return

        # Need to read params first time we get version
        if self.version == 0:
            # Save IMU version so we only run this once
            self.version = etherbotix.getImuVersion()

            # Backwards compatability with old firmware
            if etherbotix.version == 0:
                self.version = 2
                # L3GD20 (2000dps mode)
                self.gyro_scale = 0.001221111
                self.gyro_covariance = 0.004868938
                # LSM303DLHC (2g/full scale mode)
                self.accel_scale = 0.000598773
                self.accel_covariance = 0.34644996
                # LSM303DLHC (
                self.mag_scale = 0.000909

            # Set default params
            if self.version == 2:
                # L3GD20 (2000dps mode)
                self.gyro_scale = 0.001221111
                self.gyro_covariance = 0.004868938
                # LSM303DLHC (8g/full scale mode)
                self.accel_scale = 0.0023957
                self.accel_covariance = 0.34644996
                # LSM303DLHC (8.1 gauss mode)
                self.mag_scale = 0.0043478
            elif self.version == 3:
                # L3GD20H (2000dps mode)
                self.gyro_scale = 0.001221111
                self.gyro_covariance = 0.004868938
                # LSM303D (8g/full scale mode)
                self.accel_scale = 0.0023964
                self.accel_covariance = 0.34644996
                # LSM303D (12 gauss mode)
                self.mag_scale = 0.000479
            elif self.version == 5:
                # LSM6DS33 (2000dps mode)
                self.gyro_scale = 0.001221111
                self.gyro_covariance = 0.004868938
                # LSM6DS33 (8g/full scale mode)
                self.accel_scale = 0.0023964
                self.accel_covariance = 0.34644996
                # LIS3MDL (12 gauss mode)
                self.mag_scale = 0.000438404

            self.gyro_scale = rospy.get_param("~imu/gyro/scale", self.gyro_scale)
            self.gyro_covariance = rospy.get_param("~imu/gyro/covariance", self.gyro_covariance)

            self.accel_scale = rospy.get_param("~imu/accel/scale", self.accel_scale)
            self.accel_covariance = rospy.get_param("~imu/accel/covariance", self.accel_covariance)

            self.mag_scale = rospy.get_param("~imu/mag/scale", self.mag_scale)

        msg = Imu()
        msg.header.stamp = rospy.Time.now()
        msg.header.frame_id = self.frame

        # No known orientation
        msg.orientation_covariance[0] = -1.0

        msg.angular_velocity.x = etherbotix.gyro_x * self.gyro_scale
        msg.angular_velocity.y = etherbotix.gyro_y * self.gyro_scale
        msg.angular_velocity.z = etherbotix.gyro_z * self.gyro_scale
        msg.angular_velocity_covariance[0] = self.gyro_covariance
        msg.angular_velocity_covariance[4] = self.gyro_covariance
        msg.angular_velocity_covariance[8] = self.gyro_covariance

        msg.linear_acceleration.x = etherbotix.accel_x * self.accel_scale
        msg.linear_acceleration.y = etherbotix.accel_y * self.accel_scale
        msg.linear_acceleration.z = etherbotix.accel_z * self.accel_scale
        msg.linear_acceleration_covariance[0] = self.accel_covariance
        msg.linear_acceleration_covariance[4] = self.accel_covariance
        msg.linear_acceleration_covariance[8] = self.accel_covariance

        self._pub.publish(msg)

        if self._pub_mag:
            mag = Vector3Stamped()
            mag.header = msg.header
            mag.vector.x = etherbotix.mag_x * self.mag_scale
            mag.vector.y = etherbotix.mag_y * self.mag_scale
            mag.vector.z = etherbotix.mag_z * self.mag_scale
            self._pub_mag.publish(mag)