前面介绍了ROS计算全覆盖路径的整体流程，在完成区间分割之后，就需要计算区间之间的遍历顺序，本篇会对这部分的代码功能做一个分析记录。
计算各区间的遍历顺序，本质上可以看作是一个TSP问题（即旅行商问题），只不过不要求回到起点位置，故可以用求解TSP的方式求出各个区间的遍历顺序。求解TSP的方式分为两类：一类是求出最优解，另一种则是求出近似解。由于TSP属于NP完全问题，而且一般应用在智能扫地机上面的算法板算力并不高，故一般在实际应用中，不会要求求出最优解，能得到一个相对较合理的近似解即可。
ROS中调用计算TSP顺序的代码如下：
std::vector<int> optimal_order;
if (cell_visiting_order == OPTIMAL_TSP)
{
GeneticTSPSolver tsp_solver;
optimal_order = tsp_solver.solveGeneticTSP(rotated_room_map, polygon_centers, 0.25, 0.0, map_resolution, start_cell_index, 0);
if (optimal_order.size()!=polygon_centers.size())
{
std::cout << "=====================> Genetic TSP failed with 25% resolution, falling back to 100%. <=======================" << std::endl;
optimal_order = tsp_solver.solveGeneticTSP(rotated_room_map, polygon_centers, 1.0, 0.0, map_resolution, start_cell_index, 0);
}
}
else if (cell_visiting_order == LEFT_TO_RIGHT)
{
...//此处省略
}这段代码在前面有简单分析过，ROS默认使用的计算TSP遍历顺序的算法是遗传算法，且会先将地图缩放0.25倍后进行计算。本篇中，主要讲另一种求解TSP遍历顺序的方式，其源码位置为autopnp/ipa_building_navigation/common/include/ipa_building_navigation/nearest_neighbor_TSP.h，这是一种计算最邻近区域的方式。算法思想很简单，每次都取离当前位置最近的区域为下一个清扫区域，到达下一个区域后，再取最近的区域为下一个清扫区域。
该方法的函数的接口定义如下：
// compute TSP and distance matrix without cleaning it
// this version does not exclude infinite paths from the TSP ordering
std::vector<int> solveNearestTSP(const cv::Mat& original_map, const std::vector<cv::Point>& points, double downsampling_factor,
double robot_radius, double map_resolution, const int start_node, cv::Mat* distance_matrix=0);original_map 是经过预处理以及角度旋转后的地图数据，用来计算点跟点之间的路径长度；points 是前面做区间分割的时候，计算得出的每一个区间的中心点坐标数组；downsampling_factor 则是地图的缩放比率，即把传入的地图按照给定的缩放比率进行缩放，在缩放后的地图上寻找路径，需要注意的是，这种方式虽然用来处理大地图的时候，可以提高计算速度，但是存在一个隐患：当地图中存在窄通道时，缩放后该通道会闭合，从而导致两个区间不连通无法计算出区间距离；robot_radius 是机器人的半径；map_resolution 是栅格地图的分辨率；start_node 为初始区间序号，通过机器人当前坐标算出距离最近的区间；distance_matrix 则是用来返回距离矩阵。函数具体实现如下：
std::vector<int> NearestNeighborTSPSolver::solveNearestTSP(const cv::Mat& original_map, const std::vector<cv::Point>& points,
double downsampling_factor, double robot_radius, double map_resolution, const int start_node, cv::Mat* distance_matrix)
{
std::cout << "NearestNeighborTSPSolver::solveNearestTSP: Constructing distance matrix..." << std::endl;
//calculate the distance matrix
cv::Mat distance_matrix_ref;
if (distance_matrix != 0)
distance_matrix_ref = *distance_matrix;
DistanceMatrix distance_matrix_computation;
distance_matrix_computation.constructDistanceMatrix(distance_matrix_ref, original_map, points, downsampling_factor, robot_radius, map_resolution, pathplanner_);
return solveNearestTSP(distance_matrix_ref, start_node);
}distance_matrix_ref 是一个二维的距离矩阵，该矩阵保存各个区间的两两距离，在进行距离计算时，只需计算二维矩阵对角右上方的两点距离，计算完毕即可得出所有的区间之间的最短距离。
最短距离搜索策略则是：
（1）先将两点之间连一条直线，计算该直线上是否穿过障碍物，假如没有穿过障碍物，则直线距离即为两点之间的最短距离；
（2）假如两点之间的连线穿过障碍物，则使用A*算法计算出两点之间的最短距离。
计算生成距离矩阵后，就可以通过距离矩阵，计算得出最邻近点的遍历序列。这部分的代码如下：
std::vector<int> NearestNeighborTSPSolver::solveNearestTSP(const cv::Mat& path_length_matrix, const int start_node)
{
std::vector<int> calculated_order; //solution order
if(path_length_matrix.rows > 1) //check if clique has more than one member or else this algorithm produces a order of size=3
{
int last_node; //index of the last spectated node
std::vector<bool> visited(path_length_matrix.rows, false);
int current_node = start_node; //index of the current spectated node
calculated_order.push_back(current_node);
visited[current_node] = true;
//check every Point for the next nearest neighbor and add it to the order
do
{
int next_node; //saver for next node
double min_distance = 1e100; //saver for distance to current next node
for (int current_neighbor = 0; current_neighbor < path_length_matrix.cols; current_neighbor++)
{
if (visited[current_neighbor]==false) //check if current neighbor hasn't been visited yet
{
const double length = path_length_matrix.at<double>(current_node, current_neighbor);
if (length < min_distance && length > 0)
{
next_node = current_neighbor;
min_distance = length;
}
}
}
calculated_order.push_back(next_node); //add the found nearest neighbor to the order-vector
visited[next_node] = true;
current_node = next_node;
} while (calculated_order.size() < path_length_matrix.rows); //when the order has as many elements as the pathlength Matrix has the solver is ready
}
else
{
calculated_order.push_back(start_node);
}
return calculated_order;
}需要注意的是，这段代码实现存在一个隐患，上面已经讲到，调用该接口默认会将地图进行缩放，缩放地图可能就导致某一个区间与其他区间不连通，而这段代码里面没有对 next_node 进行初始化，假设由于缩放导致初始区间跟其他所有区间都不连通，则在执行完 for 循环后，next_node 的值是一个随机值，从而可能导致运行到 visited[next_node] = true;
计算得出各区间的遍历顺序后，后面就会按照遍历顺序，依次生成各个区间内弓字形覆盖路径，这个具体实现以及存在的一些问题后续再补充。
