ROS欧拉角转四元数的python实现。

找遍了全网，竟然没有python版本的，我只能自己手写了555555555555555555555555555555555

#!/usr/bin/env python
# -*- coding:utf-8 -*-
import geometry_msgs
import math

class RPY2Quar:
    def __init__(self,roll,pitch,yaw):
        self.roll=roll
        self.pitch=pitch
        self.yaw=yaw
    # 欧拉角转四元数
    def RPY2Quar(self):
        cy=math.cos(self.yaw*0.5)
        sy=math.sin(self.yaw*0.5)
        cp=math.cos(self.pitch*0.5)
        sp=math.sin(self.pitch*0.5)
        cr =math.cos(self.roll * 0.5);
        sr =math.sin(self.roll * 0.5);
        
        q=geometry_msgs.msg.Pose();
        q.orientation.w= cy * cp * cr + sy * sp * sr;
        q.orientation.x = cy * cp * sr - sy * sp * cr;
        q.orientation.y = sy * cp * sr + cy * sp * cr;
        q.orientation.z = sy * cp * cr - cy * sp * sr;
        return q;