ROS TF2当前坐标系如何计算其它历史坐标系的坐标变换

ROS TF2当前坐标系如何计算其它历史坐标系的坐标变换

• 1、时间旅行
• 2、更高级 的 lookupTransform() 函数 API
• 3、检查结果
• 4、 完成

 本教程教您有关tf2的高级功能：

TF2当前坐标系如何计算其它历史坐标系的坐标变换

在上一教程中，介绍了tf2和时间的基本概念。 本教程将使这一步骤更进一步，并介绍最强大的tf2技巧之一。

1、时间旅行

打开之前监听者的例子

就是这个

打开 src/turtle_tf2_listener.cpp 这个文件

更改 这个部分代码

 try{
        transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1", ros::Time::now(),ros::Duration(3.0));
      } catch (tf2::TransformException &ex) {
        ROS_WARN("Could NOT transform turtle2 to turtle1: %s", ex.what());
      }

 现在不让第二只乌龟去第一只乌龟当前的位置了，而是去 5s 之前的位置。

代码 改成如下

  try{
        ros::Time past = ros::Time::now() - ros::Duration(5.0);
        transformStamped = tfBuffer.lookupTransform("turtle2", "turtle1",
                               past, ros::Duration(1.0));
      } catch (tf2::TransformException &ex) {
        ROS_WARN("Could NOT transform turtle2 to turtle1: %s", ex.what());
      }

 编译运行

第1个5s，乌龟2 应该不会动，因为还没有 5s的历史数据。那之后的5s 呢。 试试

 乌龟2 在 乱动 ， 为什么呢？

因为我们向 TF2 请求的是 相对于5s前乌龟1相对与5s前乌龟2 的 位置 。然后 作用在了 当前的 乌龟2的运动中。

那如何 获取 5秒前乌龟1的位置，相对与当前乌龟2的位置呢。

就是用下面 更高级 的 lookupTransform() 函数 API

2、更高级 的 lookupTransform() 函数 API

直接上代码

 try{
        ros::Time now = ros::Time::now();
        ros::Time past = now - ros::Duration(5.0);
        transformStamped = tfBuffer.lookupTransform("turtle2", now,
                             "turtle1", past,
                             "world", ros::Duration(1.0));
      } catch (tf2::TransformException &ex) {
        ROS_WARN("Could NOT transform turtle2 to turtle1: %s", ex.what());
      }

 此时 lookupTransform() 函数 有 6 个 参数

从这个坐标系转换

参数1 的 时刻

转到这个坐标系

参数3 的时刻

一个 固定不变的坐标系

时间 溢出时间

原理就是先求 参数 3 和参数4 到固定 坐标系的 变换 再求参数1 和参数2 的

3、检查结果

再编译运行

第2 只 乌龟 现在 则 由 5s 前 的乌龟1 的位置 为导向了

4、 完成