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

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import orientation_covariance[8] [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[8] [as 别名]
    def send_message(self, event=None):
        if not self.valid or not self.connected:
            rospy.loginfo("%s: measurements not valid, not sending imu message!", self.name)
            return

        # NOTE:
        #   the axis convention is taken from the old driver implementation
        #   this should be checked again and explicitly stated in the documentation
        rotation = tft.quaternion_from_euler(0, 0, np.deg2rad(-self.yaw), axes="sxyz")

        # NOTE:
        # This is a message to hold data from an IMU (Inertial Measurement Unit)
        #
        # Accelerations should be in m/s^2 (not in g's), and rotational velocity should be in rad/sec
        #
        # If the covariance of the measurement is known, it should be filled in (if all you know is the
        # variance of each measurement, e.g. from the datasheet, just put those along the diagonal)
        # A covariance matrix of all zeros will be interpreted as "covariance unknown", and to use the
        # data a covariance will have to be assumed or gotten from some other source
        #
        # If you have no estimate for one of the data elements (e.g. your IMU doesn't produce an orientation
        # estimate), please set element 0 of the associated covariance matrix to -1
        # If you are interpreting this message, please check for a value of -1 in the first element of each
        # covariance matrix, and disregard the associated estimate.
        msg = Imu()
        msg.header.stamp = rospy.Time.now()
        msg.header.frame_id = self.frame_id
        msg.orientation.x = rotation[0]
        msg.orientation.y = rotation[1]
        msg.orientation.z = rotation[2]
        msg.orientation.w = rotation[3]

        # this is derived by looking at the datasheet under the bias offset factor
        # and it should include the drift of the gyro if the yaw is computed by integration
        msg.orientation_covariance[8] = np.deg2rad(self.yaw_std_z ** 2)

        # this is to notify that this imu is not measuring this data
        msg.linear_acceleration_covariance[0] = -1

        msg.angular_velocity.z = np.deg2rad(-self.yaw_dot)

        # this is derived by looking at the datasheet
        msg.angular_velocity_covariance[8] = np.deg2rad(KVH_SIGMA ** 2)

        self.pub_gyro.publish(msg)

        if self.mode == KVH_MODE_R:
            rospy.loginfo("%s: raw data: %+.5f -- yaw rate: %+.5f", self.name, self.data, self.yaw_dot)
        else:
            rospy.loginfo(
                "%s: raw data: %+.5f -- yaw rate: %+.5f -- yaw: %+.5f", self.name, self.data, self.yaw_dot, self.yaw
            )

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

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

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

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

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

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

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

    pub.publish(imu)

# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import orientation_covariance[8] [as 别名]
	def imuCallback(self,imuData):
		imu = Imu()
		imu.header.stamp = rospy.Time.now()
		imu.header.frame_id = "imu_frame"#"imu_frame"
		imu.angular_velocity.x = -(imuData.gyro[0] - GYRO_X_OFFSET)/self.GYRO_SENSITIVITY*pi/180
		imu.angular_velocity.y = -(imuData.gyro[1] - GYRO_Y_OFFSET)/self.GYRO_SENSITIVITY*pi/180
		imu.angular_velocity.z = -(imuData.gyro[2] - GYRO_Z_OFFSET)/self.GYRO_SENSITIVITY*pi/180
		
		imu.orientation_covariance[0]=0.001
		imu.orientation_covariance[4]=0.001
		imu.orientation_covariance[8]=0.001

		imu.angular_velocity_covariance[0]=0.001
		imu.angular_velocity_covariance[4]=0.001
		imu.angular_velocity_covariance[8]=0.001


		x=-imuData.accel[0]
		y=-imuData.accel[1]
		z=-imuData.accel[2]
		effectiveG = sqrt(x**2 + y**2 + z**2)
		x = float(x)/effectiveG*g
		y = float(y)/effectiveG*g
		z = float(z)/effectiveG*g
	
		
		imu.linear_acceleration.x = x
		imu.linear_acceleration.y = y
		imu.linear_acceleration.z = z
		
		imu.linear_acceleration_covariance[0]=0.001
		imu.linear_acceleration_covariance[4]=0.001
		imu.linear_acceleration_covariance[8]=0.001
		
		mag = Vector3Stamped()
		mag.header.stamp = imu.header.stamp
		mag.header.frame_id = "imu_frame"
		mag.vector.x = imuData.magneto[0] - MAGNETO_X_OFFSET
		mag.vector.y = imuData.magneto[1] - MAGNETO_Y_OFFSET
		mag.vector.z = imuData.magneto[2] - MAGNETO_Z_OFFSET
		
		self.pubImu.publish(imu)
		self.pubMag.publish(mag)

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