ROS学习笔记之——Navigation Stack

上图中位于导航功能正中心的是move_base节点，可以理解为一个强大的路径规划器，在实际的导航任务中，你只需要启动这一个node，并且给他提供数据，就可以规划出路径和速度。 move_base之所以能做到路径规划，是因为它包含了很多的插件，像图中的白色圆圈global_planner（全局路径规划）、local_planner（局部路径规划）、global_costmap（全局的代价图）、local_costmap（局部的代价图）、recovery_behaviors（恢复行为）。而每一个插件其实也都是一个package，放在Navigation Stack里。

move_base

参考http://wiki.ros.org/move_base

先来看看move_base外围有哪些输入输出。

move_base的输入topic

/tf:提要提供的tf包括map_frame、odom_frame、base_frame以及机器人各关节之间的完成的一棵tf树。
/odom:里程计信息
/scan或/pointcloud:传感器的输入信息，最常用的是激光雷达(sensor_msgs/LaserScan类型)，也有用点云数据(sensor_msgs/PointCloud)的。
/map：地图，可以由SLAM程序来提供，也可以由map_server来指定已知地图（或自定义的室内地图）。

以上四个Topic是必须持续提供给导航系统的，下面一个是可随时发布的topic：

move_base_simple/goal:目标点位置（也就是导航的目标点）。

有几点需要注意：

1.move_base并不会去发布tf，因为对于路径规划问题来说，假设地图和位置都是已知的，定位和建图是其他节点的事情。

2.sensor_topics一般输入是激光雷达数据，但也有输入点云的情况。（主要是用于避障）

3.图中map_server是灰色，代表可选，并不表示/map这个topic是可选，必须提供地图给move_base。

move_base的输出topic

/cmd_vel:geometry_msgs/Twist类型，为每一时刻规划的速度信息，控制机器人的地盘运动（Base Controller(base controller)）。

move_base与插件

move_base要运行起来，需要选择好插件，包括三种插件：base_local_planner、base_global_planner和recovery_behavior，这三种插件都得指定，否则系统会指定默认值。

base_local_planner插件：

base_local_planner/TrajectoryPlannerROS: 实现了Trajectory Rollout和DWA两种局部规划算法
dwa_local_planner: 实现了DWA局部规划算法，可以看作是base_local_planner的改进版本

base_global_planner插件：

parrot_planner: 实现了较简单的全局规划算法
navfn/NavfnROS: 实现了Dijkstra和A*全局规划算法
global_planner: 重新实现了Dijkstra和A*全局规划算法,可以看作navfn的改进版

recovery_behavior插件：

clear_costmap_recovery: 实现了清除代价地图的恢复行为
rotate_recovery: 实现了旋转的恢复行为
move_slow_and_clear: 实现了缓慢移动的恢复行为

以上所有的插件都是继承于nav_core里的接口，nav_core属于一个接口package，它只定义了三种插件的规范，也可以说定义了三种接口类，然后分别由以上的插件来继承和实现这些接口。因此如果要研究路径规划算法，不妨研究一下nav_core定义的路径规划工作流程，然后仿照dwa_local_planner或其他插件来实现。

costmap插件：

该插件默认已经选择好，无法更改。默认即为costmap_2d。但costmap_2d提供了不同的Layer可以供我们设置

关于此处插件的描述可以参考：http://wiki.ros.org/pluginlib

move_base的Service

make_plan: nav_msgs/GetPlan类型，请求为一个目标点，响应为规划的轨迹，但不执行该轨迹。
clear_unknown_space: std_srvs/Empty类型，允许用户清除未知区域地图。
clear_costmaps: std_srvs/Empty类型，允许用户清楚代价地图上的障碍物。

tf Transform

什么是TF

各坐标系之间的转换

TF是一个ROS世界里的一个基本的也是很重要的概念，所谓TF(TransForm)，就是坐标转换．在现实生活中，我们做出各种行为模式都可以在很短的时间里完成，比如拿起身边的物品，但是在机器人的世界里，则远远没有那么简单．当机器人的＂眼睛＂获取一组数据，关于物体的坐标方位，但是相对于机器人手臂来说，这个坐标只是相对于机器人头部的传感器，并不直接适用于机器人手臂执行，那么物体相对于头部和手臂之间的坐标转换，就是TF.

坐标变换包括了位置和姿态两个方面的变换，ROS中的tf是一个可以让用户随时记录多个坐标系的软件包。tf保持缓存的树形结构中的坐标系之间的关系，并且允许用户在任何期望的时间点在任何两个坐标系之间转换点，矢量等．

不同坐标系之间的tf变换要明确，详细可以参考博客《ROS学习笔记之——ROS之tf变换》以及

tf的定义不是那么的死板，它可以被当做是一种标准规范，这套标准定义了坐标转换的数据格式和数据结构．tf本质是树状的数据结构，所以我们通常称之为"tf tree",tf也可以看成是一个topic:/tf，话题中的message保存的就是tf tree的数据结构格式．维护了整个机器人的甚至是地图的坐标转换关系．tf还可以看成是一个package,它当中包含了很多的工具．比如可视化，查看关节间的tf,debug tf等等．tf含有一部分的接口。所以可以看成是话题转换的标准，话题，工具，接口．

观察上图，我们可以看到ROS数据结构的一个抽象图，ROS中机器人模型包含大量的部件，这些部件统称之为link,每一个link上面对应着一个frame, 即一个坐标系．link和frame概念是绑定在一起的．像上图pr2模型中我们可以看到又很多的frame,错综复杂的铺置在机器人的各个link上，维护各个坐标系之间的关系，就要靠着tf tree来处理，维护着各个坐标系之间的联通．如下图：

上图是我们常用的robot_sim_demo运行起来的tf　tree结构，每一个圆圈代表一个frame,对应着机器人上的一个link，任意的两个frame之间都必须是联通的，如果出现某一环节的断裂，就会引发error系统报错．所以完整的tf tree不能有任何断层的地方，这样我们才能查清楚任意两个frame之间的关系．仔细观察上图，我们发现每两个frame之间都有一个broadcaster,这就是为了使得两个frame之间能够正确连通，中间都会有一个Node来发布消息来broadcaster.如果缺少Node来发布消息维护连通，那么这两个frame之间的连接就会断掉．broadcaster就是一个publisher,如果两个frame之间发生了相对运动，broadcaster就会发布相关消息．　

TF消息

从上图中的tf树可以看到。在每个frame之间都会有broadcaster来发布消息维系坐标转换，这个消息TransformStampde.msg,它就是处理两个frame之间一小段tf的数据格式．

TransformStamped.msg的格式规范如下（一个TransformStamped数组就是一个TF tree）：

std_mags/Header header
        uint32 seq
        time stamp
        string frame_id
string child_frame_id
geometry_msgs/Transform transform
        geometry_msgs/Vector3 translation
                float64 x
                float64 y
                float64 z
        geometry_msgs/Quaternion rotation
                float64 x
                float64 y
                flaot64 z
                float64 w

观察标准的格式规范，首先header定义了序号，时间以及frame的名称．

接着还写了child_frame，这两个frame之间要做那种变换就是由geometry_msgs/Transform来定义．

Vector3三维向量表示平移，Quaternion四元数表示旋转．

通过下图来举例子说明。

odom就是frame_id。baselink_footprint也是child_frame_id

我们知道，一个topic上面，可能会有很多个node向上面发送消息。如图所示，不仅有我们看到的frame发送坐标变换个tf，还有别的frame也在同样的向它发送消息。最终，许多的TransformStamped.msg发向tf，形成了TF树。

关于TF的更多内容可以参考：https://sychaichangkun.gitbooks.io/ros-tutorial-icourse163/content/chapter8/8.2.html

Sensor Information（sensor sources）

激光雷达或者深度摄像头来提供环境扫描功能

导航包中采用了来自sensors的信息来进行避障（avoid obstacles）。并且假设这些传感器发布的数据是sensor_msgs/LaserScan或者sensor_msgs/PointCloud

关于具体消息的定义，可以参考：

下面补充一下在ROS中发布sensor的消息流的相关知识

Publishing Sensor Streams over ROS

http://wiki.ros.org/navigation/Tutorials/RobotSetup/Sensors（有介绍怎么发布 LaserScan消息以及PointCloud消息）

机器人导航的过程中，需要各种sensors来提供定位、避障等功能。如 lasers, cameras, sonar, infrared, bump sensors（碰撞传感器）等等。然而，ros中的导航只接收数据格式为sensor_msgs/LaserScan或者sensor_msgs/PointCloud形式的消息。

sensor_msgs/LaserScan以及sensor_msgs/PointCloud消息都带有tf帧以及时间这两个独立的信息。这些信息都存储在header中。一共有三种类型的header

#Standard metadata for higher-level flow data types
#sequence ID: consecutively increasing ID 
uint32 seq
#Two-integer timestamp that is expressed as:
# * stamp.secs: seconds (stamp_secs) since epoch
# * stamp.nsecs: nanoseconds since stamp_secs
# time-handling sugar is provided by the client library
time stamp
#Frame this data is associated with
# 0: no frame
# 1: global frame
string frame_id

seq field为从给定发布服务器发送消息时自动增加的标识符相对应。

stamp field存储时间信息

frame_id field存储消息所对应的tf帧

Odometry Information（odometry source）

导航包需要由nav_msgs/Odometry 发布的里程计的消息

http://wiki.ros.org/navigation/Tutorials/RobotSetup/Odom（相关的message的发布可以参考）

导航包会采用tf来确定机器人在世界坐标下的位置并将传感器数据与静态地图相关联。然而，tf不提供机器人的速度。因此导航包需要里程计发布变换以及nav_msgs/Odometry（http://docs.ros.org/en/melodic/api/nav_msgs/html/msg/Odometry.html）消息来包含速度信息。

Base Controller(base controller)

向移动底盘发送控制移动的命令

导航包中，通过geometry_msgs/Twist消息发布速度命令到“ cmd_vel”话题上机器人的基本坐标系（base coordinate frame）中。

Mapping(map_server)

导航包并不是需要地图才可以运行。但是还是有比较好

此处是加载SLAM建立的地图（或者自建的室内地图）

ROS中的地图很好理解，就是一张普通的灰度图像，通常为pgm格式。这张图像上的黑色像素表示障碍物，白色像素表示可行区域，灰色是未探索的区域。如下图所示

在SLAM建图的过程中，你可以在RViz里看到一张地图被逐渐建立起来的过程，类似于一块块拼图被拼接成一张完整的地图。这张地图对于我们定位、路径规划都是不可缺少的信息。事实上，地图在ROS中是以Topic的形式维护和呈现的，这个Topic名称就叫做/map，它的消息类型是nav_msgs/OccupancyGrid

（http://docs.ros.org/en/melodic/api/nav_msgs/html/msg/OccupancyGrid.html）

/map话题的数据结构，包含了三个主要的部分:header, info和data。header是消息的报头，保存了序号、时间戳、frame等通用信息，info是地图的配置信息，它反映了地图的属性，data是真正存储这张地图数据的部分。

而本人之前博客中的demo中的地图，实际上就是自建为pgm格式的图片。

map_server是一个和地图相关的功能包，它可以将已知地图发布出来，供导航和其他功能使用，也可以保存SLAM建立的地图。要让map_server发布/map，需要输入给它两个文件:

地图文件，通常为pgm格式；
地图的描述文件，通常为yaml格式

像素值的大小代表了会出现障碍物的概率。如果是0则是完全没障碍物，如果是100则是有障碍物，如果是-1，则是不确定

AMCL

http://wiki.ros.org/amcl

Adaptive Mentcarto Localization(AMCL)，蒙特卡洛自适应定位是一种很常用的定位算法，它通过比较检测到的障碍物和已知地图来进行定位。

AMCL上的通信架构如下图所示，与SLAM的框架很像，最主要的区别是/map作为了输入，而不是输出，因为AMCL算法只负责定位，而不管建图。

Costmap Configuration (local_costmap) & (global_costmap)

Overview

costmap也称为代价图，是Navigation Stack里的代价地图，它其实也是move_base插件。move_base在启动时会通过动态加载的方式调用其中的函数。

地图就是/map这个topic，它也是一张图片，一个像素代表了实际的一块面积，用灰度值来表示障碍物存在的可能性。然而在实际的导航任务中，光有一张地图是不够的，机器人需要能动态的把障碍物加入，或者清楚已经不存在的障碍物，有些时候还要在地图上标出危险区域，为路径规划提供更有用的信息。

为此，costmap可以理解为在/map之上新加的另外几层地图，不仅包含了原始地图信息，还加入了其他辅助信息。

costmap具有以下几个特点：

１．首先，代价地图有两张，一张是local_costmap，一张是global_costmap，分别用于局部路径规划器和全局路径规划器，而这两个costmap都默认并且只能选择costmap_2d作为插件。

２. 无论是local_costmap还是global_costmap，都可以配置他们的Layer，可以选择多个层次。costmap的Layer包括以下几种：

Static Map Layer：静态地图层，通常都是SLAM建立完成的静态地图（或者自定义的静态地图）。
Obstacle Map Layer：障碍地图层，用于动态的记录传感器感知到的障碍物信息（通过激光或者深度相机）。
Inflation Layer：膨胀层，在以上两层地图上进行膨胀（向外扩张），以避免机器人的外壳会撞上障碍物。
Other Layers：你还可以通过插件的形式自己实现costmap，目前已有Social Costmap Layer、Range Sensor Layer等开源插件。

以同时选择多个Layer并存。

导航包中利用两个costmap来存储世界坐标系的障碍物信息。其中一个costmap用于做全局的路径规划，意味着在整个环境中制定长期计划。而另一个则是用于做局部的路径规划以及避障。

http://wiki.ros.org/costmap_2d

costmap_2d这个包提供了2D costmap的实现，通过从世界地图中获得sensor data，建立2D或者3D的数据占用格（ occupancy grid ）。以及2D costmap的膨胀成本（ inflates costs）（基于占用格（ occupancy grid ）和用户指定的膨胀半径（inflation radius））

costmap是一个package，用在move_base的global_map和local_map当中。用于将laser扫面数据或者点云数据转化成一个2d的网格地图

简单来说，地图中分成几部分

灰色部分：安全区域，robot运动在该区域不会发生碰撞

红色部分（代表有障碍物）：危险区域，robot的footprint覆盖到该色网格就肯定会发生碰撞

蓝色部分（代表机器人内切半径（inscribed radius）膨胀inflated的障碍物）：风险区域，robot在该区域有可能发生碰撞

红色多边形（red polygon ）：robot在地图中所占的区域（ footprint of the robot）

为了避免碰撞（ collision），机器人的footprint应该切勿与红格相交，并且机器人的中心点切勿越过蓝格

costmap_2d包提供了一个可配置的结构，该结构保证了机器人在（占用网格）occupancy grid中可以导航的位置。

这个costmap利用来着静态地图（static map）的传感器数据与信息来存储以及更新世界地图中的障碍物（通过costmap_2d::Costmap2DROS对象）

而costmap_2d::Costmap2DROS对象则提供与用户交互的纯2D界面，意味着关于障碍物的查询只可以在列（columns）中实现。例如在存储在相同XY平面的一张桌子和一只鞋子，但不同的Z位置将会导致对应的单元在costmap_2d::Costmap2DROS对象的costmap中有相同的cost value。旨在帮助规划平面空间

通过不同的图层叠加的。例如静态地图是一层，障碍物时另外一层。默认情况下，障碍物层（obstacle layer）包含三维信息，维护3D障碍物数据可使图层更智能地处理标记和清除。

两个操作Marking and Clearing

costmap订阅传感器发布的主题数据并更新自身地图，其中有两个操作

marking：标识该区域为障碍信息

clearing：清除该区域的障碍信息

costmap会自动订阅传感器的话题并且自动更新。每个传感器被用于标记（如在costmap中插入障碍物的信息），清除（在costmap中移除障碍物）。标记的操作只是数组的一共索引来改变cell的cost。而清除的操作则是包含了通过对于每个观测的汇报，从传感器的原点向外向外形成一个网格的光线追踪（raytracing）

如果采用三维的结构来存储障碍物的信息，当其进入costmap中，会降维到二维

网格的三种状态（Occupied, Free, and Unknown Space）

每个网格会被赋予一个0～254之间的值，代表三种不同的状态

occupied：占有，unknow：未知，free：空闲。

地图更新（Map Updates）和tf

为了将来自传感器上的数据插入costmap中，costmap_2d::Costmap2DROS对象需要采用tf（在前文已经介绍了）

costmap根据update frequency参数定期来更新地图。每个周期内，根据传感器信息mark/clear地图中的网格。

costmap更新构造地图，默认情况是，global_frame（/map）和 robot_base_frame(/base_link)和他们之间的frame都是连接的，并且定期更新的。如果tf没有按照定期的频率来更新，那navigation stack将会停止robot的运动。

网格的赋值（Inflation，膨胀层）

inflation（膨胀层）是从占用的单元（occupied cells ）中传播成本值（cost values），该值随着距离的增大而减少。

赋值范围为0～254。定义了5种不同的costmap值的标记：

lethal：值为254，传感器扫描到的实际障碍转化成map中的lethal网格。

inscribed ：内部半径，如果网格和lethal网格距离小于等于该半径，则为inscribed网格，值为253。取robot几何型所能容纳最大的圆的半径。

被标记为253～254的网格是障碍网格，也就是lethal和inscribed网格是障碍网格。当robot的边界覆盖到这两种网格时，一定会发生碰撞。

possibly circumscribed：膨胀半径，以robot的中心为园点旋转一周所能覆盖的最大圆半径，如果所在网格距离lethal小于等于膨胀半径，大于内部半径，则为possibly circumscribed网格。值为128～252，取决与robot的运动方向和轨迹，还有算法。当robot的边界覆盖到这种网格时，可能会发生碰撞。

free：值为0,说明没有任何信息可以阻碍机器人运行到这里。

unkonw：没有任何信息，传感器还没有扫描到该区域，都是障碍网格

1～127：不会发生碰撞的区域，这里的值赋值取决于与lethal的距离，还有用户自定义的算法

地图类型（Map Types）

第一种：指定地图，地图里面有长，宽和障碍信息，通常也会有一个定位系统，例如amcl

第二种 : 没有指定地图，保持robot始终在地图的中心位置，随着robot的运动不断构建地图（类似于SLAM）

地图上的层规范（Layer Specifications）

static map layer

静态地图层，通过slam算法生成的地图，不可改变，不包括动态或静态障碍

Obstacle Map Layer

障碍地图层，通过传感器数据识别出非固定地图的障碍，包括动态，静态障碍

Inflation Layer

膨胀层，根据inflation半径计算出的robot所占的可能最大面积，在以上两层地图上进行膨胀（向外扩张），以避免机器人的撞上障碍物。

other layer

还可以通过插件的形式自己实现costmap，目前已有Social Costmap Layer、Range Sensor Layer等开源插件。

参考资料

http://moorerobots.com/blog/post/3

http://wiki.ros.org/navigation

http://wiki.ros.org/navigation/Tutorials/RobotSetup

https://blog.csdn.net/jinking01/article/details/104193666（costmap）

https://sychaichangkun.gitbooks.io/ros-tutorial-icourse163/content/chapter10/10.1.html

https://blog.csdn.net/qq_32115101/article/details/81121220