本文整理汇总了Python中sensor_msgs.msg.Imu.orientation_covariance[4]方法的典型用法代码示例。如果您正苦于以下问题：Python Imu.orientation_covariance[4]方法的具体用法？Python Imu.orientation_covariance[4]怎么用？Python Imu.orientation_covariance[4]使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类sensor_msgs.msg.Imu的用法示例。
在下文中一共展示了Imu.orientation_covariance[4]方法的3个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: callback
# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import orientation_covariance[4] [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)示例2: imuCallback
# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import orientation_covariance[4] [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)示例3: pubImu
# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import orientation_covariance[4] [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)