Traveling in time(C++)
参考资料:https://docs.ros.org/en/iron/Tutorials/Intermediate/Tf2/Time-Travel-With-Tf2-Cpp.html。
目标
学习tf2的高级时间旅行特征。
背景
在上一节课程中,我们讨论了tf2和时间的基本内容。这一节,我们会深入探究tf2的功能:时间旅行。简而言之,tf2库的一个关键特点就是它能够在时间和空间上转换数据。
tf2时间旅行特点对各种任务都有用处,如长时间监控机器人的姿态或构建一个跟随机器人(跟随领航机器人的运动)。我们将使用时间旅行功能来查找过去的转换并让海龟turtle2
运动到carrot1
5秒前的位置。
时间旅行
进入功能包learning_tf2_cpp
,修改turtle_tf2_listener.cpp
。让第二只海龟运动到胡萝卜5秒前的位置,而不是胡萝卜当前位置。
rclcpp::Time when = this->get_clock()->now() - rclcpp::Duration(5, 0);
try {
t = tf_buffer_->lookupTransform(
toFrameRel,
fromFrameRel,
when,
50ms);
} catch (const tf2::TransformException & ex) {
然后,编译运行。会发现,在开始的5秒钟,第二只海龟不会运动,因为还没有胡萝卜姿态的5秒历史数据。但5秒后会发生什么?
可以看到海龟在不受控制的乱跑,背后原因是:
- 在代码中,我们询问了tf2这样一个问题:
carrot1
5秒前的姿态是什么,与之关联的turtle2
5秒前的姿态是什么?这也表明我们正在基于第二只海龟和第一个胡萝卜5秒前的位置来控制第二只海龟。 - 但是,我们真正想问的是:
carrot1
5秒前的姿态是什么,与之关联的turtle2
当前位置在哪?
lookupTransform()
的高级API
为了询问tf2上述第二点的特定问题,我们将使用一个高级API,这使我们能够明确地说明何时获得指定的转换。这可以通过使用lookupTransform()
的额外参数来完成。代码修改如下:
rclcpp::Time now = this->get_clock()->now();
rclcpp::Time when = now - rclcpp::Duration(5, 0);
try {
t = tf_buffer_->lookupTransform(
toFrameRel,
now,
fromFrameRel,
when,
"world",
50ms);
} catch (const tf2::TransformException & ex) {
这个lookupTransform()
高级API采用了6个参数:
- 目标坐标系target frame
- 转换时间
- 源坐标系source frame
- 源坐标系将被评估的时间点
- 不随时间改变的坐标系,本例中是
world
坐标系 - 等待目标坐标系可用的时间
综上所述,tf2在后台完成了以下工作。在过去,它计算从carrot1
到world
的坐标系转换。在world
坐标系中,tf2时间从过去到现在。在当前时间,tf2计算从world
到turtle2
的转换。
检查结果
编译功能包,再次运行仿真,会发现第二只海龟直接运动到胡萝卜5秒前的位置。
ros2 launch learning_tf2_cpp turtle_tf2_fixed_frame_demo_launch.py
总结
在本课程中,你见识了tf2的一个高级特点。tf2能够及时转换数据,并通过turtlesim示例学习了这一功能。tf2允许你回到过去并使用lookupTransform
API实现在海龟的旧姿态和当前姿态之间的坐标系转换。