ROS生成弓字形覆盖路径点逻辑分析

        前面提到，在经过分区，计算区间的遍历顺序后，接下来的操作就是按照遍历顺序，依次生成每一个子区间内的弓字形覆盖路径点，路径点的生成选取策略直接影响到后续路径跟随的效果。

        前面提到过，对每一个区间生成弓字形覆盖路径前，会对每一个区间计算最佳旋转角度，并在进行区间分割后，从图上依次获取每一个子区间，旋转到最佳角度（可简单理解为该角度下生成的覆盖直线最长，数量最小）。生成弓字形覆盖路径的过程分为两部分：

      （1）从上到下生成直线覆盖路径，每一个区间都会生成一或多条覆盖线。

      （2）根据当前点到区间四个顶点的距离长短，选用距离当前点最近的顶点作为弓字形路径起始点，连接当前覆盖直线最后一个路径点与下一条。

        其中，（1）中生成的多条覆盖直线是一开始根据机器人轮廓计算得出的，（2）中在生成一个区间内的覆盖直线点后，会取最后一个点作为当前点计算与该点距离最近的区间。下面就对这两部分的实现进行分析。

        调用这部分的代码如下：

	// point that keeps track of the last point after the boustrophedon path in each cell
	cv::Point robot_pos = rotated_starting_point;	
	// this is the trajectory of centers of the robot footprint or the field of view	
	std::vector<cv::Point2f> fov_middlepoint_path;	
	for(size_t cell=0; cell<cell_polygons.size(); ++cell)
	{
		computeBoustrophedonPath(rotated_room_map, map_resolution, 
	       cell_polygons[optimal_order[cell]], fov_middlepoint_path,
	       robot_pos, grid_spacing_as_int, 
	       half_grid_spacing_as_int, path_eps,
	       max_deviation_from_track, grid_obstacle_offset/map_resolution);
	}

        函数接口声明如下：

	// computes the Boustrophedon path pattern for a single cell
	void computeBoustrophedonPath(const cv::Mat& room_map, const float map_resolution,
		const GeneralizedPolygon& cell,std::vector<cv::Point2f>& fov_middlepoint_path,
		cv::Point& robot_pos,
		const int grid_spacing_as_int, const int half_grid_spacing_as_int,
		const double path_eps, const int max_deviation_from_track, const int 
		grid_obstacle_offset=0);

        需要注意的是robot_pos是指当前点，在每一次执行完都会修改该点数据。该函数完整的实现代码比较长，为了方便理解我把里面的代码按照步骤划分为两个部分，并把一些无关紧要的代码去点，想要看完整代码可以自行下载源文件阅读。

        首先是第一个步骤：计算得出所有连续的路径点并保存：

	// compute the basic Boustrophedon grid lines
	BoustrophedonGrid grid_lines;
	GridGenerator::generateBoustrophedonGrid(rotated_cell_map, rotated_inflated_cell_map, 
-1, grid_lines, cv::Vec4i(-1, -1, -1, -1),
grid_spacing_as_int, half_grid_spacing_as_int,
 1, max_deviation_from_track);

        保存覆盖线的数据结构定义如下：

class BoustrophedonLine
{
public:
	// points of the upper part of the line (always filled first)
	std::vector<cv::Point> upper_line;
	// points of the potentially existing lower part of the line
	std::vector<cv::Point> lower_line;
	// true if both lines, upper and lower,
	// concurrently provide valid alternative points at some locations 
	// (i.e. only if this is true, there are two individual lines of places to visit)
	bool has_two_valid_lines;

	BoustrophedonLine()
	: has_two_valid_lines(false)
	{
	}
};

class BoustrophedonGrid : public std::vector<BoustrophedonLine>
{
};

        这里定义了两条覆盖线，upper_line和lower_line，在遇到小障碍物时会生成两条线保证连续，但是在实际应用场景中，不太可能为了中间的一个小的障碍物就来回重复清扫两次，所以推荐根据实际应用场景修改这部分的逻辑。

       GridGenerator::generateBoustrophedonGrid()的功能是生成覆盖间隔线，其实现逻辑也很简单，为了方便理解我对代码进行了一些简化和解释如下：

	// 第一步，遍历全图，计算得出区间的上下左右四个边界点
	int min_x=inflated_room_map.cols, max_x=-1, min_y=inflated_room_map.rows, max_y=-1;

	for (int v=0; v<inflated_room_map.rows; ++v)
	{
		for (int u=0; u<inflated_room_map.cols; ++u)
		{
			if (inflated_room_map.at<uchar>(v,u) == 255)
			{
				if (min_x > u)
					min_x = u;
				if (max_x < u)
					max_x = u;
				if (min_y > v)
					min_y = v;
				if (max_y < v)
					max_y = v;
			}
		}
	}

	int y=min_y;
	// loop through the vertical grid lines with regular grid spacing
	for (; y<=max_y+half_grid_spacing; y += grid_spacing)
	{
		if (y > max_y)	// this should happen at most once for the bottom line
			y = max_y;

		BoustrophedonLine line;
		const cv::Point invalid_point(-1,-1);
		// for keeping the horizontal grid distance
		cv::Point last_added_grid_point_above(-10000,-10000), last_added_grid_point_below(-10000,-10000);
		// for adding the rightmost possible point
		cv::Point last_valid_grid_point_above(-1,-1), last_valid_grid_point_below(-1,-1);
		// loop through the horizontal grid points with horizontal grid spacing length
		for (int x=min_x; x<=max_x; x+=1)
		{
			// 从左往右，逐个像素点判断，将该 Y 坐标下的非障碍物点添加到覆盖线 line 里面
			// 此处省略，需要可以下载源码阅读
			... ...
		}

		// 上面生成的是一条覆盖线，下面则是把当前生成的覆盖直线添加到覆盖线数组里面
		// 需要注意的是这里用到了upper_line和lower_line两条线
		BoustrophedonLine cleaned_line;
		if (line.upper_line.size()>0 && line.lower_line.size()>0)
		{
			... ...

			// add cleaned line to the grid
			grid_points.push_back(cleaned_line);
		}
	}

        生成连续覆盖直线后，执行第二个步骤：按照给定的点间距取点并按照弓字形连接生成路径点。这部分代码比较多，所以只取其中最关键的两个函数来说明。

	// downsamples a given path original_path to waypoint distances of 
	// path_eps and appends the resulting path to downsampled_path
	void downsamplePath(const std::vector<cv::Point>& original_path, 
		std::vector<cv::Point>& downsampled_path,
		cv::Point& cell_robot_pos, const double path_eps);

	// downsamples a given path original_path to waypoint distances 
	// of path_eps in reverse order as given in original_path
	// and appends the resulting path to downsampled_path
	void downsamplePathReverse(const std::vector<cv::Point>& original_path,
		std::vector<cv::Point>& downsampled_path,
		cv::Point& robot_pos, const double path_eps);

        这其中 downsamplePath是从左往右取点，downsamplePathReverse是从右往左取点，path_eps是路径点的间隔，即第一条直线从左往右取点，下一条直线从右往左取点，再下一条从左往右取点，这样依次循环执行，就生成弓字形的覆盖路径点。