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

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

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import orientation_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 orientation_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 orientation_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 orientation_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 orientation_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 orientation_covariance[0] [as 别名]
    def _log_data_imu(self, timestamp, data, logconf):
        """Callback froma the log API when data arrives"""
        msg = Imu()
        # ToDo: it would be better to convert from timestamp to rospy time
        msg.header.stamp = rospy.Time.now()
        msg.header.frame_id = self.tf_prefix + "/base_link"
        msg.orientation_covariance[0] = -1 # orientation not supported

        # measured in deg/s; need to convert to rad/s
        msg.angular_velocity.x = math.radians(data["gyro.x"])
        msg.angular_velocity.y = math.radians(data["gyro.y"])
        msg.angular_velocity.z = math.radians(data["gyro.z"])

        # measured in mG; need to convert to m/s^2
        msg.linear_acceleration.x = data["acc.x"] * 9.81
        msg.linear_acceleration.y = data["acc.y"] * 9.81
        msg.linear_acceleration.z = data["acc.z"] * 9.81

        self._pubImu.publish(msg)

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import orientation_covariance[0] [as 别名]
 def pubImu(self, event):
     imu = Imu()
     imu.header.stamp = rospy.Time.now()
     imu.header.frame_id = str(self.frame_id)
     roll = self.p[3] + random.normal(0.0, self.imu_orientation_covariance[0])
     pitch = self.p[4] + random.normal(0.0, self.imu_orientation_covariance[1])
     yaw = self.p[5] + random.normal(0.0, self.imu_orientation_covariance[2])
     
     vehicle_rpy = PyKDL.Rotation.RPY(roll, pitch, yaw)
     imu_orientation = self.imu_tf.M * vehicle_rpy
     angle = imu_orientation.GetRPY()
     angle = tf.transformations.quaternion_from_euler(angle[0], angle[1], angle[2])
     imu.orientation.x = angle[0]
     imu.orientation.y = angle[1]
     imu.orientation.z = angle[2]
     imu.orientation.w = angle[3]
     
     imu.orientation_covariance[0] = self.imu_orientation_covariance[0]
     imu.orientation_covariance[4] = self.imu_orientation_covariance[1]
     imu.orientation_covariance[8] = self.imu_orientation_covariance[2]
     
     self.pub_imu.publish(imu)

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import orientation_covariance[0] [as 别名]
def talker():
    """Initializes the publisher node"""
    global pub
    pub = rospy.Publisher("mpu6050", Imu)
    rospy.init_node("MPU6050-Driver")

    # calibrate(imu, accel+gyro, samples = 0)

    global seq
    seq = 0

    while not rospy.is_shutdown():
        sample = Imu()
        sample.header = make_header()
        sample.orientation_covariance[0] = -1
        sample.angular_velocity_covariance = [0] * 9
        sample.angular_velocity = read_gyros()
        sample.linear_acceleration_covariance = [0] * 9
        sample.linear_acceleration = read_accels()

        rospy.loginfo(str(sample))
        pub.publish(sample)
        time.sleep(0.1)

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

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import orientation_covariance[0] [as 别名]
usage: imu_bag.py [MTI.log]
"""
import sys
import rospy
import rosbag
from sensor_msgs.msg import Image, Imu

filepath = 'MTI.log' if len(sys.argv) < 2 else sys.argv[1]

with open(filepath) as f:
    lines = f.readlines()

data = [map(float, line.split()) for line in lines if not line.startswith('#')]

imu = Imu()
imu.header.frame_id = 'imu'
imu.header.seq = 0

with rosbag.Bag(filepath+'.bag', 'w') as bag:
    for date, imu.linear_acceleration.x, imu.linear_acceleration.y, \
            imu.linear_acceleration.z, imu.angular_velocity.x, \
            imu.angular_velocity.y, imu.angular_velocity.z, \
            mag_x, mag_y, mag_z in data:
        imu.header.stamp = rospy.Time.from_sec(date)
        # Inform we doesn't have orientation estimates
        # see http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html
        imu.orientation_covariance[0] = -1
        bag.write("/imu", imu, imu.header.stamp)
        imu.header.seq += 1

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import orientation_covariance[0] [as 别名]
    rate = rospy.Rate(frequency)

    # Factors for unit conversions
    acc_fact = 1000.0
    mag_fact = 16.0
    gyr_fact = 900.0
    seq = 0

    while not rospy.is_shutdown():
        buf = read_from_dev(ser, ACCEL_DATA, 45)
        if buf != 0:
            # Publish raw data
            imu_raw.header.stamp = rospy.Time.now()
            imu_raw.header.frame_id = frame_id
            imu_raw.header.seq = seq
            imu_raw.orientation_covariance[0] = -1
            imu_raw.linear_acceleration.x = float(st.unpack('h', st.pack('BB', buf[0], buf[1]))[0]) / acc_fact
            imu_raw.linear_acceleration.y = float(st.unpack('h', st.pack('BB', buf[2], buf[3]))[0]) / acc_fact
            imu_raw.linear_acceleration.z = float(st.unpack('h', st.pack('BB', buf[4], buf[5]))[0]) / acc_fact
            imu_raw.linear_acceleration_covariance[0] = -1
            imu_raw.angular_velocity.x = float(st.unpack('h', st.pack('BB', buf[12], buf[13]))[0]) / gyr_fact
            imu_raw.angular_velocity.y = float(st.unpack('h', st.pack('BB', buf[14], buf[15]))[0]) / gyr_fact
            imu_raw.angular_velocity.z = float(st.unpack('h', st.pack('BB', buf[16], buf[17]))[0]) / gyr_fact
            imu_raw.angular_velocity_covariance[0] = -1
            pub_raw.publish(imu_raw)

            #            print("read: ", binascii.hexlify(buf), "acc = (",imu_data.linear_acceleration.x,
            #                  imu_data.linear_acceleration.y, imu_data.linear_acceleration.z, ")")

            # Publish filtered data
            imu_data.header.stamp = rospy.Time.now()

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

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