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)