ROS Navigation之costmap_2d完全详解

分类: ROS Navigation源码完全详解

Posted by 白夜行的狼 on January 1, 2019

0. 写在最前面

本文持续更新地址:https://haoqchen.site/2019/01/01/costmap-2d/

本文将介绍自己在看ROS的Navigation stack中的costmap_2d包源代码时的一些理解。作者的ROS版本是indigo,nav_core版本是1.12.13。如有错误,欢迎在评论中指正。

如果觉得写得还不错,就请收藏一下啦~~~也可以找一下我写的其他包的源码解读来看一下。关注一下我的专栏什么的。

你的Star是作者坚持下去的最大动力哦~~~

1. package.xml与CMakeLists.txt

package本包提供了一个2D代价地图的实现,该实现从现实中获得传感器信息,然后构建一个2D或者3D的占据栅格(取决于是否使用基于体素的实现),然后根据占据栅格和用户定义的膨胀率来将代价膨胀成代价地图。本包同样提供了从map_server这个包来初始化代价地图的支持

CMakeLists

注意事项

  1. 代价地图的原点在左下角,向右为X轴,向上为Y轴。

  2. 内存中实际使用unsigned char*来存储代价地图,而向ROS发布时是使用nav_msgs::OccupancyGrid,其使用int8来存储。而amcl中又将nav_msgs::OccupancyGrid转换成自己的格式(只有三种状态)。

    含义 存储值 发布值 amcl
    不明(NO_INFORMATION) 255 -1 0
    致死障碍(LETHAL_OBSTACLE) 254 100 +1
    膨胀生成的(INSCRIBED_INFLATED_OBSTACLE) 253 99 0
    无障碍(FREE_SPACE) 0 0 -1
    其他代价 1-252 1-98 0

2. costmap_2d类

该类是在c++层面实现了代价地图的存储(用unsigned char*)、复制、参数设置、画多边形等功能。

  • 成员: ```cpp unsigned int size_x_;//像素大小 unsigned int size_y_; double resolution_;//meters/pixel double origin_x_;//原点的坐标,单位是米 double origin_y_; unsigned char* costmap_;//代价地图指针,用unsigned char类型的数组来表示 unsigned char default_value_;//reset地图时使用的默认值

typedef boost::recursive_mutex mutex_t; mutex_t* access_;//锁,为了代价地图的线程访问安全 ```

  • 嵌套类 MarkCell
    PolygonOutlineCells
    嵌套类声明为protected,作用域是类内以及派生类(Costmap2DROS),在这里是为了方便本类的一些实现声明的类。

2.1 构造与析构函数

  • 构造: 参数值传递给私有成员,并按照尺寸new一个costmap,初始化为default。拷贝构造调用了重载的=号,=号实现的是深拷,即重新申请一个一样大的内存,然后拷贝数据。
  • 析构: delete代价地图的内存,并将costmap_=NULL

2.2 一些窗口拷贝函数

copyCostmapWindowresetMap、不是重点,就不说了。但是需要注意的是世界坐标系到地图坐标系,地图坐标系到世界坐标系之间的转换以及边界条件的处理等,通过resolution_来进行转换。

其实看不懂updateOrigin这个函数,应该对所有像素都更新原点才对,但是看实现,应该只是将满足新原点右边,上边的拷贝了,莫名其妙。

2.3 一些设置、get地图信息的函数

这些只是提供了获取以及修改私有成员的接口,不重要。

2.4 setConvexPolygonCost

设置凸多边形的代价,参数一为多边形的顶点坐标(世界坐标系),参数二为设置的代价值。

  • 函数首先将顶点坐标转换成map坐标系,然后统一格式。
  • 调用convexFillCells获取到该多边形顶点框住的所有地图坐标点
  • convexFillCells返回的所有坐标点设置为带价值

convexFillCells的做法是,

  • 首先调用polygonOutlineCells函数来画出多边形的外边,该函数用到了上面说的嵌套类PolygonOutlineCells,该函数将嵌套类传递到raytraceLine来,由raytraceLine来根据斜率是否大于1执行2D(以x还是y为底边)的Bresenham’s raytracing algorithm(Bresenham光线追踪),得到直线点后利用嵌套类重载的()来记住对应点位置。
    bresenham2D
    bresenham2D的原理大概如上图所示,如果y+0.5>1则取上面的点,否则取下面的点。需要注意的是,我们这里是用一维矩阵来模拟图的,所以代码里面有些绕,没兴趣的大概知道代码是做什么的就好。
  • polygonOutlineCells获得的多边形边框点进行冒泡排序,以x轴从小到大排序
  • 根据排完序的边框点,找到每个x值对应的y轴最大最小值,然后在这个x轴方向上,在y轴最小最大之间进行插值就可以获得整个多边形的点啦。

很抱歉,这篇文章写着写着后面忙毕设去了,没能写完。毕业后从事的工作相关性也不大,一直996不太想去写完了,发在这里当做是一种记录吧

参考

DDA算法和Bresenham算法


喜欢我的文章的话Star一下呗Star

版权声明:本文为白夜行的狼原创文章,未经允许不得以任何形式转载