[ROS]Setting up your robot using tf

###1.Transform Configuration

许多 ROS 包需要机器人通过 tf 软件库发布的转换树.从一个抽象的等级来看,一个转换树定义了不同坐标系中的平移和旋转的偏移.为了使得描述更真实,考虑这样一个例子,一个简单的机器人具有一个顶上装有单一激光传感器的移动底盘.提到机器人,我们定义了两个坐标系:一个对应机器人底盘的中心,一个对应激光传感器的中心.让我们给这两个坐标系取个名字方便引用.移动底盘上的坐标系叫做"base_link"(对于导航,这个坐标系放置在中心是非常重要的),激光上的坐标系称为"base_laser".对于参考系的命名规范,参考:http://www.ros.org/reps/rep-0105.html.

这里,我们假设我们拥有以距离激光传感器中心长度的激光数据.另一方面,也拥有一些"base_laser"坐标系数据.现在假设我们想得到这些数据并使用他们来帮助移动底盘全局避障.为了成功的实现我们的目标,我们需要一种方式来转换扫描到的激光从"base_laser"坐标系到"base_link"坐标系.从本质上来说,我们需要定义"base_laser"坐标系到"base_link"坐标系之间的关系.

为了定义这个关系,我们假设我们之道激光传感器安装在移动底盘前面 10cm,上面 20cm.这个给出了"base_link"坐标系到"base_laser"坐标系的平移的偏移量.具体地,我们知道将得到的数据从"base_link"到"base_laser"的转换需要(x: 0.1m, y: 0.0m, z: 0.2m)的平移,从"base_laser"到"base_link"的转换需要(x: -0.1m, y: 0.0m, z: -0.20m)的平移.

我们可以选择自己去管理这个转换关系,这意味着在必要的时候完成保存和应用这个坐标系之间适当的转换,但是随着坐标系的增加将会变得很痛苦.然而幸运地,我们没必要自己去完成这部分工作.取儿代之的是,我们将使用 tf 以及让它来管理两个坐标系之间的转换的方式来一次性的定义"base_link"坐标系和"base_laser"坐标系之间关系.

为了使用 tf 来定义和保存"base_link"坐标系和"base_laser"坐标系之见的关系,我们需要将他们添加到转换树上去.概念地,转换树中的每一个节点对应了一个坐标系,每一条边对应需要应用到从当前节点移动到它的子节点的转换.tf 使用一个树结构,保证连接任意两个坐标系只有一个单独的连接,并假设树中所有的边定向的只从父节点到子节点.

为了为简单的例子创建一个转换树,我们将创建两个节点,一个用于"base_link"坐标系,一个用于"base_laser"坐标系.为了创建他们之间的转换关系,我们首先需要决定哪一个节点作为父节点,哪一个作为子节点.记住,这个差别很重要,因为 tf 假设所有的转换是从父节点到子节点.让我们选择"base_link"坐标系作为父节点,因为其他的传感器都是添加到机器人上的,这将使得有关"base_link"坐标系转换的"base_laser"坐标系变得具有极大的意义.这意味着连接"base_link"坐标系和"base_laser"坐标系的转换应该为(x: 0.1m, y: 0.0m, z: 0.2m).随着转换树的建立,转换"base_laser"坐标系中接收到的激光扫描器数据到"base_link"坐标系变得跟调用 tf 类库一样简单.我们的机器人能够安全地规划绕过环境中的障碍物的原因是因为可以使用"base_laser"坐标系中的激光扫描器的信息的原因.

###2.Writing Code

希望以上的例子可以帮助你在概念上理解 tf.现在我们将得到这转换树并通过代码创建它.对于这个例子,假设你对 ROS 很熟悉.如果你对 ROS 的任何概念不熟悉的话,查找 ROS 文档:http://wiki.ros.org/ROS.

假设我们已经完成了上面描述的高等级的任务:"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 版本,现有 navigation_tutorials 中有一个标准的 robot_setup_tf_tutorial 包.可以通过下面的命令进行安装:

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

###3.Broadcasting a Transform

在创建好包之后,需要创建节点用来通过 ROS 广播 base_laser->base_link 的转换.在刚刚创建的 robot_setup_tf 包中,选择习惯的编辑器,将下面的代码拷贝进 src/tf_broadcaster.cpp 文件中.

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>

int main(int argc, char** argv){
  ros::init(argc, argv, "robot_tf_publisher");
  ros::NodeHandle n;

  ros::Rate r(100);

  tf::TransformBroadcaster broadcaster;

  while(n.ok()){
    broadcaster.sendTransform(
      tf::StampedTransform(
        tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.1, 0.0, 0.2)),
        ros::Time::now(),"base_link", "base_laser"));
    r.sleep();
  }
}

接下来详细的介绍 base_link->base_laser 之间的转换的代码.

#include <tf/transform_broadcaster.h>

tf 包提供一个 tf::TransformBroadcaster 的实现用来使得发布转换任务变得更简单.在使用 TransformBroadcaster 之前,需要包含 tf/transform_broadcaster.h 头文件.

tf::TransformBroadcaster broadcaster;

这里我们创建一个 TransformBroadcaster 对象用于接下来发布 base_link 到 base_laser 的转换.

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

上面的代码完成了真实的转换.使用 TransformBroadcaster 对象发布一个转换需要 5 个参数.首先,我们传递一个旋转的转换,发生在任意两个坐标系之间的转换由 btQuaternion 指定.这里不进行旋转,所以 btQuaternion 的构造形参(pitch,roll,yaw)都为 0.第二个参数,btVector3 中 x 轴是激光传感器相对底盘偏移 10cm,Z 轴偏移 20cm.第三个参数,我们需要为这个转换设置一个发布时间,这里设置这个时间戳为 ros::Time::new().第四个参数,需要指定父节点,这里是"base_link".第五个参数我们需要设置子节点,这里是"base_laser".

###4.Using a Transform

以上,我们创建了一个节点通过 ROS 发布 base_laser → base_link 的转换.现在,我们将编写一个节点用来将 base_laser 坐标系中的点转换到 base_link 坐标系中的点.在 robot_setup_tf 包中创建 src/tf_listener.cpp 文件,并复制以下内容:

#include <ros/ros.h>
#include <geometry_msgs/PointStamped.h>
#include <tf/transform_listener.h>

void transformPoint(const tf::TransformListener& listener){
  //we'll create a point in the base_laser frame that we'd like to transform to the base_link frame
  geometry_msgs::PointStamped laser_point;
  laser_point.header.frame_id = "base_laser";

  //we'll just use the most recent transform available for our simple example
  laser_point.header.stamp = ros::Time();

  //just an arbitrary point in space
  laser_point.point.x = 1.0;
  laser_point.point.y = 0.2;
  laser_point.point.z = 0.0;

  try{
    geometry_msgs::PointStamped base_point;
    listener.transformPoint("base_link", laser_point, base_point);

    ROS_INFO("base_laser: (%.2f, %.2f. %.2f) -----> base_link: (%.2f, %.2f, %.2f) at time %.2f",
        laser_point.point.x, laser_point.point.y, laser_point.point.z,
        base_point.point.x, base_point.point.y, base_point.point.z, base_point.header.stamp.toSec());
  }
  catch(tf::TransformException& ex){
    ROS_ERROR("Received an exception trying to transform a point from \"base_laser\" to \"base_link\": %s", ex.what());
  }
}

int main(int argc, char** argv){
  ros::init(argc, argv, "robot_tf_listener");
  ros::NodeHandle n;

  tf::TransformListener listener(ros::Duration(10));

  //we'll transform a point once every second
  ros::Timer timer = n.createTimer(ros::Duration(1.0), boost::bind(&transformPoint, boost::ref(listener)));

  ros::spin();

}

代码的分析如下:

#include <tf/transform_listener.h>

这里,我们包含了 tf/transform_listener.h 头文件,之后将可以创建 tf::TransforListener 对象了.一个 TransformListener 对象自动通过 ROS 向转换消息主题订阅并管理消息使得他们顺序进入.

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

上面的代码创建了一个函数,给定一个 TransformListener 对象,得到 base_laser 坐标系上的点并将它转换为 base_link 坐标系上的点.这个函数将在程序的 main()方法中在 ros::Timer 创建的时候作为一个回调被每秒调用一次.

  //we'll create a point in the base_laser frame that we'd like to transform to the base_link frame
  geometry_msgs::PointStamped laser_point;
  laser_point.header.frame_id = "base_laser";

  //we'll just use the most recent transform available for our simple example
  laser_point.header.stamp = ros::Time();

  //just an arbitrary point in space
  laser_point.point.x = 1.0;
  laser_point.point.y = 0.2;
  laser_point.point.z = 0.0;

这里,我们将创建一个 geometry_msgs::PointStamped 类型的点.末尾的"Stamped"仅仅意味这它包括一个 header,允许我们将消息联系到 timestamp 和 frame_id.这里设置 laser_point 消息的 stamp 属性的值为 ros::Time().ros::Time 是一个具体的时间值,它允许我们访问 TransformListener 得到最新可用的转换.header 中的 frame_id 属性的值,我们设置为 base_laser,是因为我们创建的是 base_laser 坐标系中的点.最后将设置一些节点的数据...

  try{
    geometry_msgs::PointStamped base_point;
    listener.transformPoint("base_link", laser_point, base_point);

    ROS_INFO("base_laser: (%.2f, %.2f. %.2f) -----> base_link: (%.2f, %.2f, %.2f) at time %.2f",
        laser_point.point.x, laser_point.point.y, laser_point.point.z,
        base_point.point.x, base_point.point.y, base_point.point.z, base_point.header.stamp.toSec());
  }

现在既然我们有了 base_laser 坐标系中的点,之后想把它转换到 base_link 坐标系.为了达成这个目标, 我们将使用 TransformListener 对象,transformPoint()方法有三个参数:需要转换的目标坐标系的名字, 需要转换的点,保存转换之后的点.所以,在调用 transformPoint()之后,base_point 保存了跟 laser_point 之前一样的信息,现在这些信息在"base_link"坐标系中.

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

如果有默写原因导致 base_laser 到 base_link 的转换不可用(比如说 tf_broadcaster 没有运行),调用 transformPoint()方法的时候 TransformListener 对象可能会抛出异常.为确定我们捕获了这个异常,我们将使用 catch 来捕获这个异常并打印出这个错误信息.

###5.Building the Code

Now that we've written our little example, we need to build it. Open up the CMakeLists.txt file that is autogenerated by roscreate-pkg and add the following lines to the bottom of the file.
现在我们已经写出了简单的例子,我们需要构建它.打开 roscreate-pkg 自动生成的 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})

保存文件并构建包:

 cd %TOP_DIR_YOUR_CATKIN_WS%
 catkin_make

###6.Running the Code

下面我们将看看上面的代码执行了什么工作,启动三个命令行窗口.

第一个命令行执行:

roscore

第二个命令行执行:

rosrun robot_setup_tf tf_broadcaster

Great. Now we'll use the third terminal to run our tf_listener to transform our mock point from the "base_laser" frame to the "base_link" frame.
第三个命令行窗口用来运行 tf_listener 用来模拟将 base_laser 中的点转换到 base_link 中的点.

rosrun robot_setup_tf tf_listener

运行之后可以看到转换成功之后的信息:

[ 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

以上,就完成了二维平面上的点转换广播器.