Python Imu.angular_velocity_covariance方法代码示例

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import angular_velocity_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 angular_velocity_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 angular_velocity_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 angular_velocity_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 angular_velocity_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 angular_velocity_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 angular_velocity_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 angular_velocity_covariance [as 别名]
 def run(self):
     """Loop that obtains the latest wiimote state, publishes the IMU data, and sleeps.
     
     The IMU message, if fully filled in, contains information on orientation,
     acceleration (in m/s^2), and angular rate (in radians/sec). For each of
     these quantities, the IMU message format also wants the corresponding
     covariance matrix.
     
     Wiimote only gives us acceleration and angular rate. So we ensure that the orientation
     data entry is marked invalid. We do this by setting the first
     entry of its associated covariance matrix to -1. The covariance
     matrices are the 3x3 matrix with the axes' variance in the 
     diagonal. We obtain the variance from the Wiimote instance.  
     """
     
     rospy.loginfo("Wiimote IMU publisher starting (topic /imu/data).")
     self.threadName = "IMU topic Publisher"
     try:
         while not rospy.is_shutdown():
             (canonicalAccel, canonicalNunchukAccel, canonicalAngleRate) = self.obtainWiimoteData()
             
             msg = Imu(header=None,
                       orientation=None,                                         # will default to [0.,0.,0.,0],
                       orientation_covariance=[-1.,0.,0.,0.,0.,0.,0.,0.,0.],     # -1 indicates that orientation is unknown
                       angular_velocity=None,
                       angular_velocity_covariance=self.angular_velocity_covariance,
                       linear_acceleration=None,
                       linear_acceleration_covariance=self.linear_acceleration_covariance)
                       
                 
             # If a gyro is plugged into the Wiimote, then note the 
             # angular velocity in the message, else indicate with
             # the special gyroAbsence_covariance matrix that angular
             # velocity is unavailable:      
             if self.wiistate.motionPlusPresent:
                 msg.angular_velocity.x = canonicalAngleRate[PHI]
                 msg.angular_velocity.y = canonicalAngleRate[THETA]
                 msg.angular_velocity.z = canonicalAngleRate[PSI]
             else:
                 msg.angular_velocity_covariance = self.gyroAbsence_covariance
             
             msg.linear_acceleration.x = canonicalAccel[X]
             msg.linear_acceleration.y = canonicalAccel[Y]
             msg.linear_acceleration.z = canonicalAccel[Z]
             
             measureTime = self.wiistate.time
             timeSecs = int(measureTime)
             timeNSecs = int(abs(timeSecs - measureTime) * 10**9)
             msg.header.stamp.secs = timeSecs
             msg.header.stamp.nsecs = timeNSecs
             
             self.pub.publish(msg)
             
             rospy.logdebug("IMU state:")
             rospy.logdebug("    IMU accel: " + str(canonicalAccel) + "\n    IMU angular rate: " + str(canonicalAngleRate))
             rospy.sleep(self.sleepDuration)
     except rospy.ROSInterruptException:
         rospy.loginfo("Shutdown request. Shutting down Imu sender.")
         exit(0)

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

    rospy.init_node(NODE_NAME)

    params = rospy.get_param("/imu".format(NODE_NAME),
    {
        'gyro': {
            'zero':[-129.91, 63.81, -102.36], 
            'covariance':[0,0,0,
                          0,3.6935414044e-06,0,
                          0,0,0] 
        },
        'angle':{
            'zero':[0,-0.118467263978,0],
            'covariance':[0,0,0,
                          0,3.57350008404e-05,0,
                          0,0,0]
        }
    })
    
    rospy.set_param("/imu", params)

    gyro_zeros = np.array(params['gyro']['zero'])
    gyro_y_variance = params['gyro']['covariance'][4]
    angle_y_zero = params['angle']['zero'][Y]
    angle_y_variance = params['angle']['covariance'][4]
    
    rate = rospy.Rate(SAMPLE_FREQ_HZ)
    publisher = rospy.Publisher(IMU_TOPIC, Imu, queue_size=1, tcp_nodelay=True)
    raw_publisher = rospy.Publisher(IMU_RAW_TOPIC, Imu, queue_size=1, tcp_nodelay=True)

    rospy.loginfo("Starting {}, publishing at {} hz on {}".format(NODE_NAME, SAMPLE_FREQ_HZ, IMU_TOPIC))
    state = Sample()
    work = Sample()
    sensor_reading = Sample()
    raw_msg = Imu()
    imu_msg = Imu()
    imu_msg.angular_velocity_covariance = params['gyro']['covariance']
#    imu_msg.linear_acceleration_covariance = params['accel']['covariance']

    while not rospy.is_shutdown():

        read_sensor(sensor_reading)
        if PUBLISH_RAW:
            populate_message(raw_msg, sensor_reading)
            raw_publisher.publish(raw_msg)

        process_reading(sensor_reading, work, state, gyro_zeros, gyro_y_variance, angle_y_zero, angle_y_variance)

        populate_message(imu_msg, state)
        publisher.publish(imu_msg)

        rate.sleep()

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