def proc_imu(quat1, acc, gyro):
# New info:
# https://github.com/thalmiclabs/myo-bluetooth/blob/master/myohw.h#L292-L295
# Scale values for unpacking IMU data
# define MYOHW_ORIENTATION_SCALE 16384.0f
# See myohw_imu_data_t::orientation
# define MYOHW_ACCELEROMETER_SCALE 2048.0f
# See myohw_imu_data_t::accelerometer
# define MYOHW_GYROSCOPE_SCALE 16.0f
# See myohw_imu_data_t::gyroscope
if not any(quat1):
# If it's all 0's means we got no data, don't do anything
return
h = Header()
h.stamp = rospy.Time.now()
# frame_id is node name without /
h.frame_id = rospy.get_name()[1:]
# We currently don't know the covariance of the sensors with each other
cov = [0, 0, 0, 0, 0, 0, 0, 0, 0]
quat = Quaternion(quat1[0] / 16384.0,
quat1[1] / 16384.0,
quat1[2] / 16384.0,
quat1[3] / 16384.0)
# Normalize the quaternion and accelerometer values
quatNorm = sqrt(quat.x * quat.x + quat.y *
quat.y + quat.z * quat.z + quat.w * quat.w)
normQuat = Quaternion(quat.x / quatNorm,
quat.y / quatNorm,
quat.z / quatNorm,
quat.w / quatNorm)
normAcc = Vector3(acc[0] / 2048.0,
acc[1] / 2048.0,
acc[2] / 2048.0)
normGyro = Vector3(gyro[0] / 16.0, gyro[1] / 16.0, gyro[2] / 16.0)
imu = Imu(h, normQuat, cov, normGyro, cov, normAcc, cov)
imuPub.publish(imu)
roll, pitch, yaw = euler_from_quaternion([normQuat.x,
normQuat.y,
normQuat.z,
normQuat.w])
oriPub.publish(Vector3(roll, pitch, yaw))
oriDegPub.publish(Vector3(degrees(roll), degrees(pitch), degrees(yaw)))
posePub.publish( PoseStamped(h,Pose(Point(0.0,0.0,0.0),normQuat)) )
# Package the arm and x-axis direction into an Arm message
评论列表
文章目录
