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();