Python Imu.linear_acceleration_covariance方法代码示例

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import linear_acceleration_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 linear_acceleration_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 linear_acceleration_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 linear_acceleration_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 linear_acceleration_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 linear_acceleration_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 linear_acceleration_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 linear_acceleration_covariance [as 别名]
def GetImuFromLine(line):
	(ts, ax, ay, az, gx, gy, gz) = [t(s) for t,s in zip((int,int, int, int, int, int, int),line.split())]
	imu = Imu()
	ts = float(ts)/1000000.0
	ImuStamp = rospy.rostime.Time.from_sec(ts)
	imu.angular_velocity = create_vector3(gyro_raw_to_rads(gx), gyro_raw_to_rads(gy), gyro_raw_to_rads(gz))
	imu.angular_velocity_covariance = create_diag_mat(gyro_raw_to_rads(1.0), gyro_raw_to_rads(1.0), gyro_raw_to_rads(1.0));
	imu.linear_acceleration = create_vector3(acc_raw_to_ms(ax), acc_raw_to_ms(ay), acc_raw_to_ms(az))
	imu.linear_acceleration_covariance = create_diag_mat(acc_raw_to_ms(1.0), acc_raw_to_ms(1.0), acc_raw_to_ms(1.0))
	return (ts, ImuStamp, imu)

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

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

        while len(line) > 0:

            lineParts = line.split("\t")
            try:

                if lineParts[0] == "e":

                    self.angle_z = float(lineParts[1])

                    self._qx = 0.0
                    self._qy = 0.0
                    self._qz = math.sin(math.pi * self.angle_z / 360.0)
                    self._qw = math.cos(math.pi * self.angle_z / 360.0)

                    #######################################################################################################################
                    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.0,) * 9
                    imu_msg.angular_velocity_covariance = (-1.0,) * 9
                    imu_msg.linear_acceleration_covariance = (-1.0,) * 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 linear_acceleration_covariance [as 别名]
    def msg_template(self):
        msg = Imu()
        msg.header.frame_id = "robocape"

        msg.linear_acceleration_covariance = (0.1, 0.0, 0.0,
                                              0.0, 0.1, 0.0,
                                              0.0, 0.0, 0.1)

        msg.angular_velocity_covariance = (1.0, 0.0, 0.0,
                                           0.0, 1.0, 0.0,
                                           0.0, 0.0, 1.0)

        msg.orientation_covariance = (10.0, 0.0,  0.0,
                                      0.0,  10.0, 0.0,
                                      0.0,  0.0,  10.0)

        return msg

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import linear_acceleration_covariance [as 别名]
        def Handle_Received_Line(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_val = long(lineParts[1])
                        self._right_encoder_val = long(lineParts[2])

                        self.left_encoder_pub.publish(self._left_encoder_val)
                        self.right_encoder_pub.publish(self._right_encoder_val)
                    if(lineParts[0] == 'u'):
                        self._ultrasonic_val = float(lineParts[1])
                        self.ultrasonic_pub.publish(self._ultrasonic_val)
                    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_pub.publish(self._qx)
                        self._qy_pub.publish(self._qy)
                        self._qz_pub.publish(self._qz)
                        self._qw_pub.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 the sensor values")
                    rospy.logwarn(lineParts)
                    pass