一.欧拉角转四元数
需要注意:有两种四元数类型,如下:
//初始化四元数
geometry_msgs::Quaternion q;//初始化四元数(geometry_msgs类型)
tf::Quaternion q1;//初始化四元数(tf类型)
可以通过ctrl点击Quaternion变量查看对应类型,在使用过程中一定要看变量是对应哪一种的Quaternion,否则会报错。
欧拉角转四元数转换代码:
#include <ros/ros.h>
#include <tf/tf.h>
int main(int argc, char **argv)
{
ros::init(argc, argv, "QuaternionToRPY");
ros::NodeHandle node;
double roll = 0.1, pitch = 0.2, yaw = 0.3; // 初始化欧拉角
geometry_msgs::Quaternion q;//初始化四元数(geometry_msgs类型)
tf::Quaternion q1;初始化四元数(tf类型)
q=tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);//欧拉角转四元数(geometry_msgs::Quaternion)
q1=tf::createQuaternionFromRPY(roll,pitch,yaw);欧拉角转四元数(tf::Quaternion)
ROS_INFO("w=%.2f, x=%.2f, y=%.2f, z=%.2f", q.w,q.x,q.y,q.z);
//tf::Quaternion不可以直接输出,因为类型不是double
// ROS_INFO("w1=%.2f, x1=%.2f, y1=%.2f, z1=%.2f", q1.w,q1.x,q1.y,q1.z);
}
如何相应使用对应方法:
二.四元数转欧拉角
#include <ros/ros.h>
#include <tf/tf.h>
int main(int argc, char **argv)
{
ros::init(argc, argv, "QuaternionToRPY");
ros::NodeHandle node;
double roll = 0.0, pitch = 0.0, yaw = 0.0; // 初始化欧拉角
tf::Quaternion q(0.1, 0.2, 0.3, 0.4); // 初始化四元数
tf::Matrix3x3(q).getRPY(roll, pitch, yaw);// 四元数转欧拉角
ROS_INFO("roll=%.2f, pitch=%.2f, yaw=%.2f", roll, pitch, yaw);
}