四元素以及欧拉角

c++

头文件

#include "tf/LinearMath/Matrix3x3.h"
#include "geometry_msgs/Quaternion.h"
#include "tf/transform_datatypes.h"

欧拉角到四元素

sensor_msgs::Imu imu_msg

tf::Matrix3x3 rotation_matrix;
        rotation_matrix.setEulerYPR( nodes[i].gAngle * 3.1415926 / 180.0 * -1.0, 0., 0. );
        tf::Quaternion quat;
        rotation_matrix.getRotation( quat );
        imu_msg.orientation.x = quat.getX();
        imu_msg.orientation.y = quat.getY();
        imu_msg.orientation.z = quat.getZ();
        imu_msg.orientation.w = quat.getW();
四元素到欧拉角

geometry_msgs::Quaternion orientation = imu_msg.orientation;
        tf::Matrix3x3 mat(tf::Quaternion(orientation.x, orientation.y, orientation.z, orientation.w));
        double yaw, pitch, roll;
        mat.getEulerYPR(yaw, pitch, roll);
        ROS_INFO("The combined_odom yaw is %f ",yaw * 180 / 3.1415926);
        /*tf::Quaternion quat1;
        tf::quaternionMsgToTF(imu_msg.orientation, quat1);
        double roll, pitch, yaw;
        tf::Matrix3x3(quat1).getRPY(roll, pitch, yaw);
        ROS_INFO_STREAM("The  yaw is " << yaw * 180.0 / 3.1415926 << " " << roll << " " <<  pitch);

 

python

from tf.transformations import euler_from_quaternion

from geometry_msgs.msg import Quaternion, Twist, Pose, QuaternionStamped
from nav_msgs.msg import Odometry

quaternion = Quaternion()

quaternion.x = 0.0
            quaternion.y = 0.0
            quaternion.z = sin(self.th / 2.0)
            quaternion.w = cos(self.th / 2.0)

odom = Odometry()
            odom.header.frame_id = "odom"
            odom.child_frame_id = self.base_frame
            #odom.child_frame_id = "baselink"
            odom.header.stamp = now
            odom.pose.pose.position.x = self.x
            odom.pose.pose.position.y = self.y
            odom.pose.pose.position.z = 0
            odom.pose.pose.orientation = quaternion
            odom.twist.twist.linear.x = vxy
            odom.twist.twist.linear.y = 0
            odom.twist.twist.angular.z = vth

 quaternion_1 = [odom.pose.pose.orientation.x, odom.pose.pose.orientation.y, odom.pose.pose.orientation.z, odom.pose.pose.orientation.w]
            (roll, pitch, yaw) = euler_from_quaternion(quaternion_1)
            rospy.loginfo("The wheel odom is " + str(yaw * 180 / 3.1415926))

self.odomPub.publish(odom)