ROS Navigation-----TF配置

    下文主要介绍如何用TF设置机器人。

1 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”坐标系中就可以用传感器扫描出的数据做出推理，安全的规划路径来绕过障碍物。

2 Writing Code（代码编写）

    首先，我们需要创建一个node，在我们机器人系统发布坐标系转换关系。其次，我们还必须创建一个node，来监听发布出来的这种转换数据，并利用这种数据进行坐标转换。我们可以在某个目录创建一个源码包，同时命名“robot_setup_tf”。添加依赖包 roscpp, tf, geometry_msgs。

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

  另一个可选办法是利用navigation_tutorials的标准robot_setup_tf_tutorial包，可以通过如下命令安装，

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

3 Broadcasting a Transform（广播变换）

    至此，我们已经创建了package。下面需要创建节点，来广播转换（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();
  }
}

现在对上面的代码作更细节的解释。

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

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

3）tf::StampedTransform（）是关键部分。通过 TransformBroadcaster来发送转换关系，需要附带5个参数。

第1个参数，传递旋转变换，在两个坐标系之间任意旋转变换，都必须调用btQuaternion.现在情况下没有旋转，所以我们在调用btQauternion的时候，将pitch,roll,yaw的参数都置0；第2个参数，btVector3，任何平移都需要调用它。本例中我们确实有一个平移，所以我们调用了btVector3，相应的传感器的x方向距离机体基准偏移10cm，z方向20cm；

第3个参数，我们需要给转换关系携带一个时间戳，我们标记为ros::Time::now()；

第4个参数，我们需要传递parent节点的名字；

第5个参数，传递的是child节点的名字；

4 Using a Transform（调用变换）

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

#include 
#include 
#include 

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

}

1）我们包含tf/transform_listener.h头文件，是为了后边创建tf::TransformListener。

一个TransformListener目标会自主订阅变换消息的主题，同时管理所有的该通道上的变换数据。

2）创建一个函数transformPoint（），参数为TransformListener,作用为将“base_laser”坐标系的点，变换到“base_link”坐标系中表示。这个函数将会以ros::Timer定义的周期，作为一个回调函数周期调用。目前周期是1s。

3）时间timer回调函数中，我们创建一个虚拟点，作为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 。

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

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

 

5 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})

然后编译。

6 Running the Code（代码运行）

第一个窗口，运行master：

roscore

第二个，运行 tf_broadcaster

rosrun robot_setup_tf tf_broadcaster

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

rosrun robot_setup_tf tf_listener

如果一切顺利，每隔1s就会输出信息。

注意：以上译自ROS wiki