When using ROS in normal times, it is often necessary to use various poses. For example, the pose of the robot, the data of the odometer, the end pose of the robotic arm, etc. Most of the time, quaternions are mainly used, but quaternions themselves are not very intuitive. For example, when it is necessary to add and subtract angles, it is not convenient to directly operate quaternions but convert them into Euler angles. form, and when we convert between poses, we generally need to multiply the rotation matrix. At this time, we need to convert the quaternion into a rotation matrix. In ROS, a simple conversion between these three can be done using the tf library. Here is a brief record of the conversion methods between the three:
1. Quaternion to Euler angle
C++：
Under C++, you can directly use the TF library to convert between the two:
tf::Quaternion quat;
tf::quaternionMsgToTF(odom.pose.pose.orientation, quat);
double roll, pitch, yaw;//定义存储roll,pitch and yaw的容器
tf::Matrix3x3(quat).getRPY(roll, pitch, yaw); //进行转换
cout<<"current rpy:"<<roll<<", "<<pitch<<", "<<yaw<<endl;
python：
In python, you can use the following methods to convert:
import tf
(r, p, y) = tf.transformations.euler_from_quaternion([msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w])
2. Convert Euler angles to quaternions
C++：
geometry_msgs::Quaternion q;
q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
turngoal.pose.pose.orientation.x = q.x;
turngoal.pose.pose.orientation.y = q.y;
turngoal.pose.pose.orientation.z = q.z;
turngoal.pose.pose.orientation.w = q.w;
python:
python Euler angle to quaternion did not find a package that can be called, but you can convert it yourself according to the formula:
q = tf.transformations.quaternion_from_euler(roll,pitch,yaw)
print(q)
3. Quaternion rotation matrix
C++：
tf::Quaternion qw1={
tool.pose.orientation.x,tool.pose.orientation.y,tool.pose.orientation.z,tool.pose.orientation.w};
tf::Matrix3x3 matrixw1;
matrixw1.setRotation(qw1);/*通过四元数计算得到旋转矩阵*/
python：
import tf
q=[x,y,z,w]
matrix = tf.transformations.quaternion_matrix(q)
4. Rotation matrix to quaternion
C++：
tf::Matrix3x3 matrixw2;
tf::Quaternion qAC;
matrixw2.getRotation(qAC);
arm_pose.pose.orientation.x = qAC.getX();
arm_pose.pose.orientation.y = qAC.getY();
arm_pose.pose.orientation.z = qAC.getZ();
arm_pose.pose.orientation.w = qAC.getW();
python：
q2=tf.transformations.quaternion_from_matrix(matrix)
5. Euler angle to rotation matrix
C++：
tf::Matrix3x3 matrix;
matrix.setRPY(roll,pitch,yaw);
python:
matrix2 = tf.transformations.euler_matrix(roll,pitch,yaw)
6. Rotation matrix to Euler angle
C++：
matrixw.getEulerYPR(yaw,pitch,roll);
python：
(roll2,pitch2,yaw2) = tf.transformations.euler_from_matrix(matrix2)
The following is a brief version of the test code and test results:
C++:
#include "ros/ros.h"
#include "tf/transform_listener.h"
#include "geometry_msgs/PoseStamped.h"
using namespace std;
class tf_test
{
private:
/* data */
public:
tf_test(/* args */);
void test();
~tf_test();
};
tf_test::tf_test(/* args */)
{
ros::NodeHandle n;
}
void tf_test::test()
{
tf::Quaternion quat;
geometry_msgs::PoseStamped robot_pose;
tf::Matrix3x3 matrixw1;
robot_pose.pose.orientation.w = 0.852364;
robot_pose.pose.orientation.x = -0.001;
robot_pose.pose.orientation.y = -0.49621;
robot_pose.pose.orientation.z = 0.16507;
cout<<"初始值: q.x:"<<robot_pose.pose.orientation.x<<",q.y:"<<robot_pose.pose.orientation.y<<",q.z:"<<robot_pose.pose.orientation.z<<",q.w"<<robot_pose.pose.orientation.w<<endl;
cout<<"四元数转欧拉角："<<endl;
tf::quaternionMsgToTF(robot_pose.pose.orientation, quat);
double roll, pitch, yaw;//定义存储roll,pitch and yaw的容器
tf::Matrix3x3(quat).getRPY(roll, pitch, yaw); //进行转换
cout<<"结果:"<<roll<<", "<<pitch<<", "<<yaw<<endl;
geometry_msgs::Quaternion q;
q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
cout<<"欧拉角转四元数："<<endl;
cout<<"结果:"<<q.x<<", "<<q.y<<", "<<q.z<<", "<<q.w<<endl;
cout<<"四元数转旋转矩阵："<<endl;
tf::Quaternion qw1={
robot_pose.pose.orientation.x,robot_pose.pose.orientation.y,robot_pose.pose.orientation.z,robot_pose.pose.orientation.w};
matrixw1.setRotation(qw1);/*通过四元数计算得到旋转矩阵*/
cout<<"结果:"<<matrixw1[0][0]<<" "<<matrixw1[0][1]<<" "<<matrixw1[0][2]<<" "<<endl;
cout<<" "<<matrixw1[1][0]<<" "<<matrixw1[1][1]<<" "<<matrixw1[1][2]<<" "<<endl;
cout<<" "<<matrixw1[2][0]<<" "<<matrixw1[2][1]<<" "<<matrixw1[2][2]<<" "<<endl;
ros::Duration(0.2).sleep();
cout<<"旋转矩阵转四元数："<<endl;
tf::Matrix3x3 matrixw2;
tf::Quaternion qAC;
matrixw2[0][0] = 0.453054;
matrixw2[0][1] = -0.280408;
matrixw2[0][2] = -0.846235;
matrixw2[1][0] = 0.282392;
matrixw2[1][1] = 0.945502;
matrixw2[1][2] = -0.162114;
matrixw2[2][0] = 0.845575;
matrixw2[2][1] = -0.165524;
matrixw2[2][2] = 0.507548;
matrixw2.getRotation(qAC);
cout<<"结果:"<<qAC.getX()<<", "<<qAC.getY()<<", "<<qAC.getZ()<<", "<<qAC.getW()<<endl;
ros::Duration(0.2).sleep();
cout<<"欧拉角转旋转矩阵："<<endl;
double roll1,pitch1,yaw1;
roll1 = -0.315249;
pitch1 = -1.00764;
yaw1 = 0.557382;
tf::Matrix3x3 matrix;
matrix.setRPY(roll1,pitch1,yaw1);
cout<<"结果:"<<matrix[0][0]<<" "<<matrix[0][1]<<" "<<matrix[0][2]<<" "<<endl;
cout<<" "<<matrix[1][0]<<" "<<matrix[1][1]<<" "<<matrix[1][2]<<" "<<endl;
cout<<" "<<matrix[2][0]<<" "<<matrix[2][1]<<" "<<matrix[2][2]<<" "<<endl;
cout<<"旋转矩阵转欧拉角："<<endl;
double roll2,pitch2,yaw2;
matrix.getEulerYPR(yaw2,pitch2,roll2);
cout<<"结果:"<<roll2<<", "<<pitch2<<", "<<yaw2<<endl;
}
tf_test::~tf_test()
{
}
int main(int argc, char **argv)
{
ros::init(argc,argv,"tf_test");
tf_test tf_test;
tf_test.test();
ros::spin();
return 0;
}
python：
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
import tf
if __name__ == '__main__':
rospy.init_node('fake_test')
while not rospy.is_shutdown():
rospy.sleep(0.1)
print("四元数转欧拉角：")
q=[-0.001,-0.496,0.165,0.852]
print(q)
(r, p, y) = tf.transformations.euler_from_quaternion(q)
print(r,p,y)
rospy.sleep(0.5)
print("欧拉角转四元数：")
roll = -0.31525282759303797
pitch = -1.0076434199908184
yaw = 0.5573862967289317
q2 = tf.transformations.quaternion_from_euler(roll,pitch,yaw)
print(q2)
rospy.sleep(0.5)
print("四元数转旋转矩阵：")
matrix = tf.transformations.quaternion_matrix(q)
print(matrix)
rospy.sleep(0.5)
print("旋转矩阵转四元数：")
q3 = tf.transformations.quaternion_from_matrix(matrix)
print(q3)
rospy.sleep(0.5)
print("欧拉角转旋转矩阵：")
matrix2 = tf.transformations.euler_matrix(roll,pitch,yaw)
print(matrix2)
rospy.sleep(0.5)
print("旋转矩阵到欧拉角：")
(roll2,pitch2,yaw2) = tf.transformations.euler_from_matrix(matrix2)
print(roll2,pitch2,yaw2)
rospy.sleep(0.5)
