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 的位置 为导向了