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 
#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()){
  //发送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 
#include 
#include 

/*创建一个函数，参数为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(10));


  ros::Timer timer = n.createTimer(ros::Duration(1.0), boost::bind(&transformPoint, boost::ref(listener)));

  ros::spin();

}

修改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})

编译并运行

[ 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

可以看到两个坐标系点的位置。

1.2发布里程信息

一个通过ROS从/odom坐标系到/base_link坐标系的变换信息，包括通过ROS发送nav_msgs/Odometry消息。

导航功能包集用TF软件包来确定机器人在世界坐标系中的位置和相对于静态地图的相关传感器信息，但是TF软件包不提供与机器人速度相关的任何信息，所以导航功能包集要求里程计源程序发布一个变换和一个包含速度信息的nav_msgs/Odometry消息。

##nav_msgs/Odometry消息
std_msgs/Header header
      uint32 seq
      time stamp
      string frame_id
string child_frame_id
geometry_msgs/PoseWithCovariance pose
      geometry_msgs/Pose pose
            geometry_msgs/Point position
                  float64 x
                  float64 y
                  float64 z
            geometry_msgs/Quaternion orientation
                  float64 x
                  float64 y
                  float64 z
                  float64 w
      float64[36] covariance
geometry_msgs/TwistWithCovariance twist
      geometry_msgs/Twist twist
            geometry_msgs/Vector3 linear
                  float64 x
                  float64 y
                  float64 z
            geometry_msgs/Vector3 angular
                  float64 x
                  float64 y
                  float64 z
      float64[36] covariance

nav_msgs/Odometry消息存储机器人在自由空间位置和速度的估计。在这个消息中，pose与机器人相对于里程计坐标系的为hi在估计与位置估计的协方差相关，twist与在子坐标系里机器人的速度相关，通常是移动平台的坐标系连同速度估计的协方差。

使用tf发布里程计变换

#include 
#include 
#include 
//发布odom坐标系到base_link坐标系的变换和nav_msgs/Odometry消息
//需要的头文件
int main(int argc, char** argv){
  ros::init(argc, argv, "odometry_publisher");

  ros::NodeHandle n;
//创建publisher和broadcaster实例，以便能够分别使用ROS和tf发送消息
  ros::Publisher odom_pub = n.advertise("odom", 50);
  tf::TransformBroadcaster odom_broadcaster;
//假设机器人最初从odom坐标系的原点开始
  double x = 0.0;
  double y = 0.0;
  double th = 0.0;
//设置一些速度，让base_link坐标系相对于odom坐标系 在x上0.1m/s 
//y上-0.1m/s的速率 th方向0.1rad/s角度移动，即转圈
  double vx = 0.1;
  double vy = -0.1;
  double vth = 0.1;

  ros::Time current_time, last_time;
  current_time = ros::Time::now();
  last_time = ros::Time::now();
//以1hz速率发布odom里程计的信息
  ros::Rate r(1.0);
  while(n.ok()){

    ros::spinOnce();               // check for incoming messages,书上没有这一句
    current_time = ros::Time::now();


//根据设置恒定的速度更新里程信息
//真正的里程系统将整合计算的速度
    double dt = (current_time - last_time).toSec();
    double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
    double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
    double delta_th = vth * dt;

    x += delta_x;
    y += delta_y;
    th += delta_th;

//通常尝试使用我们系统中所有消息的3D版本，以允许2D和3D组件在适当的时候一起工作，并将我们要创建的消息数量保持在最小。
//因此，有必要将我们用于里程的偏航值转换为四元数发送出去
//幸运的是，tf提供了允许从偏航值容易地创建四元数并且容易地访问四元数的偏航值的功能。

    geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);

//创建一个TransformStamped消息，将通过tf发送
//在current_time发布从odom坐标系到base_link坐标系的转换
//确保使用odom为父坐标系 base_link为子坐标系

    geometry_msgs::TransformStamped odom_trans;
    odom_trans.header.stamp = current_time;
    odom_trans.header.frame_id = "odom";
    odom_trans.child_frame_id = "base_link";

//从我们的odometry数据填充变换消息，然后使用TransformBroadcaster发送变换。
    odom_trans.transform.translation.x = x;
    odom_trans.transform.translation.y = y;
    odom_trans.transform.translation.z = 0.0;
    odom_trans.transform.rotation = odom_quat;


    odom_broadcaster.sendTransform(odom_trans);

//还需要发送nav_msgs/Odometry消息，以便导航堆栈可以从中获取速度信息。
//将消息的头部设置为current_time和odom坐标系

    nav_msgs::Odometry odom;
    odom.header.stamp = current_time;
    odom.header.frame_id = "odom";

//使用里程数据填充消息，并且通过路线发送
    //set the position
    odom.pose.pose.position.x = x;
    odom.pose.pose.position.y = y;
    odom.pose.pose.position.z = 0.0;
    odom.pose.pose.orientation = odom_quat;
//将消息的child_frame_id设置为base_link坐标系，因为这是我们发送速度信息的坐标系
    //set the velocity
    odom.child_frame_id = "base_link";
    odom.twist.twist.linear.x = vx;
    odom.twist.twist.linear.y = vy;
    odom.twist.twist.angular.z = vth;

    //publish the message
    odom_pub.publish(odom);

    last_time = current_time;
    r.sleep();
  }
}

1.3发布传感器流数据替代PointStamped

通过ROS从传感器正确发布数据对于导航堆栈安全运行非常重要。如果导航堆栈没有收到来自机器人传感器的信息，那么它将被盲目地驾驶，并且很有可能会触发事件。有许多传感器可用于向导航堆栈提供信息：激光器，相机，声纳，红外线，碰撞传感器等。但是，当前的导航堆栈只接受使用sensor_msgs / LaserScan消息类型或sensor_msgs / PointCloud消息类型。以下将提供两种类型消息的典型设置和使用示例。

ROS的消息头

与众多通过ROS发布的消息一样
sensor_msgs/LaserScan和sensor_msgs/PointCloud消息类型包含tf的坐标系和时间相关的信息，为了使这些发送的信息标准化，这小消息中的消息头都是必不可少的一部分。

下面列出三种类型的消息头，当消息从给定发布者发出时，seq域自动增加消息的识别符。stamp域存储与消息中的数据有关的时间信息。例如，在激光扫描仪中，stamp对于扫描开始的时间。frame_id域存储了消息中的数据有关的tf坐标系信息。

uint32 seq
time stamp
string frame_id

发送激光雷达(LaserScans)信息

##LaserScans 消息
Header header
float32 angle_min
float32 angle_max
float32 angle_increment

float32 time_increment
float32 scan_time
float32 range_min
float32 range_min
float32[] ranges
float32[] intensities

#include 
#include 

int main(int argc, char** argv){
  ros::init(argc, argv, "laser_scan_publisher");

  ros::NodeHandle n;
  ros::Publisher scan_pub = n.advertise("scan", 50);

  unsigned int num_readings = 100;
  double laser_frequency = 40;
  double ranges[num_readings];
  double intensities[num_readings];

  int count = 0;
  ros::Rate r(1.0);
  while(n.ok()){
    //generate some fake data for our laser scan
    for(unsigned int i = 0; i < num_readings; ++i){
      ranges[i] = count;
      intensities[i] = 100 + count;
    }
    ros::Time scan_time = ros::Time::now();

    //populate the LaserScan message
    sensor_msgs::LaserScan scan;
    scan.header.stamp = scan_time;
    scan.header.frame_id = "laser_frame";
    scan.angle_min = -1.57;
    scan.angle_max = 1.57;
    scan.angle_increment = 3.14 / num_readings;
    scan.time_increment = (1 / laser_frequency) / (num_readings);
    scan.range_min = 0.0;
    scan.range_max = 100.0;

    scan.ranges.resize(num_readings);
    scan.intensities.resize(num_readings);
    for(unsigned int i = 0; i < num_readings; ++i){
      scan.ranges[i] = ranges[i];
      scan.intensities[i] = intensities[i];
    }

    scan_pub.publish(scan);
    ++count;
    r.sleep();
  }
}

发送点云(PointClouds)信息

#PointClouds 消息
Header header
geometry_msgs/Point32[] points

ChannelFloat32[] channels

#include 
#include 

int main(int argc, char** argv){
  ros::init(argc, argv, "point_cloud_publisher");

  ros::NodeHandle n;
  ros::Publisher cloud_pub = n.advertise("cloud", 50);

  unsigned int num_points = 100;

  int count = 0;
  ros::Rate r(1.0);
  while(n.ok()){
    sensor_msgs::PointCloud cloud;
    cloud.header.stamp = ros::Time::now();
    cloud.header.frame_id = "sensor_frame";

    cloud.points.resize(num_points);

    //we'll also add an intensity channel to the cloud
    cloud.channels.resize(1);
    cloud.channels[0].name = "intensities";
    cloud.channels[0].values.resize(num_points);

    //generate some fake data for our point cloud
    for(unsigned int i = 0; i < num_points; ++i){
      cloud.points[i].x = 1 + count;
      cloud.points[i].y = 2 + count;
      cloud.points[i].z = 3 + count;
      cloud.channels[0].values[i] = 100 + count;
    }

    cloud_pub.publish(cloud);
    ++count;
    r.sleep();
  }
}

2. SLAM

SLAM也称为CML（concurrent mapping and localization），即时定位与地图构建，或并发建图与定位。SLAM问题可以描述为：机器人在未知环境中从一个未知位置开始移动，在移动过程中根据位置估计和地图进行自身定位，同时在自身定位的基础上建造增量式地图，实现机器人的自主定位与导航。

2.1 slam_gmapping功能包

    本功能包包含了来自OpenSlam的ROS软件包GMapping。GMapping功能包提供了基于激光的SLAM，建立了名为slam_gampping的节点。使用slam_gmapping，用户可以根据从激光传感器输出的数据创建2D栅格地图。GMapping是一个从激光传感器数据建立地图的非常有效的Rao-Blackwellized粒子滤波软件包。GMapping文档：https://svn.openslam.rog/data/svn/gmapping。

    该方法使用粒子滤波方法，其中每个粒子承载了一个独立的环境地图。其中，关键的问题在于怎样降低粒子数量。在该软件包中，采用自适应技术来降低粒子数量。其中采用的方法为不光考虑移动而且考虑最近的观测值来计算精确的粒子分布。这样，在滤波的预测步，极大降低了机器人位姿的不确定性。更进一步，算法中使用有选择地重采样技术，有效降低了粒子耗散对算法的影响。

    使用：机器人平台要提供里程数据并且装备了水平安装的固定的激光测距仪。slam_gmapping节点将把收到的数据进行tf变换。

为了让机器人建立地图，需要在base_scan主题发布扫描数据：

rosrun gmapping slam_gmapping scan:=base_scan

节点

1)slam_gmapping
    slam_gmapping节点在sensor_msgs/LaserScan消息内获取数据并建立地图(nav_msgs/OccupancyGrid)。该地图可以通过ROS主题或服务来获取。

2)订阅的topic
    (1)tf(tf/tfMessage)：与坐标系相关的变换。
    (2)sacn(sensor_msgs/LaserScan):激光扫描数据

3)发布的topic
    (1)map_metadata(nav_msgs/MapMetaData)：从这个周期性更新的主题获取地图数据。
    (2)map(nav_msgs/OccupancyGrid):从这个周期性更新的主题获取地图。
    (3)entropy(std_msgs/Float64):估计机器人位姿分布的熵。

4)服务
    dynamic_map(nav_msgs/GetMap):调用该服务以获取地图数据。

5)参数
     P165

6)需要的tf变换
    (1)<the frame attached to incoming scans> -> base_link:通常是一个固定值，通过robot_state_publisher或tf的static_transform_publisher进行周期性广播。
    (2)base_link -> odom:通常由里程计系统提供。

7)提供的tf变换
    map -> odom:当前地图坐标系中机器人位姿的估计。

使用记录的数据建立地图

1.建立地图

1）如果已经安装源文件，首先编译GMapping
2）获取一个bag：自己创建或者从下载测试包
3）运行ROS core
4）设置参数：rosparam set use_sim_time true
5）运行GMapping：rosrun gmapping slam_gmapping scan:=base_scan

在新终端中，回放消息记录包，反馈数据给slam_gmapping：

rosbag play <name of the bag that you downloaded / created in step 2>

运行之后，等程序执行完成，退出。

最终，可以保存建立的地图为pgm文件：rosrun map_server map_saver。

2.下载测试包

wget http://pr.willowgarage.com/data/gmapping/basic_localization_stage.bag

3.建立个人程序包

1.搭建自己的机器人，装备激光传感器，发布变换数据，并可以操作移动。
2.记录扫描与变化数据：
rosbag record -O mylaserdata /base_scan /tf
#在当前目录写入一个名为mylaserdata_-topic.bag文件
3.驱动机器人移动
4.结束rosbag

4.实时查看建立地图的过程

如果想实时的查看地图，而不是等所有进程结束之后，可执行下命令：

1.编译nav_view : rosmake nav_view 
2.重复上一步，直至有日志未见可以回放数据，然后运行slam_gmapping
3.运行nav_view : rosrun nav_view nav_view /static_map:=/dynamic_map。随后单击重载地图按钮，即可看到最新地图。

5.模拟器中建立地图

sudo apt-get install ros-diamondback-pr2all

svn co https://code.ros.org/svn/wg-ros-pkg/stacks/wg_robots_gazebo/trunk/pr2_build_map_gazebo_demo/

#将上一步的路径加到ROS_PACKAGE_PATH然后编译 
rosmake pr2_build_map_gazebo_demo

#运行
roslaunch pr2_build_map_gazebo_demo pr2_build_map_gazebo_demo.launch

#在rviz窗口左侧面板进行设置：
Global Options中固定坐标系(Fixed Frame)设置为 /omod_combined
Global Options中目标坐标系(Target Frame)设置为 /odom_combined

#在rviz中看到的只是地图的一部分，为了建立完整地图，需要让机器人运动
#在新的终端运行
roslaunch pr2_teleop teleop_keyboard.launch 
rosrun map_server map_saver -f map ##保存地图

6.模拟器中使用客户定制地图

首先需要一个地图文件map.pgm，然后把它放在pr2_gazebo功能包下。

编译 rosmake pr2_2dnav_gazebo

创建启动文件 my_map_sim.launch

启动程序
export ROBOT = sim
roslaunch my_map_sim.launch

<launch>
<include file="$(find gazebo_worlds)/launch/office_world.launch"/>
<include file="$(find pr2_gazebo)/pr2.launch"/>

<node pkg="map_server" type="map_server" args="$(find pr2_gazebo)/map.pgm 0.05" respawn="true" name="map1" />

<node pkg="pr2_tuckarm" type="tuck_arms.py" args="b" output="screen" name="tuck_arms" />

<include file="$(find pr2_2dnav_gazebo)/2dnav-stack-amcl.launch" />
<include file="$(find 2dnav_pr2)/rviz/rviz_move_base.launch" />

launch>