牛耕分解+形态学分割 全覆盖路径规划（一）

先放个最终的效果图：

原文链接：牛耕分解+形态学分割 全覆盖路径规划（一）

源码是开源ipa覆盖算法：ipa_coverage_planning
调试过程参考：ipa 覆盖算法测试

下文是我在该项目下的调试，主要用到了ipa_coverage_planning中的牛耕覆盖和形态学分割两个算法，ipa中还给出了很多其他的算法供大家参考

2024/9/11

由于对整个房间直接进行牛耕覆盖看起来效果不太好，而且也不符合扫地机器人的实际清扫过程，考虑先进行分区，然后对于单独房间分别进行覆盖；

由于采用先分区后覆盖的思路，所以我们需要考虑两个算法的输入输出条件，以及满足我们需求的逻辑：

从牛耕的输入出发，需要的重要的参数是：

1. room_map ：地图
2. starting_position_ ： 起始坐标

针对地图，如果我们想一个房间一个房间进行规划的话，那么最好的就是输入的地图为单独的房间？单独的房间？如何处理？回想分区的结果，我们可以得到些什么：

① 带有标签的地图，所谓的标签，就是在原地图的基础上，房间被赋值为顺序的标号，也就是indexed_map
② 房间的中心点坐标（room_centers_x_values，room_centers_y_values），以及房间的四个顶点的坐标（xmin,xmax,ymin,ymax）

如果有这些数据的话，是不是可以考虑每遍历一个房间，将其余的房间全部填充0（不可到达），然后规划路径；ummmmmm显然不太好，不去考虑填充时计算量的庞大，最后生成的各个房间的路径该如何连接在一起？

有了上面的思考，思路变得清晰了，这里我们需要考虑两个问题：

1. 如何单个房间进行牛耕覆盖，输入的地图应该是什么样的
2. 两个房间的路径该如何连接起来

继续思考，先不去想输入的地图的格式，把问题转到starting_position_ 也就是起始坐标上，因为输入地图有笨办法（对标签地图进行处理，非当前房间置0）只需考虑优化，那么房间的起点该如何确定？
（为什么不用考虑房间的终点呢？因为两个房间连接的话只需要两个点就好，新房间的起点和旧房间的终点，终点就是覆盖路径的最后一个点可以由算法直接给出。有这两个点之后来一个A就可以连接两个房间）【补充：在做完之后才发现房间的终点也是十分重要的】
由于比较容易得到每个房间的边界点的坐标，同时也可以根据这个坐标得到房间的中心点（代码中考虑了机器人能否到达）那么我们是否可以以设定的机器人的初始点到第一个房间的中心点生成一条路径（A），然后随着路径寻找，属于房间范围的第一个点即为房间的初始点，理论可行！开始实践！

Step 1 ： 确定机器人初始坐标点，由于不知道放哪比较好，毕竟实际的初始坐标是根据基站或者说开机的位置确定的，所以这里设定为整个地图的最中心：

starting_position_.x = msg.info.origin.position.x +  msg.info.width * 0.5;
starting_position_.y = msg.info.origin.position.y +  msg.info.height * 0.5;

Step2 ： 了解牛耕覆盖调用的A库（因为A都差不多，不想去修改牛耕的代码，就直接拿它的A了），并使用A库去完成两点生成路径；

int index_room = 7;

cv::Point start_point = starting_position_;
cv::Point end_point(room_centers_x_values[index_room], room_centers_y_values[index_room]);
std::vector astar_path;

cv::Point start_point2;
if (start_point.x >= min_x_value_of_the_room[index_room] &&
    start_point.x <= max_x_value_of_the_room[index_room]