移动机器人全覆盖路径规划及仿真（四.全覆盖路径规划）

移动机器人全覆盖路径规划及仿真（四.全覆盖路径规划）

算法流程

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;
}