RRT*算法 + costmap代价地图的开发步骤

最近在开发RRT* + costmap代价地图的功能，总结了一些开发心得，写此帖子作以记录。

RRT*算法 + costmap代价地图的开发步骤：

1. 完成可使用的RRT*算法；

2. 完成坐标系到栅格地图的转化，如：

//大地坐标系转化为栅格坐标系
Eigen::Vector2i RRTSTARGRID::coord2gridIndex(const Eigen::Vector2d pt)
{
  Eigen::Vector2i idx;
  idx(0) = ((pt(0) - _gl_xl) * _inv_resolution);
  idx(1) = ((pt(1) - _gl_yl) * _inv_resolution);
  return idx;
}

//栅格坐标系转化为大地坐标系
Eigen::Vector2d RRTSTARGRID::gridIndex2coord(const Eigen::Vector2i idx)
{
  Eigen::Vector2d pt;
  pt(0) = ((idx(0) * _resolution) + _gl_xl);
  pt(1) = ((idx(1) * _resolution) + _gl_yl);
  return pt;
}

3. 地图存储：

将传入的costmap栅格地图，写入一个变量用以存储，例如cv::Mat，或者使用msmset()函数开启一块内存用以存储。

4. 添加Obstacle障碍物

以memset()函数存储地图的方式为例（下面代码中的data变量），在对应的栅格位置，将函数值设置为1。这里设置成其他的数值也可以，能够方便下面的isObstacleFree函数中判断障碍物即可；

data[idx_x * _GLY_SIZE + idx_y] = 1;

        其中idx_x是障碍物的x轴的栅格坐标，idx_y是障碍物y轴的栅格坐标，_GLY_SIZE是代价地图在y轴的总长度，单位是像素点个数；

5.检测障碍物

在RRT* + costmap栅格地图的开发过程中，检测障碍物是我一直没有找到可使用的方法。其难点在于：如何在栅格地图中，判断两个采样点之间是否存在障碍物。

我发现在ros的ompl库中提供的RRT*算法，在提供的Valid函数中，只需要写入障碍点的位置即可，底层是如何判断的，源码中貌似没有给出。这里如果有技术大佬知道RRT*在栅格地图中如何判断两个采样点之间是否存在障碍物的方法，欢迎留言讨论；

这里我给出了一个方法，如图所示

判断两个栅格坐标坐在的（x2-x1）*（y2-y1)表格内是否存在障碍物块，如果存在，则判断为有障碍路径，舍弃该采样点。