关于ros --tf2的使用_ros订阅tf

在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 <tf2/LinearMath/Quaternion.h>
tf2::Quaternion q;
q.setRPY(0,0,0);//把绕x、y、z轴旋转的角度转化为 四元数
//q.x()==0;
//q.y()==0;
//q.w()==0;
//q.w()==1;
1
2
3
4
5
6
7

四元数的大小应为1。 如果数值错误导致四元数的大小不为1，ROS将显示警告。 为避免这些警告，需要对四元数进行标准化：

q.normalize();
1

1.2 ROS中四元素的数据类型
ROS 有两种四元素数据类型：msg 和 tf。

geometry_msgs::Quaternion q_msg; // msg 类型
tf2::Quaternion q_tf; // tf 类型
在tf2_geometry_msgs.h文件中提供了数据类型转换的函数:

#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
tf2::Quaternion q_tf;
geometry_msgs::Quaternion q_msg;
tf2::convert(q_msg, q_tf); //①msg类型转换为tf类型
tf2::fromMsg(q_msg, q_tf); //②msg类型转换为tf类型
q_msg = tf2::toMsg(q_tf);  //tf类型转换为msg类型
1
2
3
4
5
6
7
8
9
10

1.3 通过四元素做旋转
一个刚体进行旋转，求该刚体旋转后的姿态。只需要将刚体原姿态的四元数乘以旋转的四元数。注意，乘法的顺序很重要。如：

#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

//q_ori  是原姿态转换的tf格式的四元数
//q_rot   旋转四元数
//q_new   旋转后的姿态四元数
tf2::Quaternion q_orig, q_rot, q_new;
// msg.pose.orientation 假设是订阅的topic 是一个刚体姿态的msg格式四元数
tf2::convert(msg.pose.orientation ,q_ori);//通过tf2::convert()将msg格式转换成tf格式

double r=3.14, p=0, y=0;// 设置绕x轴旋转180度
q_rot.setRPY(r, p, y);//求tf格式的旋转四元数
q_new = q_rot*q_ori;  //旋转后的姿态 = 旋转的四元素* 原姿态
q_new.normalize(); // 归一化
msg.pose.orientation = tf2::toMsg(q_new);//旋转后的姿态由tf格式转换成msg格式
1
2
3
4
5
6
7
8
9
10
11
12
13
14

(四元素的逆矩阵把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);
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

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
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

2.2 、广播tf2

#include <tf2_ros/transform_listener.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/PointStamped.h>

  tf2_ros::TransformBroadcaster br;//TransformBroadcaster
  geometry_msgs::TransformStamped transformStamped;

  transformStamped.header.stamp = ros::Time::now();
  transformStamped.header.frame_id = "world";
  transformStamped.child_frame_id = turtle_name;
  transformStamped.transform.translation.x = msg->x;
  transformStamped.transform.translation.y = msg->y;
  transformStamped.transform.translation.z = 0.0;
  tf2::Quaternion q;
  q.setRPY(0, 0, msg->theta);//RPY转成tf格式的四元素
  transformStamped.transform.rotation.x = q.x();
  transformStamped.transform.rotation.y = q.y();
  transformStamped.transform.rotation.z = q.z();
  transformStamped.transform.rotation.w = q.w();

  br.sendTransform(transformStamped);//sendTransform()
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22

2.3 监听tf
2.3.1 tf监听

```cpp
#include <tf/transform_listener.h>

tf::TransformListener tfl;
tf::StampedTransform odom_global_tf;  // tf sub,

string odom_frame_id = "odom_combined";
string global_frame_id = "map";
try {
  tfl.waitForTransform(odom_frame_id, global_frame_id, ros::Time(0), ros::Duration(1.0));  // child,parent
  tfl.lookupTransform(odom_frame_id, global1_frame_id, ros::Time(0), odom_global_tf);  // child,parent
  }
catch (tf::TransformException &ex) {
    ROS_ERROR("%s", ex.what());
    ros::Duration(1.0).sleep();
  }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

2.3.2 tf2监听
使用tf2消息的大部分工作通过tf2_ros::Buffer类完成，其通过tf2_ros::TransformListener完成对tf2_ros::Buffer类的初始化和构造，并订阅相应tf消息，后续操作都是用的tf2_ros::Buffer类的成员函数完成：

```cpp
#include <tf2_ros/transform_listener.h>//与tf监听有些区别
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/PointStamped.h>
....
  //创建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<geometry_msgs::PointStamped>(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;
    }
 }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29

2.4、tf相关工具命令
2.4.1 查看两个frame之间的变换关系

rosrun tf tf_echo source_frame(参考) target_frame
1

查看target_frame在source_frame下的位姿。

（source_frame是parent，target_frame是child）
1

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

rosrun tf view_frames
1

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

2.4.3 查看当前的tf树

rosrun rqt_tf_tree rqt_tf_tree
1

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) );
 }
1
2
3
4
5
6
7
8
9

MessageFilter将订阅任何带有报头的ros消息，并对其进行缓存，直到能够确保将其转换为target_frame坐标系下为止，然后调用回调函数，将数据转换到taget_frame坐标系下。

3、tf的C++数据结构
基本的数据类型有：

Quaternion

Vector

Point

Pose

Transform
1
2
3
4
5
6
7
8
9

其中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);
};
1
2
3
4
5
6
7
8
9

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;};
};
1
2
3
4
5
6
7
8
9
10
11
12
13
14

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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

官网

使用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
1
2

参数名字的定义对功能的说明还是很明显的，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;
    }
1
2
3
4
5
6
7
8
9