constant_yaw_to_imu.py 文件源码

def yaw_to_imu(yaw):

    # WARNING: we assume zero roll and zero pitch!
    q_avg = tf.quaternion_from_euler(0.0, 0.0, yaw);
    imu_msg = Imu()
    imu_msg.orientation.w = q_avg[3]
    imu_msg.orientation.x = q_avg[0]
    imu_msg.orientation.y = q_avg[1]
    imu_msg.orientation.z = q_avg[2]

    pub_imu.publish(imu_msg)