坐标变换学习笔记—代码篇ROS

坐标变换-ROS

在ROS中，与旋转有关的函数主要集中在Matrix3x3和Quaternion 2个类中，它们都在tf命名空间中。下面旋转量之间的转换与 坐标变换学习笔记—理论篇 是对应的。

transform_datatypes

在 transform_datatypes.h 中，定义了一些常用的不同类型数据的转换方法，注意，ROS中的四元数默认的顺序是: (x, y, z, w)：

double yaw;
tf::Quaternion q;  // 注意，ROS中的四元数默认的顺序是: (x, y, z, w)
geometry_msgs::Quaternion msg_q; // 四元数消息类型，在geometry_msgs中，通过ROS命令 rosmsg show geometry_msgs/Quaternion 可查看其中内容为四个float类型的变量：x,y,z,w;

// 从欧拉角创建四元数:
q = tf::createIdentityQuaternion();  // 构造单位四元数：(0, 0, 0, 1)
q = tf::createQuaternionFromRPY(roll, pitch, yaw);  // 根据欧拉角构造四元数；
q = tf::createQuaternionFromYaw(yaw);  // 根据航偏角构造四元数；
// 上面这三个函数，内部都是调用的四元数内部的setRPY函数来实现的:
	// q.setRPY(0, 0, 0); q.setRPY(roll, pitch, yaw); q.setRPY(0, 0, yaw); // 根据欧拉角参数设置四元数；
	// return q; // 返回四元数；

// 四元数消息类型转换:
tf::quaternionTFToMsg(q, msg_q);  // 将 tf::Quaternion 类型的四元数直接转换成ROS的四元数消息类型。内部实现方式如下：
  // 若q的模不为1，利用 normalize() 方法将其归一化；
  // 然后 msg_q.x = q.x(); msg_q.y = q.y(); msg_q.z = q.z();  msg_q.w = q.w(); 对四元数消息赋值；
tf::quaternionMsgToTF(msg_q, q);  // 从ROS的四元数消息类型中得到 tf::Quaternion 类型的四元数。内部实现方式如下：
  // q = Quaternion(msg_q.x, msg_q.y, msg_q.z, msg_q.w); // 构造四元数；
  // 如果q模不为1，则 q.normalize(); （利用 normalize() 函数将其归一化）；
msg_q = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);  // 将欧拉角（roll, pitch, yaw）直接转换为ROS的四元数消息类型。内部实现方式如下：
  // quaternionTFToMsg(createQuaternionFromRPY(roll, pitch, yaw), q_msg); 
  // return q_msg;
msg_q = tf::createQuaternionMsgFromYaw(yaw);  // 将欧拉角（yaw）直接转换为ROS的四元数消息类型。内部实现方式如下：
   // q.setRPY(0.0, 0.0, yaw);
   // quaternionTFToMsg(q, q_msg);
   // return q_msg;

// 从四元数中得到欧拉角：
yaw = tf::getYaw(msg_q);  // 从ROS的四元数消息类型中得到航偏角yaw。内部实现方式如下：
  // quaternionMsgToTF(msg_q, q);
  // return getYaw(q);
yaw = tf::getYaw(q);  // 通过 tf::Matrix3x3(bt_q).getRPY( useless_roll, useless_pitch,yaw); 得到欧拉角，然后返回yaw值。内部实现方式如下：
  // tf::Matrix3x3(q).getRPY(useless_roll, useless_pitch, yaw); // 先将四元数转化为Matrix3x3的矩阵，然后再将矩阵转成欧拉角；
  // return yaw;

tf::Quaternion类

在Quaternion类中，主要涉及到有各种不同方式的四元数初始化，欧拉角转四元数，利用四元数计算旋转向量，重载与四元数相关的运算符和数据类型（两个四元数相乘，点乘，求逆，加减乘除，…），四元数插值等操作 ：

四元数初始化及与其它旋转量的转换

#include <ros/ros.h>
#include <tf/tf.h>
#include <geometry_msgs/Quaternion.h>
#include <iostream>


inline void printQuaternion(const tf::Quaternion& q, const std::string& info)
{
   
    // std::cout << "quaternion: " << q[0] << ", " << q[1] << ", " << q[2] << ", " << q[3] << std::endl;  // x,y,z,w
    std::cout << info << " quaternion: " << q.x() << ", " << q.y() << ", " << q.z() << ", " << q.w() << std::endl; // 两种元素访问方式均可
}

inline void printQuaternion(const geometry_msgs::Quaternion& msg_q, const std::string& info)  // 通过 rosmsg show geometry_msgs/Quaternion 可查看四元数消息内容:
{
   
    std::cout << info << " msg quaternion: " << msg_q.x << ", " << msg_q.y << ", " << msg_q.z << ", " << msg_q.w << std::endl;  // x,y,z,w
}


int main(int argc, char** argv)
{
   
    ros::init(argc, argv, "transform_ros");
    ros::NodeHandle nh;
    
    // 1, 四元数初始化方法：
    std::cout << "1: -------------------- quaternion initialize --------------------\n";
    double angle = M_PI / 4.0;
    tf::Vector3 axis(0, 0, 1);
    double qx = 0, qy = 0, qz = 0, qw = 1;
    double roll = M_PI / 2.0, pitch = M_PI / 3.0, yaw = M_PI / 4.0;
    
    tf::Quaternion q1(qx, qy, qz, qw);  // 直接初始化；
    tf::Quaternion q2(axis, angle);     // 通过旋转向量来初始化四元数 (见 https://blog.csdn.net/sunqin_csdn/article/details/107969585 式(3.2))；
    tf::Quaternion q3;
    q3.setRotation(axis, angle);  
    tf::Quaternion q4 = tf::Quaternion(axis, angle);  // 旋转向量转四元数, 内部实现方式如下:
        // tfScalar d = axis.length();
        // tfScalar s = tfSin(angle * tfScalar(0.5)) / d;
        // setValue(axis.x() * s, axis.y() * s, axis.z() * s, tfCos(angle * tfScalar(0.5)));
    tf::Quaternion q5;
    q5.setRPY(roll, pitch, yaw);  // 根据欧拉角转四元数公式(5.1)，初始化四元数；
    printQuaternion(q1, "q1");
    printQuaternion(q2, "q2");
    printQuaternion(q3, "q3");
    printQuaternion(q4, "q4");
    printQuaternion(q5, "q5");

    // 2, 创建四元数(下面几种创建四元数的方法，内部均是通过setRPY的函数方法实现的):
    std::cout << "\n2: -------------------- quaternion creation --------------------\n";
    tf::Quaternion q6 = tf::createIdentityQuaternion();  // x,y,z,w: 0,0,0,1
    tf::Quaternion q7 = tf::createQuaternionFromRPY(roll, pitch, yaw);  // setRPY(roll, pitch, yaw)
    tf::Quaternion q8 = tf::createQuaternionFromYaw(yaw);  // setRPY(0, 0, yaw)
    geometry_msgs::Quaternion msg_q1 = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);  // 根据欧拉角创建四元数, 再将其转换为消息类型;
    geometry_msgs::Quaternion msg_q2 = tf::createQuaternionMsgFromYaw(yaw);
    printQuaternion(q6, "q6");
    printQuaternion(q7, "q7");
    printQuaternion(q8, "q8");
    printQuaternion(msg_q1, "msg q1");
    printQuaternion(msg_q2, "msg q2");
    tf::quaternionTFToMsg(q1, msg_q1);  // 将四元数消息类型转换为数据类型(q1 --> msg_q1)  // 四元数数据类型(tf::Quaternion)和四元数消息类型(geometry_msgs::Quaternion)间的转换
    tf::quaternionMsgToTF(msg_q2, q2);  // 将四元数数据类型转换为消息类型(msg_q2 --> q2)

    // 3, 四元数与其它旋转量间的转换:
    std::cout << "\n3: -------------------- quaternion conversion --------------------\n";
    q6.setRotation(axis, angle);  // Axis-Angle to quaternion:
    angle = q3.getAngle();  // quaternion to Axis-Angle:
    axis = q3.getAxis();