costmap( 代价地图)
costmap是Navigation Stack里的代价地图,它其实也是move_base插件,本质上是C++的动态链接库,用过catkin_make之后生成.so文件,然后move_base在启动时会通过动态加载的方式调用其中的函数。
costmap是一个package是机器人收集传感器信息建立和更新的二维或三维地图,用在move_base的global_map和local_map当中。用于将laser扫描数据或者点云数据转化成一个2d的网格地图。
ROS里的地图的概念,就是/map
这个topic,它也是一张图片,一个像素代表了实际的一块面积,用灰度值来表示障碍物存在的可能性。然而在实际的导航任务中,光有一张地图是不够的,机器人需要能动态的把障碍物加入,或者清楚已经不存在的障碍物,有些时候还要在地图上标出危险区域,为路径规划提供更有用的信息。
因为导航的需要,所以出现了代价地图。你可以将代价地图理解为,在/map
之上新加的另外几层地图,不仅包含了原始地图信息,还加入了其他辅助信息。
代价地图有一下特点:
1.首先,代价地图有两张,一张是local_costmap
,一张是global_costmap
,分别用于局部路径规划器和全局路径规划器,而这两个costmap都默认并且只能选择costmap_2d
作为插件。
2. 无论是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
等开源插件。
地图中分成几部分
灰色部分:安全区域,robot运动在该区域不会发生碰撞
红色部分:危险区域,robot的footprint覆盖到该色网格就肯定会发生碰撞
蓝色部分:风险区域,robot在该区域有可能发生碰撞
红色几何型:robot在地图中所占的区域
2.两个操作
costmap订阅传感器发布的主题数据并更新自身地图,其中有两个操作
marking:标识该区域为障碍信息
clearing:清除该区域的障碍信息
3.网格的三种状态
每个网格会被赋予一个0~254之间的值,代表三种不同的状态
occupied:占有,unknow:未知,free:空闲。
4.地图更新和tf
costmap根据update frequency参数定期来更新地图。每个周期内,根据传感器信息mark/clear地图中的网格。
costmap更新构造地图,默认情况是,global_frame(/map)和 robot_base_frame(/base_link)和他们之间的frame都是连接的,并且定期更新的。如果tf没有按照定期的频率来更新,那navigation stack将会停止robot的运动。
5.网格的赋值
赋值范围为0~254
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的距离,还有用户自定义的算法
6.地图类型
第一种:指定地图,地图里面有长,宽和障碍信息,通常也会有一个定位系统,例如amcl
第二种 : 没有指定地图,保持robot始终在地图的中心位置,随着robot的运动不断构建地图
7.API
订阅的主题:
~<name>/footprint (geometry_msgs/Polygon):Specification for the footprint of the robot. This replaces the previous parameter specification of the footprint.
发布的主题
~<name>/grid (nav_msgs/OccupancyGrid):The values in the costmap
~<name>/grid_updates (map_msgs/OccupancyGridUpdate):The value of the updated area of the costmap
~<name>/voxel_grid (costmap_2d/VoxelGrid):Optionally advertised when the underlying occupancy grid uses voxels and the user requests the voxel grid be published.
参数
Plugins
8.需要tf 转换
(value of global_frame parameter) → (value of robot_base_frame parameter)
Usually provided by a node responsible for odometry or localization such as amcl.
9.地图上的层规范
9.1 static map layer
静态地图层,通过slam算法生成的地图,不可改变,不包括动态或静态障碍
9.2 Obstacle Map Layer
障碍地图层,通过传感器数据识别出非固定地图的障碍,包括动态,静态障碍
9.3 Inflation Layer
膨胀层,根据inflation半径计算出的robot所占的可能最大面积,可选的,
9.4 other layer
通过插件可以增加其他的层结构
Costmap初始化流程:
在navigation的主节点move_base中,建立了两个costmap。其中planner_costmap_ros_是用于全局导航的地图,controller_costmap_ros_是用于局部导航用的地图。下图为costmap的初始化流程。
(1)Costmap初始化首先获得全局坐标系和机器人坐标系的转换(世界坐标系和机器人坐标系相对变化关系)
(2)加载各个Layer,例如StaticLayer,ObstacleLayer,InflationLayer。
(3)设置机器人的轮廓
(4)实例化了一个Costmap2DPublisher来发布可视化数据。
(5)通过一个movementCB函数不断检测机器人是否在运动
(6)开启动态参数配置服务,服务启动了更新map的线程。
Costmap更新
Costmap的更新在mapUpdateLoop线程中实现,此线程分为两个阶段:
(阶段一)UpdateBounds:这个阶段会更新每个Layer的更新区域,这样在每个运行周期内减少了数据拷贝的操作时间。StaticLayer的Static map只在第一次做更新,Bounds 范围是整张Map的大小,而且在UpdateBounds过程中没有对Static Map层的数据做过任何的更新。ObstacleLayer在这个阶段主要的操作是更新ObstaclesMap层的数据,然后更新Bounds。InflationLayer则保持上一次的Bounds。
(阶段二)UpdateCosts:这个阶段将各层数据逐一拷贝到Master Map,可以通过下图观察Master Map的生成流程。(图来源于David Lu的《Layered Costmaps for Context-Sensitive Navigation》)
在(a)中,初始有三个Layer和Master costmap,Static Layer和Obstacles Layer维护它们自己的栅格地图,而inflation Layer并没有。为了更新costmap,算法首先在各层上调用自己的UpdateBounds方法(b)。为了决定新的bounds,Obstacles Layer利用新的传感器数据更新它的costmap。然后每个层轮流用UpdateCosts方法更新Master costmap的某个区域,从Static Layer开始(c),然后是Obstacles Layer(d),最后是inflation Layer(e)。
---------------------
感谢作者sunyoop这么精彩的讲述,谢谢!
作者:sunyoop
原文:https://blog.csdn.net/sunyoop/article/details/78183145
参考链接:http://wiki.ros.org/costmap_2d