移动机器人全覆盖路径规划及仿真(四.全覆盖路径规划)
1.规划到从初始点到第一个cell左上角路径;
2.深度优先搜索链接每一个cell;
3.对每个cell内做BoustrophedonPath全覆盖路径规划;
4.寻找进入下一个区域入口;
5.寻找路口连接路径。
std::deque<std::deque<Point2D>> StaticPathPlanning(const cv::Mat& map, std::vector<CellNode>& cell_graph, const Point2D& start_point, int robot_radius, bool visualize_cells, bool visualize_path, int color_repeats=10)
{
cv::Mat3b vis_map;
cv::cvtColor(map, vis_map, cv::COLOR_GRAY2BGR);
std::deque<std::deque<Point2D>> global_path;
std::deque<Point2D> local_path;
int corner_indicator = TOPLEFT;
int start_cell_index = DetermineCellIndex(cell_graph, start_point).front();
std::deque<Point2D> init_path = WalkInsideCell(cell_graph[start_cell_index], start_point, ComputeCellCornerPoints(cell_graph[start_cell_index])[TOPLEFT]);
local_path.assign(init_path.begin(), init_path.end());
std::deque<CellNode> cell_path = GetVisittingPath(cell_graph, start_cell_index);
std::deque<cv::Scalar> JetColorMap;
InitializeColorMap(JetColorMap, color_repeats);
std::deque<Point2D> inner_path;
std::deque<std::deque<Point2D>> link_path;
Point2D curr_exit;
Point2D next_entrance;
std::deque<int> return_cell_path;
std::deque<Point2D> return_path;
for(int i = 0; i < cell_path.size(); i++)
{
inner_path = GetBoustrophedonPath(cell_graph, cell_path[i], corner_indicator, robot_radius);
local_path.insert(local_path.end(), inner_path.begin(), inner_path.end());
cell_graph[cell_path[i].cellIndex].isCleaned = true;
if(i < (cell_path.size()-1))
{
curr_exit = inner_path.back();
next_entrance = FindNextEntrance(curr_exit, cell_path[i+1], corner_indicator);
link_path = FindLinkingPath(curr_exit, next_entrance, corner_indicator, cell_path[i], cell_path[i+1]);
local_path.insert(local_path.end(), link_path.front().begin(), link_path.front().end());
global_path.emplace_back(local_path);
local_path.clear();
local_path.insert(local_path.end(), link_path.back().begin(), link_path.back().end());
}
}
global_path.emplace_back(local_path);
if(visualize_cells||visualize_path)
{
cv::waitKey(0);
}
return global_path;
}
---------------------
作者:tutu_321
来源:CSDN
原文:https://blog.csdn.net/sinat_38625360/article/details/106125889
版权声明:本文为作者原创文章,转载请附上博文链接!
内容解析By:CSDN,CNBLOG博客文章一键转载插件