ros角度与四元素的转换与TF程序包相关

1、Yaw角到Quaternion

 geometry_msgs::Quaternion geo_q = tf::createQuaternionMsgFromYaw(refLinePose_.yaw);

Quaternion到Yaw角返回的是弧度

double yaw=tf::getYaw(i.pose.orientation); //
pose current_pose;  //得到的Pose
 
tf::StampedTransform tf_ 
tf_.setOrigin( tf::Vector3(current_pose.x, current_pose.y, current_pose.z) );    //设置平移

  //欧拉角 -->>四元数
tf::Quaternion q;
q.setRPY(current_pose.roll, current_pose.pitch, current_pose.yaw); 
 
tf_.setRotation(q);    //TF 的transform 必须从四元数获取旋转信息
 
 //四元数 -->> 欧拉角
tf::Quaternion quat;
tf::quaternionMsgToTF(odom.pose.pose.orientation, quat);
double roll, pitch, yaw;   //定义存储r\p\y的容器
tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);//进行转换
 

/*********四元数转欧拉角********/
tf::Quaternion tf_q;
tf::quaternionMsgToTF(amcl_pose.pose.pose.orientation,tf_q);
double roll,pitch,yaw;
tf::Matrix3x3(tf_q).getRPY(roll,pitch,yaw);
 
//四元数转yaw角返回的是弧度
tf::getYaw(tf_q);
tf::getYaw(ros_q);
 
/********弧度欧拉角转四元数*****/
ros_q = tf::createQuaternionMsgFromYaw(yaw);
rt_q = f::createQuaternionFromYaw(yaw);
ros_q = tf::createQuaternionMsgFromRollPitchYaw(roll,pitch,yaw);
tf_q = tf::createQuaternionFromRollPitchYaw(roll,pitch,yaw);
tf_q.setRPY(roll,pitch,yaw);
 
void GetXYZWFromRPY(double roll, double pitch, double yaw, double& x, double& y, double& z, double& w)
{
  roll /= 2;
  pitch /= 2;
  yaw /= 2;
  x = sin(roll) * cos(pitch) * cos(yaw) - cos(roll) * sin(pitch) * sin(yaw);
  y = cos(roll) * sin(pitch) * cos(yaw) + sin(roll) * cos(pitch) * sin(yaw);
  z = cos(roll) * cos(pitch) * sin(yaw) - sin(roll) * sin(pitch) * cos(yaw);
  w = cos(roll) * cos(pitch) * cos(yaw) + sin(roll) * sin(pitch) * sin(yaw);
}

2、tf四元素的理解
该链接重要
ROS TF2 中的 四元数 基础部分：https://blog.csdn.net/qq_32761549/article/details/109110598
ROS使用四元数来跟踪和应用旋转。

一个四元素有4个成员（x,y,z,w）。

注意： w 是最后一个，但是一些库像 Eigen 把 w 放在第一的位置。

不绕x / y / z轴旋转的 常用单位 四元数为（0,0,0,1）：

在一个节点 加入下面 代码 setRPY() 会 把 绕 x、y、z轴旋转的角度 转化 为 四元数
设置不绕x / y / z轴旋转的setRPY( 0, 0, 0 ) 打印出四元数 则 为 （0,0,0,1）

#include <tf2/LinearMath/Quaternion.h>
...
tf2::Quaternion myQuaternion;
myQuaternion.setRPY( 0, 0, 0 );  // Create this quaternion from roll/pitch/yaw (in radians)
ROS_INFO("%f  %f  %f  %f" ,myQuaternion.x(),myQuaternion.y(),myQuaternion.z(),myQuaternion.w());  // Print the quaternion components (0,0,0,1)

欧拉角使用绕x,y,z三个轴的旋转来表示物体旋转，四元数表示为 Q=(x,y,z,w)，其中的 x、y、z分别表示如下，表示绕对应的轴旋转多少弧度。
绕 X轴 旋转，称之为 横滚角，使用roll表示;
绕 Y轴 旋转，称之为 俯仰角，使用pitch表示;
绕 Z轴 旋转，称之为 航向角，使用yaw表示;
w表示为标量，不绕x / y / z轴旋转的 常用单位 四元数为（0,0,0,1）
四元数是使用一个旋转的向量 + 一个旋转的角度来表示物体旋转。
用于旋转的四元数，平方和为1.

TF程序包相关的命令行使用

用命令行显示当前所有frame的方法：

    rosrun tf tf_monitor #显示当前坐标变换树的信息，主要是名称和实时的时间延时

rostopic echo /tf #以TransformStamped消息类型的数组显示所有父子frame的位姿转换关系

以上主要是数据显示

根据当前的tf树创建一个pdf图：

$ rosrun tf view_frames

这个工具首先订阅/tf，订阅5秒钟，根据这段时间接受到的tf信息，绘制成一张tf tree，然后创建成一个pdf图。

将会以图形的形式显示出TF树中所有的frame和两个frame 的父子关系及其Broadcaster、Average rate等

查看当前的tf树：

$ rosrun rqt_tf_tree rqt_tf_tree

该命令同样是查询tf tree的，但是与第一个命令的区别是该命令是动态的查询当前的tf tree,当前的任何变化都能当即看到，例如何时断开何时连接，捕捉到这些然后通过rqt插件显示出来。

查看两个frame之间的变换关系：

$

rosrun tf tf_echo[source_frame][target_frame]

将会持续的显示源坐标系和目标坐标系的位姿变换关系。
tf2_msgs/TFMessage消息的具体格式：

geometry_msgs/TransformStamped[] transforms
        std_msgs/Header header
                uint32 seq
                time stamp
                string frame_id
        string child_frame_id
        geometry_msgs/Transform transform
                geometry_msgs/Vector3 translation
                        float64 x
                        float64 y
                        float64 z
                geometry_msgs/Quaternion rotation
                        float64 x
                        float64 y
                        flaot64 z
                        float64 w