本文整理汇总了Python中sensor_msgs.msg.Imu.linear_acceleration_covariance[4]方法的典型用法代码示例。如果您正苦于以下问题：Python Imu.linear_acceleration_covariance[4]方法的具体用法？Python Imu.linear_acceleration_covariance[4]怎么用？Python Imu.linear_acceleration_covariance[4]使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类sensor_msgs.msg.Imu的用法示例。
在下文中一共展示了Imu.linear_acceleration_covariance[4]方法的4个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: imuCallback
# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import linear_acceleration_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)示例2: talker
# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import linear_acceleration_covariance[4] [as 别名]
#.........这里部分代码省略.........
m9a, m9g, m9m = imu.getMotion9()
#----------update IMU
ax = m9a[0]
ay = m9a[1]
az = m9a[2]
gx = m9g[0]
gy = m9g[1]
gz = m9g[2]
q0 = 0.0 #W
q1 = 0.0 #X
q2 = 0.0 #Y
q3 = 0.0 #Z
'''
#----------Calculate delta time
t = time()
currenttime = 0
previoustime = currenttime
currenttime = 1000000 * t + t / 1000000
dt = (currenttime - previoustime) / 1000000.0
if (dt < (1/1300.0)) :
time.sleep((1/1300.0 - dt) * 1000000)
t = time()
currenttime = 1000000 * t + t / 1000000
dt = (currenttime - previoustime) / 1000000.0
print "Delta time: d = %f" % dt
#Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if not ((ax == 0.0) and (ay == 0.0) and (az == 0.0)) :
#Normalise accelerometer measurement
recipNorm = (ax * ax + ay * ay + az * az)**-.5
ax *= recipNorm
ay *= recipNorm
az *= recipNorm
#Estimated direction of gravity and vector perpendicular to magnetic flux
halfvx = q1 * q3 - q0 * q2
halfvy = q0 * q1 + q2 * q3
halfvz = q0 * q0 - 0.5 + q3 * q3
#Error is sum of cross product between estimated and measured direction of gravity
halfex = (ay * halfvz - az * halfvy)
halfey = (az * halfvx - ax * halfvz)
halfez = (ax * halfvy - ay * halfvx)
#Compute and apply integral feedback (if enabled)
integralFBx += twoKi * halfex * dt;
integralFBy += twoKi * halfey * dt;
integralFBz += twoKi * halfez * dt;
gx += integralFBx
gy += integralFBy
gz += integralFBz
#Apply proportional feedback
gx += twoKp * halfex;
gy += twoKp * halfey;
gz += twoKp * halfez;
#Integrate rate of change of quaternion
gx *= (0.5 * dt)
gy *= (0.5 * dt)
gz *= (0.5 * dt)
qa = q0
qb = q1
qc = q2
q0 += (-qb * gx - qc * gy - q3 * gz)
q1 += (qa * gx + qc * gz - q3 * gy)
q2 += (qa * gy - qb * gz + q3 * gx)
q3 += (qa * gz + qb * gy - qc * gx)
#Normalise quaternion
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3)
q0 *= recipNorm
q1 *= recipNorm
q2 *= recipNorm
q3 *= recipNorm
'''
#Fill message
msg.orientation.x = q1
msg.orientation.y = q2
msg.orientation.z = q3
msg.orientation.w = q0
msg.orientation_covariance[0] = q1 * q1
msg.orientation_covariance[0] = q2 * q2
msg.orientation_covariance[0] = q3 * q3
msg.angular_velocity.x = m9g[0]
msg.angular_velocity.y = m9g[1]
msg.angular_velocity.z = m9g[2]
msg.angular_velocity_covariance[0] = m9g[0] * m9g[0]
msg.angular_velocity_covariance[4] = m9g[1] * m9g[1]
msg.angular_velocity_covariance[8] = m9g[2] * m9g[2]
msg.linear_acceleration.x = m9a[0]
msg.linear_acceleration.y = m9a[1]
msg.linear_acceleration.z = m9a[2]
msg.linear_acceleration_covariance[0] = m9a[0] * m9a[0]
msg.linear_acceleration_covariance[4] = m9a[1] * m9a[1]
msg.linear_acceleration_covariance[8] = m9a[2] * m9a[2]
pub.publish(msg)
rate.sleep()示例3: talker
# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import linear_acceleration_covariance[4] [as 别名]
def talker():
pub = rospy.Publisher('imu', Imu, queue_size=10)
rospy.init_node('ros_erle_imu', anonymous=True)
rate = rospy.Rate(10)
imu = MPU9250()
imu.initialize()
msg = Imu()
while not rospy.is_shutdown():
imu.read_acc()
m9a, m9g, m9m = imu.getMotion9()
msg.header.stamp = rospy.get_rostime()
#ORIENTATION calculation
'''You can just integrate your angular velocity to get angular
position (as Euler angles), convert the Euler angles to Quaternion,
then multiply the Quaternion to accumulate the orientation.
Suppose your input is given by a 3D vector of angular velocity:
omega = (alpha, beta, gamma), given by degrees per second.
To get the Euler angles, E, in degrees, multiply omega by the change
in time, which we can call dt '''
dt = 0.1 #10Hz in seconds
AngVelX = m9g[0]*57.29577951308*dt
AngVelY = m9g[1]*57.29577951308*dt
AngVelZ = m9g[2]*57.29577951308*dt
w_or = math.cos(AngVelX/2) * math.cos(AngVelY/2) * math.cos(AngVelZ/2) + math.sin(AngVelX/2) * math.sin(AngVelY/2) * math.sin(AngVelZ/2)
x_or = math.sin(AngVelX/2) * math.cos(AngVelY/2) * math.cos(AngVelZ/2) - math.cos(AngVelX/2) * math.sin(AngVelY/2) * math.sin(AngVelZ/2)
y_or = math.cos(AngVelX/2) * math.sin(AngVelY/2) * math.cos(AngVelZ/2) + math.sin(AngVelX/2) * math.cos(AngVelY/2) * math.sin(AngVelZ/2)
z_or = math.cos(AngVelX/2) * math.cos(AngVelY/2) * math.sin(AngVelZ/2) - math.sin(AngVelX/2) * math.sin(AngVelY/2) * math.cos(AngVelZ/2)
msg.orientation.x = x_or
msg.orientation.y = y_or
msg.orientation.z = z_or
msg.orientation.w = w_or
msg.orientation_covariance[0] = x_or * x_or
msg.orientation_covariance[0] = y_or * y_or
msg.orientation_covariance[0] = z_or * z_or
msg.angular_velocity.x = m9g[0]
msg.angular_velocity.y = m9g[1]
msg.angular_velocity.z = m9g[2]
msg.angular_velocity_covariance[0] = m9g[0] * m9g[0]
msg.angular_velocity_covariance[4] = m9g[1] * m9g[1]
msg.angular_velocity_covariance[8] = m9g[2] * m9g[2]
msg.linear_acceleration.x = m9a[0]
msg.linear_acceleration.y = m9a[1]
msg.linear_acceleration.z = m9a[2]
msg.linear_acceleration_covariance[0] = m9a[0] * m9a[0]
msg.linear_acceleration_covariance[4] = m9a[1] * m9a[1]
msg.linear_acceleration_covariance[8] = m9a[2] * m9a[2]
pub.publish(msg)
rate.sleep()示例4: publish
# 需要导入模块: from sensor_msgs.msg import Imu [as 别名]
# 或者: from sensor_msgs.msg.Imu import linear_acceleration_covariance[4] [as 别名]
def publish(self, etherbotix):
# Can't publish yet if don't know the version of our IMU
if etherbotix.version > 0 and etherbotix.getImuVersion() == 0:
return
# Need to read params first time we get version
if self.version == 0:
# Save IMU version so we only run this once
self.version = etherbotix.getImuVersion()
# Backwards compatability with old firmware
if etherbotix.version == 0:
self.version = 2
# L3GD20 (2000dps mode)
self.gyro_scale = 0.001221111
self.gyro_covariance = 0.004868938
# LSM303DLHC (2g/full scale mode)
self.accel_scale = 0.000598773
self.accel_covariance = 0.34644996
# LSM303DLHC (
self.mag_scale = 0.000909
# Set default params
if self.version == 2:
# L3GD20 (2000dps mode)
self.gyro_scale = 0.001221111
self.gyro_covariance = 0.004868938
# LSM303DLHC (8g/full scale mode)
self.accel_scale = 0.0023957
self.accel_covariance = 0.34644996
# LSM303DLHC (8.1 gauss mode)
self.mag_scale = 0.0043478
elif self.version == 3:
# L3GD20H (2000dps mode)
self.gyro_scale = 0.001221111
self.gyro_covariance = 0.004868938
# LSM303D (8g/full scale mode)
self.accel_scale = 0.0023964
self.accel_covariance = 0.34644996
# LSM303D (12 gauss mode)
self.mag_scale = 0.000479
elif self.version == 5:
# LSM6DS33 (2000dps mode)
self.gyro_scale = 0.001221111
self.gyro_covariance = 0.004868938
# LSM6DS33 (8g/full scale mode)
self.accel_scale = 0.0023964
self.accel_covariance = 0.34644996
# LIS3MDL (12 gauss mode)
self.mag_scale = 0.000438404
self.gyro_scale = rospy.get_param("~imu/gyro/scale", self.gyro_scale)
self.gyro_covariance = rospy.get_param("~imu/gyro/covariance", self.gyro_covariance)
self.accel_scale = rospy.get_param("~imu/accel/scale", self.accel_scale)
self.accel_covariance = rospy.get_param("~imu/accel/covariance", self.accel_covariance)
self.mag_scale = rospy.get_param("~imu/mag/scale", self.mag_scale)
msg = Imu()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = self.frame
# No known orientation
msg.orientation_covariance[0] = -1.0
msg.angular_velocity.x = etherbotix.gyro_x * self.gyro_scale
msg.angular_velocity.y = etherbotix.gyro_y * self.gyro_scale
msg.angular_velocity.z = etherbotix.gyro_z * self.gyro_scale
msg.angular_velocity_covariance[0] = self.gyro_covariance
msg.angular_velocity_covariance[4] = self.gyro_covariance
msg.angular_velocity_covariance[8] = self.gyro_covariance
msg.linear_acceleration.x = etherbotix.accel_x * self.accel_scale
msg.linear_acceleration.y = etherbotix.accel_y * self.accel_scale
msg.linear_acceleration.z = etherbotix.accel_z * self.accel_scale
msg.linear_acceleration_covariance[0] = self.accel_covariance
msg.linear_acceleration_covariance[4] = self.accel_covariance
msg.linear_acceleration_covariance[8] = self.accel_covariance
self._pub.publish(msg)
if self._pub_mag:
mag = Vector3Stamped()
mag.header = msg.header
mag.vector.x = etherbotix.mag_x * self.mag_scale
mag.vector.y = etherbotix.mag_y * self.mag_scale
mag.vector.z = etherbotix.mag_z * self.mag_scale
self._pub_mag.publish(mag)