文章目录
- TF坐标变换
- TF功能包
- TF工具
- 乌龟例程中的TF
- 创建TF广播器
- 创建TF监听器
- 实现乌龟跟随运动
- 存在的问题
- 总结
- 参考
TF坐标变换
坐标变换是机器人学中一个非常基础，同时也是非常重要的概念。机器人本体和机器人的工作环境中往往存在大量的组件元素，在机器人设计和机器人应用中都会涉及不同组件的位置和姿态，这就需要引入坐标系和坐标变换的概念。
对于A，B两个坐标系，A坐标系下的位姿可以通过平移和旋转变换成B坐标系下的位姿，这里的平移和旋转可以通过4×4的变换矩阵来描述。
坐标变换是机器人系统中常用的基础功能，ROS中的坐标变换系统由TF功能包维护。
TF功能包
TF是一个让用户随时间跟踪多个坐标系的功能包，它用树形数据结构，根据时间缓冲并维护多个坐标系之间的坐标变换关系，可以帮助开发者在任意时间，在坐标系间完成点，向量等坐标的变换。
一个机器人系统通常有很多三维坐标系，而且会随着时间的推移发生变化，如世界坐标系（Word Frame），基坐标系（Base Frame），机械夹爪坐标系（Gripper Frame），机器人头部坐标系（Head Frame）等。TF可以时间为轴跟踪这些坐标系（默认10s之内），并且允许开发者请求如下类型的数据：
- 5s之前，机器人头部坐标系相对于全局坐标系的关系是怎样的？
- 机器人夹取的物体相对于机器人中心坐标系的位置在哪里？
- 机器人中心坐标系相对于全局坐标系的位置在哪里？
TF可以在分布式系统中进行操作，也就是说，一个机器人系统中的所有坐标变换关系，对于所有的节点组件都是可用的，所有订阅TF消息的节点都会缓冲一份所有坐标系的变换关系数据，这种数据结构不需要中心服务器来存储任何数据。
想要使用TF功能包，总体来说需要以下两个步骤。
（1）监听TF变换
接收并缓存系统中发布的所有坐标变换数据，并从中查询所需要的坐标变化关系。
（2）广播TF变换
向系统中广播坐标系之间的坐标变换关系。系统中可能会存在多个不同部分的TF变换广播，每个广播都可以直接将坐标变换关系插入到TF树中，不需要再进行同步。
TF工具
坐标系统虽然是一个基础理论，但是由于涉及多个空间之间的变换，不容易进行想象，所以TF提供了丰富的终端工具来帮助开发者调试和创建TF变换。
1.tf_monitor
tf_monitor工具的功能是打印TF树中所有坐标系的发布状态，也可以通过输入参数来查看指定坐标系之间的发布状态。
tf_monitor
tf_monitor <source_frame> <target_frame>
2.tf_echo
tf_echo工具的功能是查看指定坐标系之间的变换关系，命令的格式如下：
tf_echo <source_frame> <target_frame>
3.tf_transform_publisher
tf_transform_publisher工具的功能是发布两个坐标系之间的静态坐标变换，这两个坐标系不发生相对位置变化。命令的合适如下：
tf_transform_publisher x y z yaw pitch roll frame_id child_frame_id period_in_ms
tf_transform_publisher x y z qx qy qz qw frame_id child_frame_id period_in_ms
以上两种命令格式，需要设置坐标的偏移参数和旋转参数：偏移参数使用相对于x，y，z三轴的坐标位移；而旋转参数的第一种命令格式使用以弧度为单位的yaw/pitch/roll角度（yaw是围绕z轴旋转的偏航角，pitch是围绕y轴旋转的俯仰角，roll是围绕x轴旋转的翻滚角），第二种命令格式使用四元数表达旋转角度。发布频率以ms为单位。
该命令不仅可以在终端中使用，还可以在launch文件中使用，方法如下：
<launch>
<node pkg="tf" type="tf_transform_publisher" name="link1_broadcaster"
args="1 0 0 0 0 0 1 link1_parent link1 100" />
</launch>
4.view_frames
view_frames是可视化的调试工具，可以生成pdf文件，显示整棵TF树的信息。执行方式为：
rosrun tf view_frames
然后使用以下命令，或者pdf阅读器查看生成的pdf文件。
evince frames.pdf
此外，rviz中也提供了TF可视化的插件。
乌龟例程中的TF
接下来我们在乌龟仿真器中通过一个例程（turtle_tf)来理解TF的作用，并且熟悉以上学到的TF工具。该例程的功能包turtle_tf可以使用如下命令进行安装：
sudo apt-get install ros-kinetic-turtle-tf
安装完成后使用如下命令运行例程：
roslaunch turtle_tf turtle_tf_demo.launch
乌龟仿真器打开后会出现两只小乌龟，并且下方的小乌龟会自动向中心位置的小乌龟移动。
打开键盘控制节点，控制中心位置的小乌龟运行：
rosrun turtlesim turtle_teleop_key
另外一只乌龟总会跟随我们控制的那只乌龟运行。在这个例程中，TF是如何运用的呢？首先，使用TF工具来查看以下这个例程中的TF树是什么样的：
rosrun tf view_frames
在当前系统中存在3个坐标系：word，turtle1，tutle2。word是世界坐标系，作为系统的基础坐标系，其他坐标系都相对该坐标系建立，所以word是TF树的根节点。相对于word坐标系，有分别针对两只乌龟创建了两个乌龟坐标系，这两个坐标系的原点就是乌龟在世界坐标系下的坐标位置。
现在要让turtle2跟随turtle1运动，等价于turtle2坐标系要向turtle1坐标系移动，这就需要知道turtle2与turtle1之间的坐标变换。三个坐标系之间的变换关系为：
T
t
u
r
t
l
e
1
_
t
u
r
t
l
e
2
=
T
t
u
r
t
l
e
1
_
w
o
r
d
×
T
w
o
r
d
_
t
u
r
t
l
e
2
T_{turtle1\_turtle2}=T_{turtle1\_word}×T_{word\_turtle2}
Tturtle1_turtle2=Tturtle1_word×Tword_turtle2
使用tf_echo工具在TF树中查找乌龟坐标系之间的变换关系：
rosrun tf tf_echo turtle1 turtle2
也可以通过rviz的图形界面更加形象地看到这三者之间的坐标关系：
rosrun rviz rviz -d 'rospack find turtle_tf'/rviz/turtle_rviz.rviz
得到turtle2与turtle1之间的坐标变换后，就可以通过计算两只乌龟间的距离和角度，即可控制turtle2向turtle1运动了。
接下来我i们以这个例程为目标，学习如何实现TF的广播和监听功能。
创建TF广播器
首选，我们需要创建一个发布乌龟坐标系与世界坐标系之间TF变换的节点，源码如下：
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>
std::string turtle_name;
void poseCallback(const turtlesim::PoseConstPtr& msg)
{
static tf::TransformBroadcaster br;
tf::Transform transform;
transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
tf::Quaternion q;
q.setRPY(0, 0, msg->theta);
transform.setRotation(q);
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "my_tf_broadcaster");
if (argc != 2)
{
ROS_ERROR("need turtle name as argument");
return -1;
};
turtle_name = argv[1];
ros::NodeHandle node;
ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
ros::spin();
return 0;
};
以上代码的关键部分是处理乌龟pose消息的回调函数poseCallback，在广播TF消息之前要定义tf::TransformBroadcaster广播器，然后根据乌龟当前的位姿设置tf::Transform类型的坐标变换，包含setOrigin设置的平移变换，setRotation设置的旋转变换。
然后使用广播器将坐标变换插入TF树并进行发布，这里发布的TF消息类型是tf::StampedTransform，不仅包含tf::Transform类型的坐标变换、时间戳，而且需要指定坐标变换的源坐标系（parent）和目标坐标系（child）。
创建TF监听器
TF消息广播之后，其他节点就可以监听该TF消息，从而获取需要的坐标变换了。目前我们已经将乌龟相对于world坐标系的TF变换进行了广播，接下来需要监听TF消息，并从中获取turtle2相对于turtle1坐标系的变换，从而控制turtle2移动。源码如下：
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>
int main(int argc, char** argv)
{
ros::init(argc, argv, "my_tf_listener");
ros::NodeHandle node;
ros::service::waitForService("spawn");
ros::ServiceClient add_turtle =
node.serviceClient<turtlesim::Spawn>("spawn");
turtlesim::Spawn srv;
add_turtle.call(srv);
ros::Publisher turtle_vel =
node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);
tf::TransformListener listener;
ros::Rate rate(10.0);
while (node.ok())
{
tf::StampedTransform transform;
try
{
listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0));
listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
}
catch (tf::TransformException &ex)
{
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
continue;
}
geometry_msgs::Twist vel_msg;
vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),
transform.getOrigin().x());
vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
pow(transform.getOrigin().y(), 2));
turtle_vel.publish(vel_msg);
rate.sleep();
}
return 0;
};
该节点首先通过服务调用产生乌龟turtle2，然后声明控制turtle2速度的Publisher。在监听TF消息之前，需要创建一个tf::TransformListener类型的监听器，创建成功后监听器会自动接收TF树的消息，并且缓存10s。然后在循环中就可以实时查找TF树中的坐标变换了，这里需要调用的是tf::TransformListener中的两个接口：
- waitForTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration &timeout)：给定源坐标系和目标坐标系，等待两个坐标系指定时间的变换关系，该函数会阻塞程序运行，因此要设置超时时间。
- lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform)：给定源坐标系和目标坐标系，得到两个坐标系指定时间的变换关系，ros::Time(0)表示我们想要得到最新一次的坐标变换。
通过以上两个接口的调用，就可以获取turtle2相对于turtle1的坐标变换了。然后根据坐标系之间的位置关系，计算得到turtle2需要运动的线速度和角速度，并发布速度控制指令使turtle2向turtle1移动。
实现乌龟跟随运动
编写一个launch文件，使所有节点运行起来，源码：
<launch>
<!-- 海龟仿真器 -->
<node pkg="turtlesim" type="turtlesim_node" name="sim"/>
<!-- 键盘控制 -->
<node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
<!-- 两只海龟的tf广播 -->
<node pkg="learning_tf" type="turtle_tf_broadcaster"
args="/turtle1" name="turtle1_tf_broadcaster" />
<node pkg="learning_tf" type="turtle_tf_broadcaster"
args="/turtle2" name="turtle2_tf_broadcaster" />
<!-- 监听tf广播，并且控制turtle2移动 -->
<node pkg="learning_tf" type="turtle_tf_listener"
name="listener" />
</launch>
存在的问题
看理论基本都能明白，但是通过实践，我发现程序并不能实现想要的效果。
启动launch文件后，打开了一个乌龟仿真器，但里面只有一个乌龟，在终端按方向键能控制其运动，但是没有另一只乌龟做跟随运动。
而且在上一个乌龟例程中，乌龟2没有一直跟随乌龟1运动。
这里先记录以下，不知道是程序问题，还是我操作的问题。
总结
通过这个例程的实现，我们学习了TF广播与监听的实现方法，在实际使用中会产生更多的坐标系，TF树的结构也会更加复杂，但是基本的使用方法依然相同。
最近遇到TF的问题，发现对其理解不够深入，所以重温了古月居编写的《ROS机器人开发实践》，这部分内容写得通俗易懂，在此向古月居大佬致敬。
参考
[1]《ROS机器人开发实践》胡春旭 编著
[2] https://github.com/huchunxu/ros_exploring
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)
