Python Imu.linear_acceleration方法代码示例

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import linear_acceleration [as 别名]
    def read_from_imu(self):
        while True:
            try:
                imu_msg = Imu()
                imu_msg.header.stamp = rospy.Time.now()
                imu_msg.header.frame_id = 'imu'

                orientation = self.bno.read_quaternion()
                linear_acceleration = self.bno.read_linear_acceleration()
                angular_velocity = self.bno.read_gyroscope()

                imu_msg.orientation_covariance[0] = -1
                imu_msg.linear_acceleration_covariance[0] = -1
                imu_msg.angular_velocity_covariance[0] = -1

                imu_msg.orientation = Quaternion(orientation[1],
                                                 orientation[2],
                                                 orientation[3],
                                                 orientation[0])
                imu_msg.linear_acceleration = Vector3(linear_acceleration[0],
                                                      linear_acceleration[1],
                                                      linear_acceleration[2])
                imu_msg.angular_velocity = Vector3(np.deg2rad(angular_velocity[0]),
                                                   np.deg2rad(angular_velocity[1]),
                                                   np.deg2rad(angular_velocity[2]))
                return imu_msg
            except IOError:
                pass

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import linear_acceleration [as 别名]
    def publish(self, data):
        q = data.getOrientation()
        roll, pitch, yaw = euler_from_quaternion([q.w, q.x, q.y, q.z])

        # print "Before ", "Yaw: " + str(yaw * 180 / pi), "Roll: " + str(roll * 180 / pi), "pitch: " + str(pitch * 180 / pi), "\n\n"

        array = quaternion_from_euler(roll, pitch, yaw + (self._angle * pi / 180))

        res = Quaternion()
        res.w = array[0]
        res.x = array[1]
        res.y = array[2]
        res.z = array[3]

        roll, pitch, yaw = euler_from_quaternion([q.w, q.x, q.y, q.z])

        # print "after ", "Yaw: " + str(yaw * 180 / pi), "Roll: " + str(roll * 180 / pi), "pitch: " + str(pitch * 180 / pi), "\n\n"

        msg = Imu()
        msg.header.frame_id = self._frameId
        msg.header.stamp = rospy.get_rostime()
        msg.orientation = res
        msg.linear_acceleration = data.getAcceleration()
        msg.angular_velocity = data.getVelocity()

        magMsg = MagneticField()
        magMsg.header.frame_id = self._frameId
        magMsg.header.stamp = rospy.get_rostime()
        magMsg.magnetic_field = data.getMagnetometer()

        self._pub.publish(msg)
        self._pubMag.publish(magMsg)

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import linear_acceleration [as 别名]
    def publish(self, data):
        if not self._calib and data.getImuMsgId() == PUB_ID:
            q = data.getOrientation()
            roll, pitch, yaw = euler_from_quaternion([q.w, q.x, q.y, q.z])
            array = quaternion_from_euler(roll, pitch, yaw + (self._angle * pi / 180))

            res = Quaternion()
            res.w = array[0]
            res.x = array[1]
            res.y = array[2]
            res.z = array[3]

            msg = Imu()
            msg.header.frame_id = self._frameId
            msg.header.stamp = rospy.get_rostime()
            msg.orientation = res
            msg.linear_acceleration = data.getAcceleration()
            msg.angular_velocity = data.getVelocity()

            magMsg = MagneticField()
            magMsg.header.frame_id = self._frameId
            magMsg.header.stamp = rospy.get_rostime()
            magMsg.magnetic_field = data.getMagnetometer()

            self._pub.publish(msg)
            self._pubMag.publish(magMsg)

        elif data.getImuMsgId() == CALIB_ID:
            x, y, z = data.getValues()
            msg = imuCalib()

            if x > self._xMax:
                self._xMax = x
            if x < self._xMin:
                self._xMin = x

            if y > self._yMax:
                self._yMax = y
            if y < self._yMin:
                self._yMin = y

            if z > self._zMax:
                self._zMax = z
            if z < self._zMin:
                self._zMin = z


            msg.x.data = x
            msg.x.max = self._xMax
            msg.x.min = self._xMin

            msg.y.data = y
            msg.y.max = self._yMax
            msg.y.min = self._yMin

            msg.z.data = z
            msg.z.max = self._zMax
            msg.z.min = self._zMin

            self._pubCalib.publish(msg)

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import linear_acceleration [as 别名]
    def parse_imu(self, data):
        '''
        Given data from jaguar imu, parse and return a standard IMU message.
        Return None when given bad data, or no complete message was found in data.
        '''
         # Use regular expressions to extract complete message
        # message format: $val,val,val,val,val,val,val,val,val,val#\n
        hit = re.search(r"\$-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*,-?[0-9]*#", data)
        if not hit:
            # if there are no hits, return None
            return None
        else:
            imu_data = hit.group(0)
            try:
                # Try to format the data
                imu_data = imu_data[1:-1].split(",")
                #Format (from drrobot docs): seq, accelx, accely, accelz, gyroY, gyroZ, gyroX, magnetonX, magnetonY, magnetonZ
                seq = int(imu_data[0])

                accelx = float(imu_data[1])
                accely = float(imu_data[2])
                accelz = float(imu_data[3])

                gyroy = float(imu_data[4])
                gyroz = float(imu_data[5])
                gyrox = float(imu_data[6])

                magnetonx = float(imu_data[7])
                magnetony = float(imu_data[8])
                magnetonz = float(imu_data[9])

                rot_mat = [ [float(imu_data[10]), float(imu_data[11]), float(imu_data[12])],
                            [float(imu_data[13]), float(imu_data[14]), float(imu_data[15])],
                            [float(imu_data[16]), float(imu_data[17]), float(imu_data[18])] ]

            except:
                # bad data in match, pass
                return None
            else:
                # data formatted fine, build message and publish

                # if we didn't get a magnetometer update, set to current reading
                if magnetonz == 0:
                    magnetonx = self.current_mag[0]
                    magnetony = self.current_mag[1]
                    magnetonz = self.current_mag[2]
                # otherwise, update current magnetometer
                else:
                    self.current_mag = [magnetonx, magnetony, magnetonz]

                # Calculate quaternion from given rotation matrix
                quat = rot_mat_to_quat(rot_mat);
                # Build message 
                msg = Imu()
                msg.header = Header(stamp = rospy.Time.now(), frame_id = "imu")
                msg.linear_acceleration = Vector3(accelx, accely, accelz)
                msg.angular_velocity = Vector3(gyrox, gyroy, gyroz)
                if quat != None: msg.orientation = Quaternion(quat[0], quat[1], quat[2], quat[3])
                return msg

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import linear_acceleration [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 [as 别名]
def publish_imu_data (imu_data) :
    mag_msg = Vector3Stamped()
    mag_msg.header.stamp = rospy.Time.now()
    mag_msg.vector              = Vector3(float(imu_data[0]),float(imu_data[1]),float(imu_data[2]))
    mag_pub.publish(mag_msg)

    imu_msg = Imu()
    imu_msg.header.stamp = rospy.Time.now()
    imu_msg.linear_acceleration = Vector3(float(imu_data[3]),float(imu_data[4]),float(imu_data[5]))
    imu_msg.angular_velocity    = Vector3(float(imu_data[6]),float(imu_data[7]),float(imu_data[8]))
    imu_pub.publish(imu_msg)

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import linear_acceleration [as 别名]
def callback(raw_data, pub):
    imu = Imu()
    imu.header = raw_data.header

    # set imu.orientation quaternion
    # Btw, robot_pose_ekf expects the Roll and Pitch angles to be
    # absolute, but the Yaw angle to be relative. Compute the relative
    # angle from the currently reported and previously reported absolute
    # angle
    r = raw_data.roll
    p = raw_data.pitch
    y = raw_data.yaw - callback.old_yaw
    callback.old_yaw = raw_data.yaw

    quat = quaternion_from_euler(r,p,y,"sxyz")

    imu.orientation.x = quat[0]
    imu.orientation.y = quat[1]
    imu.orientation.z = quat[2]
    imu.orientation.w = quat[3]

    #TODO: set imu.orientation_covariance
    #row major about x, y, z
    # According to OS5000 datasheet, accuracy
    # depends on tilt:
    # 0.5 deg RMS level heading,
    # 1 deg typical RMS accuracy < +-30deg tilt,
    # 1.5deg < +-60deg tilt
    # Roll and Pitch:
    # Typical 1deg accuracy for tilt < +-30 deg
    # Assume tilt < +- 30 deg, set all standard
    # deviations to 1 degree
    std_dev = from_degrees(1)
    imu.orientation_covariance[0] = std_dev**2
    imu.orientation_covariance[4] = std_dev**2
    # standard deviation on yaw is doubled since
    # it's computed as a difference of two measurements,
    # each with a std_dev of 1 degree
    imu.orientation_covariance[8] = (2*std_dev)**2

    # no angular velocity data is available, so set
    # element 0 of covariance matrix to -1
    # alternatively, set the covariance really high
    imu.angular_velocity = Vector3(0,0,0)
    imu.angular_velocity_covariance[0] = -1

    # Set linear acceleration
    imu.linear_acceleration = raw_data.acceleration
    # TODO: Set linear acceleration covariance
    imu.linear_acceleration_covariance[0] = -1

    pub.publish(imu)

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import linear_acceleration [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 linear_acceleration [as 别名]
def talker():
	#Create publisher ('Topic name', msg type)
	pub = rospy.Publisher('IMU', Imu)
	#Tells rospy name of the node to allow communication to ROS master
	rospy.init_node('IMUtalker')

	while not rospy.is_shutdown():
		#Grab relevant information from parse()
		try:
			(accel,gyro,magne) = parse()
		#If data is bad, uses presvious data
		except: continue
		
		#Define IMUmsg to be of the Imu msg type
		IMUmsg = Imu()
		
		#Set header time stamp
		IMUmsg.header.stamp = rospy.Time.now()
		
		#Set orientation parameters
		IMUmsg.orientation = Quaternion()
		IMUmsg.orientation.x = magne[0]
		IMUmsg.orientation.y = magne[1]
		IMUmsg.orientation.z = magne[2]
		IMUmsg.orientation_covariance = (0, 0, 0, 0, 0, 0, 0, 0, 0)
		
		#Set angular velocity parameters
		IMUmsg.angular_velocity = Vector3()
		IMUmsg.angular_velocity.x = gyro[0]
		IMUmsg.angular_velocity.y = gyro[1]
		IMUmsg.angular_velocity.z = gyro[2]
		IMUmsg.angular_velocity_covariance = (0, 0, 0, 0, 0, 0, 0, 0, 0)
		
		#Set linear acceleration parameters
		IMUmsg.linear_acceleration = Vector3()
		IMUmsg.linear_acceleration.x = accel[0]
		IMUmsg.linear_acceleration.y = accel[1]
		IMUmsg.linear_acceleration.z = accel[2]
		IMUmsg.linear_acceleration_covariance = (0, 0, 0, 0, 0, 0, 0, 0, 0)
		
		#Publish the data
		pub.publish(IMUmsg)

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