Python Imu.orientation_covariance方法代码示例

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import orientation_covariance [as 别名]
    def _send_one(self, now, angle):
        msg = Imu()
        msg.header.stamp = now
        msg.header.seq = self._seq
        self._seq += 1
        msg.header.frame_id = self._frame_id
        msg.angular_velocity_covariance = np.zeros(9)
        msg.orientation_covariance = np.zeros(9)
        if self._gyro.current_mode == self._gyro.MODE_RATE:
            msg.angular_velocity.z = angle - self._rate_bias
            msg.angular_velocity_covariance[8] = (self._sigma_omega*self._sample_period)**2
            if self._invert_rotation:
                msg.angular_velocity.z *= -1
        if self._gyro.current_mode == self._gyro.MODE_DTHETA:
            msg.angular_velocity = angle/self._sample_period - self._rate_bias
            msg.angular_velocity_covariance[8] = (self._sigma_omega*self._sample_period)**2
            if self._invert_rotation:
                msg.angular_velocity.z *= -1
        if self._gyro.current_mode == self._gyro.MODE_INTEGRATED:
            dt = (now - self._bias_measurement_time).to_sec()
            corrected_angle = angle - self._rate_bias*dt
            msg.orientation.w = cos(corrected_angle/2.0)
            msg.orientation.z = sin(corrected_angle/2.0)
            if self._invert_rotation:
                msg.orientation.z *= -1
            msg.orientation_covariance[8] = self._sigma_theta*self._sigma_theta

        self._pub.publish(msg)

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import orientation_covariance [as 别名]
	def unpackIMU(self, data):
		imu_msg = Imu()
		#Unpack Header
		imu_msg.header = self.unpackIMUHeader(data[0:19])
		#Unpack Orientation Message
		quat = self.bytestr_to_array(data[19:19 + (4*8)])
		imu_msg.orientation.x = quat[0]
		imu_msg.orientation.y = quat[1]
		imu_msg.orientation.z = quat[2]
		imu_msg.orientation.w = quat[3]
		#Unpack Orientation Covariance
		imu_msg.orientation_covariance = list(self.bytestr_to_array(data[55:(55 + (9*8))]))
		#Unpack Angular Velocity
		ang = self.bytestr_to_array(data[127: 127 + (3*8)])
		imu_msg.angular_velocity.x = ang[0]
		imu_msg.angular_velocity.y = ang[1]
		imu_msg.angular_velocity.z = ang[2]
		#Unpack Angular Velocity Covariance
		imu_msg.angular_velocity_covariance = list(self.bytestr_to_array(data[155:(155 + (9*8))]))
		#Unpack Linear Acceleration
		lin = self.bytestr_to_array(data[227: 227 + (3*8)])
		imu_msg.linear_acceleration.x = lin[0]
		imu_msg.linear_acceleration.y = lin[1]
		imu_msg.linear_acceleration.z = lin[2]
		#Unpack Linear Acceleration Covariance
		imu_msg.linear_acceleration_covariance = list(self.bytestr_to_array(data[255:(255 + (9*8))]))
		return imu_msg	

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import orientation_covariance [as 别名]
def fake_imu():
    pub = rospy.Publisher('IMU_ned', Imu, queue_size=10)
    rospy.init_node('test_convertimu', anonymous=True)
    rate = rospy.Rate(0.5) # 10hz
    
    while not rospy.is_shutdown():
        imu_message=Imu()

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

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

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

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

        pub.publish(imu_message)
        rate.sleep()

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import orientation_covariance [as 别名]
def publish_imu_data():
    global imu, imu_publisher

    imu_data = Imu()

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

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

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

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

    imu_publisher.publish(imu_data)

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import orientation_covariance [as 别名]
    def Publish(self):
        imu_data = self._hal_proxy.GetImu()

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

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

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

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

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

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

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

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

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import orientation_covariance [as 别名]
    def spin(self):
        self.prev_time = rospy.Time.now()

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

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

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

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

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

            #nums = str.split()

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

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

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

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

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

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

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

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

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import orientation_covariance [as 别名]
	def _HandleReceivedLine(self,  line):
		self._Counter = self._Counter + 1
		self._SerialPublisher.publish(String(str(self._Counter) + ", in:  " + line))

		if(len(line) > 0):

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

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

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

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

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

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

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

					imu_msg.header = h

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

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

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

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

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

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import orientation_covariance [as 别名]
def Vn100Pub():
    pub = rospy.Publisher('IMUData', Imu, queue_size=1)
    pub2 = rospy.Publisher('IMUMag', MagneticField, queue_size=1)
    # Initialize the node and name it.
    rospy.init_node('IMUpub')
    vn = imuthread3.Imuthread(port=rospy.get_param("imu_port"), pause=0.0)

    vn.start()
    vn.set_data_freq50()
    vn.set_qmr_mode()
    #vn.set_data_freq10() #to see if this fixes my gps drop out problem
    rospy.sleep(3)
    msg = Imu()
    msg2 = MagneticField()
    count = 0
    while not rospy.is_shutdown():
        if len(vn.lastreadings)>0:
            if vn.lastreadings[0] =='VNQMR':
                msg.header.seq = count
                msg.header.stamp = rospy.Time.now()
                msg.header.frame_id = 'imu'
                msg.orientation.x = float(vn.lastreadings[1])
                msg.orientation.y = float(vn.lastreadings[2]) 
                msg.orientation.z = float(vn.lastreadings[3])
                msg.orientation.w = float(vn.lastreadings[4])
                msg.orientation_covariance = [0,0,0,0,0,0,0,0,0]
                msg.angular_velocity.x = float(vn.lastreadings[8])
                msg.angular_velocity.y = float(vn.lastreadings[9])
                msg.angular_velocity.z = float(vn.lastreadings[10])
                msg.angular_velocity_covariance = [0,0,0,0,0,0,0,0,0]
                msg.linear_acceleration.x = float(vn.lastreadings[11])
                msg.linear_acceleration.y = float(vn.lastreadings[12])
                msg.linear_acceleration.z = float(vn.lastreadings[13])
                msg.linear_acceleration_covariance = [0,0,0,0,0,0,0,0,0]
                msg2.header.seq = count
                msg2.header.stamp = rospy.Time.now()
                msg2.header.frame_id = 'imu'
                msg2.magnetic_field.x = float(vn.lastreadings[5])
                msg2.magnetic_field.x = float(vn.lastreadings[6])
                msg2.magnetic_field.x = float(vn.lastreadings[7])
                msg2.magnetic_field_covariance =  [0,0,0,0,0,0,0,0,0]

            #rospy.loginfo("vn100_pub " + str(msg.header.stamp) + " " + str(msg.orientation.x) + " "+ str(msg.orientation.y) + " "+ str(msg.orientation.z) + " "+ str(msg.orientation.w) + " ")
                current_pose_euler = tf.transformations.euler_from_quaternion([
                                    msg.orientation.x,
                                    msg.orientation.y,
                                    msg.orientation.z,
                                    msg.orientation.w
                                    ])                        
                pub.publish(msg)
                pub2.publish(msg2)
                count += 1
        #rospy.sleep(1.0/100.0)            
    vn.kill = True

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import orientation_covariance [as 别名]
def talker():
	global oldTime
	pubString = rospy.Publisher('sensor/string', String, queue_size=1000)
	pubIMU = rospy.Publisher('imuBoat', Imu, queue_size=1000)
	rospy.init_node('sensornimureader', anonymous=True)
	rate = rospy.Rate(100) # 100hz
	while not rospy.is_shutdown():
		# sample data
		currentTime = rospy.Time.now()
		timeCheck = currentTime.to_sec() - oldTime.to_sec()
		print timeCheck
		if (timeCheck) > 2.0:
			data = 'Z1.341725,103.965008,1.5100,0.0000'
			oldTime = currentTime;
		else: data = 'Y0.0000,0.0730,255.4516,1.5100,0.0000,0.0000,0.000,0.000,0.000'
		# data = ser.readline()
		pubString.publish(data)
		if data[0] == 'Y':
			data = data.replace("Y","").replace("\n","").replace("\r","")
			data_list = data.split(',')
			if len(data_list) == 9:
				try:
					##data_list structure: accel, magni, gyro
					float_list = [float(i) for i in data_list]
					imuData = Imu()
					imuData.header.frame_id = "base_link"
					imuData.header.stamp = rospy.Time.now()
					##data form in yaw, pitch, roll
					quat = tf.transformations.quaternion_from_euler(float_list[3], float_list[4], float_list[5], 'rzyx')
					imuData.orientation.x = quat[0]
					imuData.orientation.y = quat[1]
					imuData.orientation.z = quat[2]
					imuData.orientation.w = quat[3]
					imuData.angular_velocity.x = math.radians(float_list[6]*gyro_scale)
					imuData.angular_velocity.y = math.radians(-float_list[7]*gyro_scale)
					imuData.angular_velocity.z = math.radians(-float_list[8]*gyro_scale)
					imuData.linear_acceleration.x = float_list[0]*accel_scale
					imuData.linear_acceleration.y = -float_list[1]*accel_scale
					imuData.linear_acceleration.z = -float_list[2]*accel_scale
					imuData.orientation_covariance = [1.5838e-6, 0, 0, 0, 1.49402e-6, 0, 0, 0, 1.88934e-6]
					imuData.angular_velocity_covariance = [7.84113e-7, 0, 0, 0, 5.89609e-7, 0, 0, 0, 6.20293e-7]
					imuData.linear_acceleration_covariance = [9.8492e-4, 0, 0, 0, 7.10809e-4, 0, 0, 0, 1.42516e-3]
					pubIMU.publish(imuData)
					log = "IMU Data: %f %f %f %f %f %f %f %f %f Publish at Time: %s" \
					% (imuData.linear_acceleration.x, imuData.linear_acceleration.y, imuData.linear_acceleration.z,
						float_list[3], float_list[4], float_list[5],
						imuData.angular_velocity.x, imuData.angular_velocity.y, imuData.angular_velocity.z, rospy.get_time())
				except: log = "IMU Data Error! Data :  %s" % data
			else: log = "Data Error! Data :  %s" % data
			rospy.loginfo(log)
		rate.sleep()

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import orientation_covariance [as 别名]
    def convert_to_baselink(self, imu_msg, imu_model, transform):
        # convert imu_msg from its frame to baselink frame
        
        # Assumption! TODO: I'm going to assume that the axis are aligned between frames to start 
        # come back to this later, and rotate to align axis first
        #   proposed path:
        #       -> rorate-transform data (orientation, angular velocity, linear acceleration) to align with base_link
        #       -> run the same code below
        '''
        [sensor_msgs/Imu]:
        std_msgs/Header header - DONE
          uint32 seq
          time stamp
          string frame_id
        geometry_msgs/Quaternion orientation
          float64 x
          float64 y
          float64 z
          float64 w
        float64[9] orientation_covariance
        geometry_msgs/Vector3 angular_velocity
          float64 x
          float64 y
          float64 z
        float64[9] angular_velocity_covariance
        geometry_msgs/Vector3 linear_acceleration - DONE
          float64 x
          float64 y
          float64 z
        float64[9] linear_acceleration_covariance - DONE
        '''
        new_msg = Imu()

        # Header
        new_msg.header = imu_msg.header
        new_msg.header.frame_id = '/base_link'
        
        # Orientation (same based on Assumption! above)
        new_msg.orientation = imu_msg.orientation
        #   including covariance, because same. will likely drop for rotation
        new_msg.orientation_covariance = imu_msg.orientation_covariance

        # Angular Velocity (same based on Assumption! above)
        new_msg.angular_velocity = imu_msg.angular_velocity
        #   including covariance, because same. will likely drop for rotation
        new_msg.angular_velocity_covariance = imu_msg.angular_velocity_covariance

        # Linear Acceleration (not the same, even with assumption)
        new_msg.linear_acceleration = self.solid_body_translate_lin_acc(imu_msg.linear_acceleration, imu_model, transform)

        return new_msg

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import orientation_covariance [as 别名]
	def _HandleReceivedLine(self,  line):
		self._Counter = self._Counter + 1
		self._SerialPublisher.publish(String(str(self._Counter) + ", in:  " + line))
		if(len(line) > 0):
			lineParts = line.split('\t')
			try:
				if(lineParts[0] == 'e'):
					self._left_encoder_value = long(lineParts[1])
					self._right_encoder_value = long(lineParts[2])
					self._Left_Encoder.publish(self._left_encoder_value)
					self._Right_Encoder.publish(self._right_encoder_value)
				#if(lineParts[0] == 'b'):
					#self._battery_value = float(lineParts[1])
					#self._Battery_Level.publish(self._battery_value)
				if(lineParts[0] == 'u'):
					self._ultrasonic_value = float(lineParts[1])
					self._Ultrasonic_Value.publish(self._ultrasonic_value)
				if(lineParts[0] == 'i'):

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

					self._qx_.publish(self._qx)
					self._qy_.publish(self._qy)
					self._qz_.publish(self._qz)
					self._qw_.publish(self._qw)

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

					imu_msg.header = h

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


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

					self.imu_pub.publish(imu_msg)
			except:
				rospy.logwarn("Error in Sensor values")
				rospy.logwarn(lineParts)
				pass

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import orientation_covariance [as 别名]
	def convertData(self):

		if self.msgid < self.msgs_len:

			msg = Imu()

			msg.header.seq = self.timestamp_ms[self.msgid][0]
			msg.header.stamp = rospy.Time.from_sec(self.stamp)

			# TODO:
			# SCALED_IMU.msg:int16
			# HIGHRES_IMU.msg:float32
			# RAW_IMU.msg:int16 
			msg.angular_velocity.x = self.getMsgValue("GyrX")
			msg.angular_velocity.y = self.getMsgValue("GyrY")
			msg.angular_velocity.z = self.getMsgValue("GyrZ")
			msg.linear_acceleration.x = self.getMsgValue("AccX")
			msg.linear_acceleration.y = self.getMsgValue("AccY")
			msg.linear_acceleration.z = self.getMsgValue("AccZ")
			# TODO: This is not enough for ROS IMU msg, add cov matrices and calculate orientation

			# simple complementary filter for orientation
			GYROSCOPE_SENSITIVITY = 1
			# dt = self.last_stamp - self.stamp
			#pitch += (msg.angular_velocity.x / GYROSCOPE_SENSITIVITY) * dt # Angle around the X-axis
    		#roll -= (msg.angular_velocity.y / GYROSCOPE_SENSITIVITY) * dt # Angle around the Y-axis

			# Turning around the X axis results in a vector on the Y-axis
			#pitchAcc = atan2(msg.linear_acceleration.x, ( sqrt(pow(msg.linear_acceleration.y,2.0) + pow(msg.linear_acceleration.z,2.0)) )) * 180.0 / M_PI
			#pitch = pitch * 0.98 + pitchAcc * 0.02

			# Turning around the Y axis results in a vector on the X-axis
			#rollAcc = atan2(msg.linear_acceleration.y, (sqrt(pow(msg.linear_acceleration.x,2.0) + pow(msg.linear_acceleration.z,2.0)) )) * 180.0 / M_PI
			#roll = roll * 0.98 + rollAcc * 0.02

			#yaw =  (msg.angular_velocity.z / GYROSCOPE_SENSITIVITY) * dt

			#orientation.setRPY(roll, pitch, yaw)
			#tf::quaternionTFToMsg(orientation, orientation_msg)

			#msg.header.frame_id = frame_id_imu_link;
			#imu_transform.setRotation(orientation);
			msg.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 1]
			msg.orientation_covariance = [0.001, 0, 0, 0, 0.001, 0, 0, 0, 0.1]
			#self.orientation += imu.angular_velocity.z * (imu.header.stamp - self.prev_time).to_sec()
			#self.prev_time = imu.header.stamp
			#(imu.orientation.x, imu.orientation.y, imu.orientation.z, imu.orientation.w) = Rotation.RotZ(self.orientation).GetQuaternion()
			self.bag.write(self.topic, msg, msg.header.stamp)
			self.msgid = self.msgid + 1

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import orientation_covariance [as 别名]
def talker():
	pub = rospy.Publisher('IMUBoat', Imu, queue_size=1000)
	rospy.init_node('callibIMU', anonymous=True)
 	rate = rospy.Rate(10) # 10hz
	while not rospy.is_shutdown():
		data = ser.readline()
		# print data
		# print len(data)
		if data[0] == 'Y' and len(data) >= 70 and len(data) <= 85:
			data = data.replace("Y","").replace("\n","").replace("\r","")
			data_list = data.split(',')
			if len(data_list) == 9:
				try:
					##data_list structure: accel, magni, gyro
					float_list = [float(i) for i in data_list]
					imuData = Imu()
					imuData.header.frame_id = "imu"
					imuData.header.stamp = rospy.Time.now()
					quat = tf.transformations.quaternion_from_euler(float_list[3], float_list[4], float_list[5], 'rzyx')
					imuData.orientation.x = quat[0]
					imuData.orientation.y = quat[1]
					imuData.orientation.z = quat[2]
					imuData.orientation.w = quat[3]
					imuData.angular_velocity.x = math.radians(float_list[6]*gyro_scale)
					imuData.angular_velocity.y = math.radians(-float_list[7]*gyro_scale)
					imuData.angular_velocity.z = math.radians(-float_list[8]*gyro_scale)
					imuData.linear_acceleration.x = float_list[0]*accel_scale
					imuData.linear_acceleration.y = -float_list[1]*accel_scale
					imuData.linear_acceleration.z = -float_list[2]*accel_scale
					imuData.orientation_covariance = []
					imuData.angular_velocity_covariance = []
					imuData.linear_acceleration_covariance = []
					pub.publish(imuData)
					# log = "Data: %f %f %f %f %f %f %f %f %f Publish at Time: %s" \
					# % (float_list[0], float_list[1], float_list[2], float_list[3], 
			 	# 	float_list[4], float_list[5], float_list[6], float_list[7],
					# float_list[8], rospy.get_time())
					print "%f %f %f %f %f %f %f %f %f" % (
						imuData.linear_acceleration.x, imuData.linear_acceleration.y, imuData.linear_acceleration.z,
						float_list[3], float_list[4], float_list[5],
						imuData.angular_velocity.x, imuData.angular_velocity.y, imuData.angular_velocity.z)
				except: continue #log = "Data Error! Data :  %s" % data
			else: continue #log = "Data Error! Data :  %s" % data
			#rospy.loginfo(log)
		else:
			continue
			#log = "Non Data Serial Message: %s" % data
			#rospy.loginfo(log)
		rate.sleep()