用tf设置机器人

用tf设置机器人

tf的功能主要就是用来定义，存储、管理各种坐标系之间的转换关系。使用tf假定已经在机器人上安装了ROS。

转换配置

先设定这样一个场景，有一辆机器人小车，它的顶部前段安装了一个激光扫描仪，如下图.激光扫描仪在机器人基座中心前方10cm，上方20cm处。

现在定义两个坐标系统。一个以机器人基座中心为坐标系中心，叫做base_link，另一个以激光扫描仪为坐标系中心，叫做base_laser.通常情况下机器人需要使用机关扫描仪获取的数据信息，辅助于对机器人的控制。也就是base_link坐标系需要用上base_laser坐标系的数据信息，但由于坐标系的不同，我们需要进行转换之后才能使用，即base_link坐标系进行(0.2m,0m,0.1m)的平移之后能够得到base_laser坐标系。

在两种不同的坐标系上的数据进行转换时都会有固定的转换关系，这种转换关系可以由我们人工进行管理，这就意味着我们必须定义，存储和应用转换关系来完成不同坐标系之间的数据转换。但是当这种转换关系数量剧增的时候，人工管理将变得麻烦。不过有幸的是，我们可以通过tf软件工具定义不同坐标系之间的转换关系，然后通过tf软件工具来存储、管理坐标系间数据的转换。

要用tf软件来定义和存储各种转换关系，我们需要将转换关系添加到转换树中。从概念上来说，就是每一个节点对应一个坐标系，连接节点的边对应每一种转换关系，转换树中的边默认都是从父节点到子节点的转换关系。

建立转换树之后，两个坐标系间数据的转换就是简单地通过调用一下tf库而已，这使得一个坐标系可以很轻松地通过转换使用其它坐标系的数据信息。

代码编写

前面一部分我们是从概念上了解了tf的功能和作用。接下来我么就以上述的例子来实际地感受一下代码的编写过程。

我们假设在base_laser上有一个点，我们需要将其转换到base_link上。首先，我们需要创建一个节点，其负责发布坐标间的转换关系。之后，我们需要创建另一个节点，其负责监听坐标间的转换关系并应用于具体的数据转换。

我们先创建一个源码包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上广播由base_laser到base_link的转换关系的节点。在刚才创建的robot_setup_tf/src目录中创建一个tf_broadcaster.cpp文本文件，将以下代码拷贝进去。

#include 
#include 

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();
  }
}

我们来看一下代码细节。

   2 #include 

tf软件提供了tf::TransformBroadcaster的实现使得我们发布转换关系变得简单。要使用TransformBroadcaster，我们需要包含上述头文件。

  10   tf::TransformBroadcaster broadcaster;

这里，我们创建了一个TransformBroadcaster对象，后面的base_link->base_laser转换发布需要用上。

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

这里是发挥主要作用的代码。发送一种转换需要5个参数。第一个，我们需要传入旋转变换，它由btQuaternion对象指定，可以实现两个坐标系间的任何旋转变换。在我们这个例子中，并没有涉及旋转变换，所以构造了pitch,roll,yaw值都为0的Quaternion对象；第二个，一个btVector3对象，它实现两个坐标系间的平移变换，这里base_laser相对于base_linkx方向的偏移量为0.1m，y方向上的偏移量为0m，z方向上的偏移量为0.2m；第三个，我们需要指定一个转换关系发布的时间戳，这里指定为ros::Time::now()；第四个，指明转换关系的父节点；第五个，指明转换关系的子女节点。

使用一种转换

接下来，我们需要创建一个监听转换关系已经应用转换关系的节点。同样，在刚才创建的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();

}

接下来解释一下代码含义。

   3 #include 

后面需要用上tf::TransformListener.一个TransformListener对象自动订阅ROS上的转换信息并管理所有的这些转换信息。

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

这是我们创建的函数，给定一个TransformListener对象，在base_laser上模拟一个点，然后将其转换到base_link上。这个函数会被在main函数中创建的ros::Timer调用，并且每秒钟启动一次。

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

这里，我们模拟了一个点数据，为geometry_msgs::PointStamped类型。”Stamped”放在消息名字的末尾只是意味着它定义的对象包含一个信息头，允许我们将一个具体的timestamp和frame_id与数据信息（这里对应的就是点数据）关联。我们将laser_point消息的stamp字段设为ros::Time(),这是一个特殊的时间意味着让TransformListener订阅最新的有效转换。frame_id字段，我们将其设为base_laser，因为我们模拟的点数据就在base_laser坐标系上。最后，我们设置点的据体坐标x:1.0, y:0.2, z:0.0.

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

这里是将base_laser上的点数据转换到base_link上的代码。我们使用TransformListener对象，并调用它的方法transformPoint()函数。方法有3个参数：第一个，点数据转换的目标坐标系；第二个，我们正要转换的点数据；第三个，存储转换之后的目标点数据。这个方法调用之后，base_point将会存有之前只在基于base_laser坐标系上的base_laser才有的点坐标信息。

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

这里的代码处理一些异常情况。例如，转换不可用（可能是因为tf_broadcaster没有允许），当我们调用tranformPoint()的时候TransformListener就会抛出异常。

构建代码

上面写的是发布转换和监听转换的源代码，接下来需要让它们能够像节点一样运行，需要添加必要的编译信息并进行编译构建。

添加编译信息，打开robot_setup_tf下的CMakeLists.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

运行代码

第一个终端，运行roscore.

roscore

第二个终端，运行tf_broadcaster.

rosrun robot_setup_tf tf_broadcaster

第三个终端，运行tf_listener.

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

目前为止，我们已经成功地为平面激光仪写好了转换广播器。下一步要做的就是将上述中的PointStamped替换成ROS接受到的其它传感器流，来实现不同数据的转换。

参考网站： http://wiki.ros.org/navigation/Tutorials/RobotSetup/TF