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

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import linear_acceleration_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 linear_acceleration_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 linear_acceleration_covariance[0] [as 别名]
    def send_message(self, event=None):
        if not self.valid or not self.connected:
            rospy.loginfo("%s: measurements not valid, not sending imu message!", self.name)
            return

        # NOTE:
        #   the axis convention is taken from the old driver implementation
        #   this should be checked again and explicitly stated in the documentation
        rotation = tft.quaternion_from_euler(0, 0, np.deg2rad(-self.yaw), axes="sxyz")

        # NOTE:
        # This is a message to hold data from an IMU (Inertial Measurement Unit)
        #
        # Accelerations should be in m/s^2 (not in g's), and rotational velocity should be in rad/sec
        #
        # If the covariance of the measurement is known, it should be filled in (if all you know is the
        # variance of each measurement, e.g. from the datasheet, just put those along the diagonal)
        # A covariance matrix of all zeros will be interpreted as "covariance unknown", and to use the
        # data a covariance will have to be assumed or gotten from some other source
        #
        # If you have no estimate for one of the data elements (e.g. your IMU doesn't produce an orientation
        # estimate), please set element 0 of the associated covariance matrix to -1
        # If you are interpreting this message, please check for a value of -1 in the first element of each
        # covariance matrix, and disregard the associated estimate.
        msg = Imu()
        msg.header.stamp = rospy.Time.now()
        msg.header.frame_id = self.frame_id
        msg.orientation.x = rotation[0]
        msg.orientation.y = rotation[1]
        msg.orientation.z = rotation[2]
        msg.orientation.w = rotation[3]

        # this is derived by looking at the datasheet under the bias offset factor
        # and it should include the drift of the gyro if the yaw is computed by integration
        msg.orientation_covariance[8] = np.deg2rad(self.yaw_std_z ** 2)

        # this is to notify that this imu is not measuring this data
        msg.linear_acceleration_covariance[0] = -1

        msg.angular_velocity.z = np.deg2rad(-self.yaw_dot)

        # this is derived by looking at the datasheet
        msg.angular_velocity_covariance[8] = np.deg2rad(KVH_SIGMA ** 2)

        self.pub_gyro.publish(msg)

        if self.mode == KVH_MODE_R:
            rospy.loginfo("%s: raw data: %+.5f -- yaw rate: %+.5f", self.name, self.data, self.yaw_dot)
        else:
            rospy.loginfo(
                "%s: raw data: %+.5f -- yaw rate: %+.5f -- yaw: %+.5f", self.name, self.data, self.yaw_dot, self.yaw
            )

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import linear_acceleration_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 linear_acceleration_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 linear_acceleration_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 linear_acceleration_covariance[0] [as 别名]
def pub_imu(msg_type, msg, bridge):
    pub = bridge.get_ros_pub("imu", Imu, queue_size=1)
    imu = Imu()
    imu.header.stamp = bridge.project_ap_time(msg)
    imu.header.frame_id = 'base_footprint'
    # Orientation as a Quaternion, with unknown covariance
    quat = quaternion_from_euler(msg.roll, msg.pitch, msg.yaw, 'sxyz')
    imu.orientation = Quaternion(quat[0], quat[1], quat[2], quat[3])
    imu.orientation_covariance = (0, 0, 0, 0, 0, 0, 0, 0, 0)
    # Angular velocities, with unknown covariance
    imu.angular_velocity.x = msg.rollspeed
    imu.angular_velocity.y = msg.pitchspeed
    imu.angular_velocity.z = msg.yawspeed
    imu.angular_velocity_covariance = (0, 0, 0, 0, 0, 0, 0, 0, 0)
    # Not supplied with linear accelerations
    imu.linear_acceleration_covariance[0] = -1
    pub.publish(imu)

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

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