ROS 导航功能包为你的机器人配置 tf ROS

cn/navigation/Tutorials/RobotSetup/TF

Note: This tutorial assumes that you have completed the ROS Tutorials and the tf basic tutorials. The code for this tutorial is available in the robot_setup_tf_tutorial package..

文章图片
Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

为你的机器人配置 tf Description: This tutorial provides a guide to set up your robot to start using tf.

Tutorial Level: BEGINNER

Next Tutorial: Learn how to have the robot state publisher do all the tf publishing for you: using the robot state publisher on your own robot
目录

Transform Configuration（变换配置）
Writing Code（代码编写）
Broadcasting a Transform（广播变换）
Using a Transform（调用变换）
Building the Code代码构建
Running the Code（代码运行）

Transform Configuration（变换配置）许多ROS功能包，都要求利用tf软件库，以机器人识别的变换树的形式进行发布。抽象层面上，变换树其实就是一种“偏移”，代表了不同坐标系之间的变换和旋转。更具体点来说，设想一个简单的机器人，只有一个基本的移动机体和挂在机体上方的扫描仪。基于此，我们定义了两个坐标系：一个对应于机体中心点的坐标系，一个对应于扫描仪中心的坐标系。分别取名为“base_link”和“baser_laser”。关于坐标系的命名习惯，参考REP 105.
此时，可以假设，我们已经从传感器获取了一些数据，以一种代表了物体到扫描仪中心点的距离的形式给出。换句话说，我们已经有了一些“base_laser”坐标系的数据。现在，我们期望通过这些数据，来帮助机体避开物理世界的障碍物。成功的关键是，我们需要一种方式，把传感器扫描的数据，从“base_laser”坐标系转换到“base_link”坐标系中去。本质上，就是定义一种两个坐标系的“关系”。

文章图片

为了定义这种关系，假设我们知道，传感器是挂在机体中心的前方10cm，高度20cm处。这就等于给了我们一种转换的偏移关系。具体来说，就是，从传感器到机体的坐标转换关系应该为(x:0.1m,y:0.0m, z:0.2m),相反的转换即是(x:-0.1m,y:0.0m,z:0.2m)。
我们可以选择去自己管理这种变换关系，意味着需要自己去保存，以及在需要的时候调用。但是，这种做法的缺陷即是随着坐标转换关系数量的增加，而愈加麻烦。幸运的是，我们也没有必要这么干。相反，我们利用tf定义了这么一种转换关系，那么就让它来帮我们管理这种转换关系吧。
利用tf来管理这种关系，我们需要把他们添加到转换树（transform tree）中。一方面来说，转换树中的每一个节点都对应着一类坐标系，节点之间的连线即是两个坐标相互转换的一种表示，一种从当前节点到子节点的转换表示。Tf利用树结构的方式，保证了两个坐标系之间的只存在单一的转换，同时假设节点之间的连线指向是从parent到child。

文章图片

基于我们简单的例子，我们需要创建两个节点，一个“base_link”，一个是“base_laser”。为了定义两者的关系，首先，我们需要决定谁是parent，谁是child。时刻记得，由于tf假设所有的转换都是从parent到child的，因此谁是parent是有差别的。我们选择“base_link”坐标系作为parent，其他的传感器等，都是作为“器件”被添加进robot的，对于“base_link”和“base_laser”他们来说，是最适合的。这就意味着转换关系的表达式应该是(x:0.1m,y0.0m,z:0.2m)。关系的建立，在收到“base_laser”的数据到“base_link”的转换过程，就可以是简单的调用tf库即可完成。我们的机器人，就可以利用这些信息，在“base_link”坐标系中，就可以推理出传感器扫描出的数据，并可安全的规划路径和避障等工作。

Writing Code（代码编写）希望上面的例子，一定程度上可以帮助大家理解tf。现在，我们可以建立通过代码来实现转换树。对于这个例子来说，前提是熟悉ROS，所以如果不熟悉，先预习下ROS Documentation吧。
假定，我们以上层来描述“base_laser”坐标系的点，来转换到"base_link"坐标系。首先，我们需要创建节点，来发布转换关系到ROS系统中。下一步，我们必须创建一个节点，来监听需要转换的数据，同时获取并转换。在某个目录创建一个源码包，同时命名“robot_setup_tf”。添加依赖包roscpp,tf,geometry_msgs。

$ cd %TOP_DIR_YOUR_CATKIN_WS%/src $ catkin_create_pkg robot_setup_tf roscpp tf geometry_msgs

至此，你必须运行上面的命令，当然你必须有必要的权限（例如，~/ros目录下，你可能在之前的文档中操作过这个目录）
另外，在 fuerte, groovy 和 hydro 中，有一个标准的 robot_setup_tf_tutorial 软件包，在 navigation_tutorials 中。你可以按如下方法安装他们 (%YOUR_ROS_DISTRO% 可以是 { fuerte, groovy } den):

$ sudo apt-get install ros-%YOUR_ROS_DISTRO%-navigation-tutorials

Broadcasting a Transform（广播变换）至此，我们已经创建了package。我们需要创建对于的节点，来实现广播任务base_laser->base_link。在robot_setup_tf包中，用你最喜欢的编辑器打开，然后将下面的代码粘贴到src/tf_broadcaster.cpp文件中去。

1 #include 2 #include 3 4 int main(int argc, char** argv){ 5ros::init(argc, argv, "robot_tf_publisher"); 6ros::NodeHandle n; 7 8ros::Rate r(100); 9 10tf::TransformBroadcaster broadcaster; 11 12while(n.ok()){ 13broadcaster.sendTransform( 14tf::StampedTransform( 15tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2)), 16ros::Time::now(),"base_link", "base_laser")); 17r.sleep(); 18} 19 }

现在，让我们来针对上面的代码，作更细节的解释。

2 #include 3

Tf功能包提供了一种实现tf::TransformBroadcaster ，使任务发布变换更容易。为了调用TransformBroadcaster, 我们需要包含 tf/transform_broadcaster.h 头文件.

10tf::TransformBroadcaster broadcaster;

我们创建一个TransformBroadcaster对象，之后我们可以利用他来发送变换关系，即base_link → base_laser。

13broadcaster.sendTransform( 14tf::StampedTransform( 15tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2)), 16ros::Time::now(),"base_link", "base_laser"));

这部分是关键部分。通过TransformBroadcaster来发送转换关系，需要附带5个参数。第1个参数，我们传递了旋转变换，在两个坐标系的发送的任意旋转，都必须通过调用btQuaternion.现在情况下，我们不想旋转，所以我们在调用btQauternion的时候，将pitch,roll,yaw的参数都置0.第2个参数，btVector3，任何变换过程都需要调用它。无论怎样，我们确实需要做一个变换，所以我们调用了btVector3，相应的传感器的x方向距离机体基准偏移10cm，z方向20cm。第3个参数，我们需要给定转换关系携带一个时间戳，我们标记为ros::Time::now()。第4个参数，我们需要传递parent节点的名字。第5个参数，传递的是child节点的名字。

Using a Transform（调用变换）上面的代码，我们创建了一个节点来发布转换关系，baser_laser->base_link。现在，我们需要利用转换关系，将从传感器获取的数据转换到机体对应的数据，即是“base_laser”->到“base_link”坐标系的转换。下面的代码，后边会紧根更详细的解析。在robot_setup_if功能包中，在src目录下创建tf_listener.cpp，并将下面的代码粘贴到里面：

1 #include 2 #include 3 #include 4 5 void transformPoint(const tf::TransformListener& listener){ 6//we'll create a point in the base_laser frame that we'd like to transform to the base_link frame 7geometry_msgs::PointStamped laser_point; 8laser_point.header.frame_id = "base_laser"; 9 10//we'll just use the most recent transform available for our simple example 11laser_point.header.stamp = ros::Time(); 12 13//just an arbitrary point in space 14laser_point.point.x = 1.0; 15laser_point.point.y = 0.2; 16laser_point.point.z = 0.0; 17 18try{ 19geometry_msgs::PointStamped base_point; 20listener.transformPoint("base_link", laser_point, base_point); 21 22ROS_INFO("base_laser: (%.2f, %.2f. %.2f) -----> base_link: (%.2f, %.2f, %.2f) at time %.2f", 23laser_point.point.x, laser_point.point.y, laser_point.point.z, 24base_point.point.x, base_point.point.y, base_point.point.z, base_point.header.stamp.toSec()); 25} 26catch(tf::TransformException& ex){ 27ROS_ERROR("Received an exception trying to transform a point from \"base_laser\" to \"base_link\": %s", ex.what()); 28} 29 } 30 31 int main(int argc, char** argv){ 32ros::init(argc, argv, "robot_tf_listener"); 33ros::NodeHandle n; 34 35tf::TransformListener listener(ros::Duration(10)); 36 37//we'll transform a point once every second 38ros::Timer timer = n.createTimer(ros::Duration(1.0), boost::bind(&transformPoint, boost::ref(listener))); 39 40ros::spin(); 41 42 }

好的，现在让我们将上面代码的重点步步分解：

3 #include 4

这里，我们包含tf/transform_listener.h头文件，是为了后边创建tf::TransformListener。一个TransformListener目标会自动的订阅ROS系统中的变换消息主题，同时管理所有的该通道上的变换数据。

5 void transformPoint(const tf::TransformListener& listener){

【ROS 导航功能包为你的机器人配置 tf】创建一个函数，参数为TransformListener,作用为将“base_laser”坐标系的点，变换到“base_link”坐标系中。这个函数将会以ros::Timer定义的周期，作为一个回调函数周期调用。目前周期是1s。

6//we'll create a point in the base_laser frame that we'd like to transform to the base_link frame 7geometry_msgs::PointStamped laser_point; 8laser_point.header.frame_id = "base_laser"; 9 10//we'll just use the most recent transform available for our simple example 11laser_point.header.stamp = ros::Time(); 12 13//just an arbitrary point in space 14laser_point.point.x = 1.0; 15laser_point.point.y = 0.2; 16laser_point.point.z = 0.0;

此处，我们创建一个虚拟点，作为geometry_msgs::PointStamped。消息名字最后的“Stamped”的意义是，它包含了一个头部，允许我们去把时间戳和消息的frame_id相关关联起来。我们将会设置laser_point的时间戳为ros::time(),即是允许我们请求TransformListener取得最新的变换数据。对于header里的frame_id，我们设置为“base_laser”,因为我们是创建的是扫描仪坐标系里的虚拟点。最后，我们将会设置具体的虚拟点，比如x:1.0,y:0.2,z:0.0

18try{ 19geometry_msgs::PointStamped base_point; 20listener.transformPoint("base_link", laser_point, base_point); 21 22ROS_INFO("base_laser: (%.2f, %.2f. %.2f) -----> base_link: (%.2f, %.2f, %.2f) at time %.2f", 23laser_point.point.x, laser_point.point.y, laser_point.point.z, 24base_point.point.x, base_point.point.y, base_point.point.z, base_point.header.stamp.toSec()); 25}

至此，我们已经有了从“base_laser”到“base_link”变换的点数据。进一步，我们通过TransformListener对象，调用transformPoint(),填充三个参数来进行数据变换。第1个参数，代表我们想要变换的目标坐标系的名字。第2个参数填充需要变换的原始坐标系的点对象，第3个参数填充，目标坐标系的点对象。所以，在函数调用后，base_point里存储的信息就是变换后的点坐标。

26catch(tf::TransformException& ex){ 27ROS_ERROR("Received an exception trying to transform a point from \"base_laser\" to \"base_link\": %s", ex.what()); 28}

如果某些其他的原因，变换不可得（可能是tf_broadcaster 挂了），调用transformPoint()时，TransformListener调用可能会返回异常。为了体现代码的优雅性，我们将会截获异常并把异常信息呈现给用户。

Building the Code代码构建至此，根据我们写的例子，接下来我们需要构建编译。打开CMakeList.txt，在文件末尾添加下面的几行：

add_executable(tf_broadcaster src/tf_broadcaster.cpp) add_executable(tf_listener src/tf_listener.cpp) target_link_libraries(tf_broadcaster ${catkin_LIBRARIES}) target_link_libraries(tf_listener ${catkin_LIBRARIES})

Next, make sure to save the file and build the package.

$ cd %TOP_DIR_YOUR_CATKIN_WS% $ catkin_make

Running the Code（代码运行）好的，万事俱备只欠东风！让我恩试着实际运行下吧。这部分，你需要开三个终端窗口
第一个窗口，运行core

roscore

第二个，运行 tf_broadcaster

rosrun robot_setup_tf tf_broadcaster

好的。现在，我们会在第三个窗口运行tf_listener，将从传感器坐标系获取的虚拟点，变换到机体坐标系。

rosrun robot_setup_tf tf_listener

如果一切顺利，应该会看到类似的结果。每次打印间隔1s。

[ INFO] 1248138528.200823000: base_laser: (1.00, 0.20. 0.00) -----> base_link: (1.10, 0.20, 0.20) at time 1248138528.19 [ INFO] 1248138529.200820000: base_laser: (1.00, 0.20. 0.00) -----> base_link: (1.10, 0.20, 0.20) at time 1248138529.19 [ INFO] 1248138530.200821000: base_laser: (1.00, 0.20. 0.00) -----> base_link: (1.10, 0.20, 0.20) at time 1248138530.19 [ INFO] 1248138531.200835000: base_laser: (1.00, 0.20. 0.00) -----> base_link: (1.10, 0.20, 0.20) at time 1248138531.19 [ INFO] 1248138532.200849000: base_laser: (1.00, 0.20. 0.00) -----> base_link: (1.10, 0.20, 0.20) at time 1248138532.19

祝贺你，你已经成功的编写了一个针对平面传感器的坐标变换。下一步就是替换PointStamped，来使用真正的传感器进行操作。幸运的是，已经有相关的指导文档了here。