10.SLAM和导航

1. 为机器人配置TF

1.1 变换配置

许多ROS软件包都要使用TF软件功能包，以机器人识别的变换树的形式发布。抽象层面上，变换其实就是一种“偏移”（包括平移和旋转），代表了不同坐标系之间的变换和旋转。

更具体点来说，设想一个简单的机器人，只有一个基本的移动机体和挂在机体上方的扫描仪。基于此，我们定义了两个坐标系：一个对应于机体中心点的坐标系，一个对应于扫描仪中心的坐标系。分别取名为“base_link”和“baser_laser。

此时，可以假设，我们已经从传感器获取了一些数据，以一种代表了物体到扫描仪中心点的距离的形式给出。换句话说，我们已经有了一些“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)。

可以理解成传感器到机体中心的向量（0.1,0.0,0.2）

可以自己去定义变换关系，以及完成变换的代码。但是随着坐标系的增多，每对坐标系之间如果都有相应的变换关系时，就会显得复杂，而且没有标准。

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”坐标系中，就可以推理出传感器扫描出的数据，并可安全的规划路径和避障等工作。

假定，我们以上层来描述“base_laser”坐标系的点，来转换到”base_link”坐标系。首先，我们需要创建节点，来发布转换关系到ROS系统中。下一步，我们必须创建一个节点，来监听需要转换的数据，同时获取并转换。在某个目录创建一个源码包，同时命名“robot_setup_tf”。添加依赖包roscpp,tf,geometry_msgs。

catkin_create_pkg robot_setup_tf roscpp tf geometry_msgs

广播变换

#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()){
  //发送tf函数，有5个参数
    broadcaster.sendTransform(
    //arg1 变换关系（父到子），它也有2个参数，旋转和位移
      tf::StampedTransform(
            tf::Quaternion(0, 0, 0, 1), 
            tf::Vector3(0.1, 0.0, 0.2)
        ),
    //发送时间
        ros::Time::now(),
    //父节点名
        "base_link", 
    //子节点名
        "base_laser")
        );
    r.sleep();
  }
}

调用变换

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

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

void transformPoint(const tf::TransformListener& listener)
{
/*创建一个虚拟点，作为geometry_msgs::PointStamped。消息名字最后的"Stamped"的意义是，它包含了一个头部，允许我们去把时间戳和消息的frame_id相关关联起来。*/
  geometry_msgs::PointStamped laser_point;
/*关联frame_id也就是将node定义为frame_id*/
  laser_point.header.frame_id = "base_laser";
/*设置laser_point的时间戳为ros::time(),即是允许我们请求TransformListener取得最新的变换数据*/
  laser_point.header.stamp = ros::Time();
//创建虚拟的点，即要变换过去的数据  
  laser_point.point.x = 1.0;
  laser_point.point.y = 0.2;
  laser_point.point.z = 0.0;

  try{
    geometry_msgs::PointStamped base_point;
/*通过TransformListener对象，调用transformPoint(),填充三个参数来进行数据变换*/
/*arg1，代表我们想要变换的目标坐标系的名字。
  arg2，填充需要变换的原始坐标系的点对象。
  arg3，目标坐标系的点对象。
  所以，在函数调用后，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());

/*某些其他的原因，变换不可得（可能是tf_broadcaster 挂了），调用transformPoint()时，TransformListener调用可能会返回异常。为了体现代码的优雅性，我们将会截获异常并把异常信息呈现给用户。*/
  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;
//自动的订阅ROS系统中的变换消息主题，同时管理所有的该通道上的变换数据
  tf::TransformListener listener(ros::Duration(