关于ros --tf2的使用
在tf2系统中，将包分为tf2和tf2_ros。
tf2用来进行坐标旋转，以及tf、msg两种四元素数据结构的变换；
tf2_ros负责与ROS数据通信打交道，负责发布tf或订阅tf，即发布者和订阅者是在tf2_ros命名空间下的。
1、tf2 (姿态变换和tf格式转换)
1.1 四元数的组成
一个四元素有4个成员（x,y,z,w）。不绕xyz轴旋转的常用单位四元数为（0,0,0,1)。
注意： w 是最后一个，但是一些库像 Eigen 把 w 放在第一的位置。
#include 四元数的大小应为1。 如果数值错误导致四元数的大小不为1，ROS将显示警告。 为避免这些警告，需要对四元数进行标准化：
q.normalize();
1.2 ROS中四元素的数据类型
ROS 有两种四元素数据类型：msg 和 tf。
geometry_msgs::Quaternion q_msg; // msg 类型
tf2::Quaternion q_tf; // tf 类型
在tf2_geometry_msgs.h文件中提供了数据类型转换的函数:
#include 1.3 通过四元素做旋转
一个刚体进行旋转，求该刚体旋转后的姿态。只需要将刚体原姿态的四元数乘以旋转的四元数。注意，乘法的顺序很重要。如：
#include (四元素的逆矩阵把w参数设成负的即可)
类似的，假如在一个坐标系下有两个姿态用四元数表示为q_1和q_2，那如何求这两个姿态的旋转四元数q_r呢。其实也可以看成q_2如何旋转为q_1:
q_2 = q_r*q_1
1
那么：
q_r = q_2*q_1_inverse;//q_1_inverse是q_1的逆矩阵
1
如：
q1_inv[0] = q1.pose.orientation.x;
q1_inv[1] = q1.pose.orientation.y;
q1_inv[2] = q1.pose.orientation.z;
q1_inv[3] = -q1.pose.orientation.w; //注意这个负号
q2[0] = c_pose.pose.orientation.x;
q2[1] = c_pose.pose.orientation.y;
q2[2] = c_pose.pose.orientation.z;
q2[3] = c_pose.pose.orientation.w;
q_r = tf.transformations.quaternion_multiply(q2, q1_inv);
2、tf2_ros (tf广播和监听)
ROS官网tf广播和监听教程
2.1、消息格式
ROS中广播和监听的tf2消息类型是tf2_msgs::TFMessage，其本质是：
geometry_msgs/TransformStamped类型的向量形式
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
float64 z
float64 w
2.2 、广播tf2
#include 2.3 监听tf
2.3.1 tf监听
```cpp
#include
2.3.2 tf2监听
使用tf2消息的大部分工作通过tf2_ros::Buffer类完成，其通过tf2_ros::TransformListener完成对tf2_ros::Buffer类的初始化和构造，并订阅相应tf消息，后续操作都是用的tf2_ros::Buffer类的成员函数完成：
```cpp
#include //与tf监听有些区别
#include
#include
#include
....
//创建TransformListener对象。一旦创建了侦听器，它就开始通过连接接收tf2转换，并对它们进行长达10秒的缓冲
tf2_ros::Buffer tfBuffer;
tf2_ros::TransformListener tfListener(tfBuffer);
ros::Rate rate(10.0);
while (node.ok()){
geometry_msgs::TransformStamped transformStamped;
try{
transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1",ros::Time(0), ros::Duration(3.0));//lookupTransform(“target frame”, “source frame”,ros::Time(0));第三个参数ros::Time(0)代表从buffer中获取“最新可获取时间”的变换。 每一个listener都有一个buffer储存来自来自不同tf2广播者的坐标系变换关系。这些变换进入缓冲区需要一段很小的时间，所以第三个参数不应该为ros::Time::now(),一般使用ros::Time(0)就很好。
geometry_msgs::PointStamped world, velo_link;
tfBuffer.transform(world, velo_link, "velo_link", ros::Duration(1.0));//transform
}
catch (tf2::TransformException &ex) {
ROS_WARN("%s",ex.what());
ros::Duration(1.0).sleep();
continue;
}
}
2.4、tf相关工具命令
2.4.1 查看两个frame之间的变换关系
rosrun tf tf_echo source_frame(参考) target_frame
查看target_frame在source_frame下的位姿。
（source_frame是parent，target_frame是child）
2.4.2 根据当前的tf树创建一个pdf图
rosrun tf view_frames
这个工具首先订阅 /tf ，订阅5秒钟，根据这段时间接受到的tf信息，绘制成一张tf tree，然后创建成一个pdf图
2.4.3 查看当前的tf树
rosrun rqt_tf_tree rqt_tf_tree
2.5、tf2_ros::MessageFilter
使用tf2_ros::MessageFilter去处理待有时间戳的数据类型。
private：
message_filters::Subscriber<geometry_msgs::PointStamped> point_sub_;
tf2_ros::MessageFilter<geometry_msgs::PointStamped> tf2_filter_;
public：
tf2_filter_(point_sub_, buffer_, target_frame_, 10, 0)
{
point_sub_.subscribe(n_, "turtle_point_stamped", 10);
tf2_filter_.registerCallback( boost::bind(&PoseDrawer::msgCallback, this, _1) );
}
MessageFilter将订阅任何带有报头的ros消息，并对其进行缓存，直到能够确保将其转换为target_frame坐标系下为止，然后调用回调函数，将数据转换到taget_frame坐标系下。
3、tf的C++数据结构
基本的数据类型有：
Quaternion
Vector
Point
Pose
Transform
其中Quaternion 表示四元数，vector3是一个3*1 的向量，point表示一个点坐标，Pose是位姿（包括点坐标以及方向），Transform是一个转换的模版。
3.1 tf::Stamped
tf::Stamped 是一种包含除了Transform的其他几种基本的数据结构的一种数据结构:
template <typename T> //模版结构可以是tf::Pose tf:Point 这些基本的结构
class Stamped : public T{
public:
ros::Time stamp_; //记录时间
std::string frame_id_; //ID
Stamped() :frame_id_ ("NO_ID_STAMPED_DEFAULT_CONSTRUCTION"){}; //Default constructor used only for preallocation
Stamped(const T& input, const ros::Time& timestamp, const std::string & frame_id);
void setData(const T& input);
};
3.2 tf::StampedTransform
TF::stampedtransform是TF的一种特殊情况：它需要frame_id和stamp以及child_frame_id。
/** \brief The Stamped Transform datatype used by tf */
class StampedTransform : public tf::Transform
{
public:
ros::Time stamp_; // 时间戳
std::string frame_id_; //定义转换坐标框架的frameID，parent
std::string child_frame_id_; ///的坐标系变换定义的id ，child
StampedTransform(const tf::Transform& input, const ros::Time& timestamp, const std::string & frame_id, const std::string & child_frame_id):
tf::Transform (input), stamp_ ( timestamp ), frame_id_ (frame_id), child_frame_id_(child_frame_id){ };
/** \brief Default constructor only to be used for preallocation */
StampedTransform() { };
/** \brief Set the inherited Traonsform data */
void setData(const tf::Transform& input){*static_cast<tf::Transform*>(this) = input;};
};
3.3 tf::Pose/tf::StampedTransform获取位姿元素
tf::Pose odom;
//或tf::StampedTransform odom;
odom.getOrigin().x();
odom.getOrigin().y();
tf::getYaw(odom.getRotation());
//四元素
odom.getRotation().getW();
odom.getRotation().getX();
odom.getRotation().getY();
odom.getRotation().getZ();
odom.setOrigin( tf::Vector3(msg->x, msg->y, 0.0));
odom.setRotation( tf::Quaternion(0, 0, 0, 1) );
//源码
namespace tf{
typedef tf::Transform Pose;
}
官网
使用tf::transform进行简单的不同frame间的pose转换。
tf::TransformListener listener;
void transformPose(const std::string &target_frame, const geometry_msgs::PoseStamped &stamped_in, geometry_msgs::PoseStamped &stamped_out) const
参数名字的定义对功能的说明还是很明显的，target_frame就是你要把源pose转换成哪个frame上的pose。假如你的源pose的frame_id是"odom"，你想转到"map"上，那么target_frame写成“map”就可以了。stamped_in就是源pose，而stamped_out就是目标数据了，也就是转换完成的数据。需要注意的是，从参数上来看，转换时是不需要指定源frame_id的，这是因为它已经包含在了stamped_in中，换句话说，就是这个函数一个隐含的使用条件是，stamped_in中必须指明源pose属于哪个frame。
// transform from "odom" to "map"
try{
listener.transformPose("map", pose_odom, pose_map);
}
catch( tf::TransformException ex)
{
ROS_WARN("transfrom exception : %s",ex.what());
return;
}