tf 四元素 转欧拉角

tf 四元素 转欧拉角
       

#include <tf2_ros/transform_broadcaster.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>        
             tf2::Quaternion quaternion(
                     uwb_pose_stamped_.pose.orientation.x,
                     uwb_pose_stamped_.pose.orientation.y,
                     uwb_pose_stamped_.pose.orientation.z,
                     uwb_pose_stamped_.pose.orientation.w
             );
             tf2::Matrix3x3 matrix(quaternion);
             double uwb_roll, uwb_pitch, rz_mapTuwbposemsg;
             matrix.getRPY(roll, pitch, rz_mapTuwbposemsg);
         

#include "tf/transform_datatypes.h"
#include "tf/transform_listener.h"
             tf::Quaternion quaternion1(
                     uwb_pose_stamped_.pose.orientation.x,
                     uwb_pose_stamped_.pose.orientation.y,
                     uwb_pose_stamped_.pose.orientation.z,
                     uwb_pose_stamped_.pose.orientation.w
             );
             double uwb_roll, uwb_pitch, rz_mapTuwbposemsg;//定义存储r\p\y的容器
             tf::Matrix3x3(quaternion1).getRPY(uwb_roll, uwb_pitch, rz_mapTuwbposemsg);//进行转换

欧拉角转4元素

        geometry_msgs::PoseStamped pose_stamped;
     //欧拉角转4元素
            tf::Vector3 axis(0, 0, 1);
            tf::Quaternion q(axis, (1.57));

            pose_stamped.pose.orientation.x = q.x();
            pose_stamped.pose.orientation.y = q.y();
            pose_stamped.pose.orientation.z = q.z();
            pose_stamped.pose.orientation.w = q.w();

计算路径的朝向

    //计算路径的朝向，计算向量的斜率，向量的朝向
    tf2::Quaternion global_planner::getSlopeOfVector(double sart_x, double sart_y,double end_x, double end_y,double &yaw){
        double x=end_x-sart_x;
        double y=end_y-sart_y;
        yaw= atan2(y,x);
        //欧拉角转4元素
        tf2::Vector3 axis(0, 0, 1);
        tf2::Quaternion q(axis, yaw);

//        geometry_msgs::PoseStamped pose_stamped;
//        pose_stamped.pose.orientation.x = q.x();
//        pose_stamped.pose.orientation.y = q.y();
//        pose_stamped.pose.orientation.z = q.z();
//        pose_stamped.pose.orientation.w = q.w();

        return q;
    }