costmap converter

背景：

该软件包用于配合teb local planner一起使用，用于加速teb的路径规划。teb中随着costmap的障碍物cell数量增加，耗时也会急速上升，对障碍物cell进行聚类就很有必要。

costmap converter plugin:

作用: costmap_2d单元格 转化为 几何图元（geometry primitives: points, lines, polygons）作为obstacles.

作用

如果没有激活该插件costmap_2d单元格 作为 单点障碍物，在high resolution map,计算时间长并且不稳定。

但是conversion也需要花时间，转换时间主要取决于算法，可以在单独的线程中执行。

配置的例子：

TebLocalPlannerROS:

  ## Costmap converter plugin   
  costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSMCCH"
  #costmap_converter_plugin: "costmap_converter::CostmapToLinesDBSRANSAC"
  #costmap_converter_plugin: "costmap_converter::CostmapToLinesDBSMCCH"
  #costmap_converter_plugin: "costmap_converter::CostmapToPolygonsDBSConcaveHull"
  #costmap_converter_plugin: "" # deactivate plugin
  costmap_converter_spin_thread: True
  costmap_converter_rate: 5
 
 
  ##Configure plugins (namespace move_base/TebLocalPlannerROS/PLUGINNAME)
  ## The parameters must be added for each plugin separately
  costmap_converter/CostmapToLinesDBSRANSAC:
    cluster_max_distance: 0.4
    cluster_min_pts: 2

提供的转换方法：

CostmapToLinesDBSRANSAC：单元格转换成点，线。

costmap_2d转换成lines （和points）的集合

CostmapToLinesDBSMCCH：单元格转换成点，线表示。

CostmapToPolygonsDBSConcaveHull：单元格转换成凹多边形表示。

CostmapToPolygonsDBSMCCH：单元格转换成凸多边形表示。

代码架构

compute（）函数

代价地图的障碍物处理逻辑都在compute（）函数中

dbscan函数

该函数将给定的点集进行聚类，分成一系列的点簇。 DBSCAN（Density-Based Spatial Clustering of Applications with Noise，带有噪声的基于密度的聚类方法）是一种基于密度的空间聚类算法。

选中一个样本点a，围绕点a画一个圆，规定这个圆的半径以及圆内最少包含的样本点，如果在指定半径内有足够多的样本点在内，那圆圈的圆心就转移到这个内部样本点，继续去圈附近其它的样本点，类似病毒传播。等到这个移动的圆所圈住的样本点数量少于预定值，就停止了。最开始那个点a称为为核心点，停下来的那个点为边界点，没被圈到的点为离群点。

• 特点： 基于密度
• 与其他算法比较
  kmeans 只能处理球形的簇，一个聚成实心的团（因为算法本身计算平均距离的
  局限）。但环形和不规则形状则不行了

convexHull2

每个点簇转换成凸多边形 graham算法

simplifypolygon

抽取主要凸多边形主要的轮廓，用到了douglaspeuker算法