ROS——RPY角转四元数（python）

RPY角转四元数

from tf.transformations import quaternion_from_euler
from geometry_msgs.msg import Pose

# 创建位姿实例
pos = Pose()
# 角度转弧度
DE2RA = pi / 180
# RPY的单位是角度值
roll = -140.0
pitch = 0.0
yaw = 0.0
# RPY转四元素
q = quaternion_from_euler(roll * DE2RA, pitch * DE2RA, yaw * DE2RA)

pos.orientation.x = q[0]
pos.orientation.y = q[1]
pos.orientation.z = q[2]
pos.orientation.w = q[3]