沉默杀手HomeArchivesTagsCategories
tf2的学习(一)
|ROSROS程序和源码分析|
Word count: 1.3k|Reading time: 5 min

现在的ROS提倡使用tf2

tf2经过重新设计，只提供tf的关键功能，不涉及转换等函数。

tf_conversions.transformations.quaternion_from_euler(roll, pitch, yaw)

每个listener都有一个buffer储存所有tf广播发出的transforms，当广播发出transform时，需要花点时间(毫秒级)才会进入buffer，所以请求now的transform时，会有一小段时间差。

使用tf2_ros::Buffer的lookupTransform()函数可以获得tf树的指定时间的转换。常用的指定时间是ros::Time(0)和ros::Time::now，前者是缓冲区中最新可用的转换，后者就是当前的时间。对于now，由于时间差，可能出现报警

有时会出现报警： Lookup would require extrapolation into the past

对于这个报警，我们有四种解决方法：

  1. 完善lookupTransform的参数，增加ros::Duration(sec)，sec大于报警中的时间差即可
  2. 使用tf2_ros::Buffer的canTransform函数，有可用的变换了再获得
  3. 使用tf message filter
  4. 忽视这个报警，未成功获取的新transform会放弃

一个比较优雅的程序是这样的:

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
30
31
// #include <ros/ros.h>
// #include <tf2_ros/transform_listener.h>
// #include <geometry_msgs/TransformStamped.h>

ros::init(argc, argv, "tf_node");
ros::NodeHandle nh;
tf2_ros::Buffer buff;
tf2_ros::TransformListener listener(buff);
ros::Publisher pub = nh.advertise<geometry_msgs::TransformStamped>("transform",10);
ros::Rate rate(400);
while(nh.ok())
{
    geometry_msgs::TransformStamped trans;
    try {
        ros::Time now = ros::Time::now();
       // if(buff.canTransform("base_footprint","imu",now,
        // ros::Duration(0.03),NULL)
        // 30毫秒应该足够了
        trans = buff.lookupTransform("base_footprint",
                         "imu", now, 
                         ros::Duration(0.03) );
       // else
           // ROS_WARN("no transform in buffer");
    } catch (tf2::TransformException & ex) {
        ROS_WARN("%s", ex.what());
        ros::Duration(1.0).sleep();
        continue;
    }
    pub.publish(trans);
    rate.sleep();
}

创建listener后，会接受tf2 transformations，对其缓存10秒。TransformListener对象应当是persist，否则cache不能填充。常见方法是让TransformListener对象作为类的成员变量。

lookupTransform获取的是TransformStamped消息，最后发布出来。因为加了Duration，try catch可以去掉。

由于实际上加了Duration，所以得到的不是当前的，而是上一个“当前”。换句话说，想得到当前最新的transform其实是没有意义的，一般都用ros::Time(0)，Wiki上也是这么说的

怎么才知道我们修改后的程序有效了？一个是看程序运行后是否报警，还有就是echo transform话题，结果可以看到头部

只看seq和时间，如果增长一直很流畅，那就是修改生效了。如果修改还有问题，比如Duration时间太短，seq在增长一会后会有停顿，然后继续增长，这就是buffer里面空了，tf数据还没有插入到里面

已知点在子坐标系中的坐标和父子坐标系的变换，求点在父坐标系的坐标

用tf::transformPose更方便 
1
2
3
4
5
6
7
8
9
10
geometry_msgs::PointStamped point_in, point_out;
point_in.header.frame_id = "child";
point_in.header.seq = 0;
point_in.header.stamp = ros::Time(0);
point_in.point.x = 2;
point_in.point.y = 1;
point_in.point.z = 4;

// buff跟上面的使用一样， 返回的point_out是point_in在 parent坐标系 的坐标
point_out = buff.transform(point_in, point_out, "parent", ros::Duration(1));

使用这段程序前，必须在find_package里添加 tf2-geometry-msgs，否则编译不成功。如果没有，需要先安装这两个包：

1
2
3
# 依赖库，  即使目前有，也要安装，可能需要更新
sudo apt-get install ros-kinetic-orocos-kdl
sudo apt-get install ros-kinetic-tf2-geometry-msgs

手动实现 tf2_ros::Buffer::transform函数

也就是用程序实现上面的转换

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
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
ros::init(argc, argv, "test_tf");
ros::NodeHandle nh;

tf2_ros::Buffer buff;
tf2_ros::TransformListener listener(buff);
geometry_msgs::TransformStamped transform =
        buff.lookupTransform("parent","child",ros::Time(0),ros::Duration(1));

ROS_INFO("transform from parent to child  x: %f",transform.transform.translation.x);
ROS_INFO("transform from parent to child  y: %f",transform.transform.translation.y);
ROS_INFO("transform from parent to child  z: %f\n",transform.transform.translation.z);

geometry_msgs::PointStamped point_in, point_out;
point_in.header.frame_id = "child";
point_in.header.seq = 0;
point_in.header.stamp = ros::Time(0);
// 在 child 坐标系中的点
point_in.point.x = 2;
point_in.point.y = 1;
point_in.point.z = 4;

ROS_INFO("point in child: 2, 1, 4\n");
point_out = buff.transform(point_in, point_out, "parent", ros::Duration(1));

ROS_INFO("point int parent, x: %f",point_out.point.x);
ROS_INFO("point int parent, y: %f",point_out.point.y);
ROS_INFO("point int parent, z: %f\n",point_out.point.z);

cout<<fixed<<setprecision(2);

Eigen::Quaterniond quaternion(transform.transform.rotation.w,
                              transform.transform.rotation.x,
                              transform.transform.rotation.y,
                              transform.transform.rotation.z
                              );
Eigen::Matrix3d rotation = quaternion.matrix();
cout << "rotation matrix: " <<endl<< rotation <<endl<<endl;

Eigen::Matrix4d m;
m.block(0,0,3,3) = rotation;

Eigen::Vector4d Vcol, Vrow;
Vcol << transform.transform.translation.x,
        transform.transform.translation.y,
        transform.transform.translation.z, 1;
m.col(3) = Vcol;

Vrow << 0,0,0,1;
m.row(3) = Vrow.transpose();
cout << "transform matrix: "<<endl<< m<<endl<<endl;

Eigen::Vector4d point;
point << point_in.point.x,
         point_in.point.y,
         point_in.point.z, 1;

Eigen::Vector4d transformed_point = m * point;
cout << "transformed point: "<<endl<<transformed_point.head(3) << endl;

我们已知parent-child的变换，首先需要从位移和欧拉角获得齐次变换矩阵，把point_in变换为齐次坐标，然后左乘齐次矩阵，再取结果的前三个元素。 tf变换的本质就是左乘变换矩阵

常用函数

1
2
3
4
5
6
7
8
9
10
11
12
13
void tf2::Transform::setIdentity()
{
    // Matrix3x3 m_basis;
    m_basis.setIdentity();   // 单位矩阵
    // Vector3   m_origin;
    m_origin.setValue(tf2Scalar(0.0), tf2Scalar(0.0), tf2Scalar(0.0));
}

Transform::operator*(const Transform& t) const
{
    return Transform(m_basis * t.m_basis, 
               (*this)(t.m_origin)  );
}

参考：
tf2_ros::Buffer Class
tf2_ros::MessageFilter
tf2_ros::BufferInterface
using “tf2” to transform coordinates

古月学院：位置角度平移旋转，“乱七八糟”的坐标变换