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

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

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

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

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

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

调用这部分的代码如下：

 // point that keeps track of the last point after the boustrophedon path in each cellcv::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 cellvoid 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 linesBoustrophedonGrid 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 linestd::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 spacingfor (; y<=max_y+half_grid_spacing; y += grid_spacing){if (y > max_y) // this should happen at most once for the bottom liney = max_y;BoustrophedonLine line;const cv::Point invalid_point(-1,-1);// for keeping the horizontal grid distancecv::Point last_added_grid_point_above(-10000,-10000), last_added_grid_point_below(-10000,-10000);// for adding the rightmost possible pointcv::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 lengthfor (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 gridgrid_points.push_back(cleaned_line);}}

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

 // downsamples a given path original_path to waypoint distances of // path_eps and appends the resulting path to downsampled_pathvoid 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_pathvoid 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是路径点的间隔，即第一条直线从左往右取点，下一条直线从右往左取点，再下一条从左往右取点，这样依次循环执行，就生成弓字形的覆盖路径点。

ROS生成弓字形覆盖路径点逻辑分析相关推荐

ROS与智能机器人技术发展路径探索
ROS与智能机器人技术发展路径探索 Sonictl, Aug 2015 序本文按照目标路径.需求.资源.方法.计划为提纲,根据本人对市场.技术.商业模式等的经验理解,梳理总结 ROS 和机器人技术对 ...
全覆盖路径规划思想（2）
全覆盖清扫机器人思路预规划路线思路方案一(可快速demo) 动态规划思路方案二预规划路线思路机器人在遍历整个环境前,应已知环境,即已获取全局地图:目前较常见的为激光slam方案,如科沃斯.小 ...
谈asp.net解决方案的项目生成时的输出路径
今日将一个大的asp.net解决方案从VS2005升级为VS2008,本来以为由VS自动转换后就应该不会有问题.没想到出现了不少问题,弄了大半天. 其中的一个主要原因就是项目生成时的"输出路 ...
移动机器人全覆盖路径规划及仿真（三.地图分割）
标题移动机器人全覆盖路径规划级仿真(三.地图分割) 标题算法流程 1.建立event类和CellNode类 2.将Wall(obostacle)每个坐标点变成event,加入event_list 3. ...
设计测试用例实现语句覆盖,判定覆盖,条件覆盖，判定/条件覆盖,条件组合覆盖,路径覆盖.
第一题: 设计测试用例实现语句覆盖,判定覆盖,条件覆盖,判定/条件覆盖,条件组合覆盖,路径覆盖. [1]语句覆盖(设计若干个测试用例,使程序中的每个可执行语句至少执行一次) (x>3)& ...
java coherence_Coherence配置默认覆盖路径
我已经花了两天多的时间试图完成这项工作而没有任何结果 . 服务器是带有嵌入式Coherence服务器的WebLogic 12c . 值得一提的是,我没有在独立模式下运行Coherence,而是在可以通 ...
poj2594（最小可相交覆盖路径问题）
最小可相交覆盖: 先用floyd求出原图的传递闭包,即如果a->b有路径,b->c有路径,,则加边a->c.然后就转化成了最小路径覆盖问题(最小不相交路径覆盖问题). 最小路径覆盖数 ...
java 生成并覆盖文件,基于mybatis-plus生成不被覆盖的文件并支持swagger注解
情况是这样的: 原本mybatis-plus的框架的模板是不支持swagger的注解的,需要手动写. 自己折腾了1个多小时,建立在mybatis-plus的基础上进行修改.可以选择生成文件时,不覆盖某 ...
洛谷 - P2764 最小路径覆盖问题(最大流+二分图最小路径覆盖+路径打印)
题目链接:点击查看题目大意:给出一个由n个点和m条边组成的有向无环图,现在需要我们求最少可以将n个点分为多少条简单路径,并打印出每一条路径题目分析:题意挺难懂的..简单来说就是让求二分图最小路径覆 ...

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

ROS生成弓字形覆盖路径点逻辑分析相关推荐

最新文章

热门文章