move_base的全局路径规划代码研究

最新推荐文章于 2022-11-28 18:34:33 发布

爱不到要偷

最新推荐文章于 2022-11-28 18:34:33 发布

阅读量860

点赞数

文章标签： c/c++

原文链接：http://www.cnblogs.com/shhu1993/p/6337004.html

版权

本文深入探讨了navfn全局路径规划算法，讲解了Dijkstra或A*算法在其中的应用，以及如何计算相邻点之间的cost。重点介绍了如何计算所有可行点、单个点的可行点以及最终路径。同时提到了navfn参数配置和潜在路径的概念。

摘要由CSDN通过智能技术生成

algorithmn
parameter
code
- 主要是以下三个函数
todo

algorithmn

算法的解释

Dijkstra

其实就是A star或者Dijkstra(基于priority queue实现的)的路径规划算法，关键是相邻点之间的cost怎么计算，怎么从可行点找到path

Navfn's optimal path is based on a path's "potential"(可能可以行走的目标). The potential is the relative cost of a
path based on the distance from the goal and from the existing path itself.(怎么计算两个点之间的距离cost) It must be noted that Navfn update's each cell's potential in the potential map, or potarr(定义的potential array变量) as it's called in navfn, as it checks that cell. This way,it can step back through the potential array to find the best possible path. The potential is determined by the cost of traversing a cell (traversability factor, hf)
and the distance away that the next cell is from the previous cell.

parameter

navfn 参数

global planner

上面两个链接一个是navfn的配置，一个是具体哪个global planner的配置，具体的global planner 是可以覆盖navfn中的配置(要是大家有相同的参数)

比如下面这个参数global planner中的可以覆盖navfn中的配置：

~<name>/allow_unknown (bool, default: true)

这个参数可以让你看见potential array的图像，看计算出的cost是怎么样子(颜色深浅代表距离起始点的远近)

~<name>/visualize_potential (bool, default: false)

code

void GlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id) {
    if(!old_navfn_behavior_)
        convert_offset_ = 0.5;
    else
        convert_offset_ = 0.0;

    if (use_quadratic)
        p_calc_ = new QuadraticCalculator(cx, cy);
    else
        p_calc_ = new PotentialCalculator(cx, cy);


    if (use_dijkstra)
    {
        DijkstraExpansion* de = new DijkstraExpansion(p_calc_, cx, cy);
        if(!old_navfn_behavior_)
            de->setPreciseStart(true);
        planner_ = de;
    }
    else
        planner_ = new AStarExpansion(p_calc_, cx, cy);

    if (use_grid_path)
        path_maker_ = new GridPath(p_calc_);
    else
        path_maker_ = new GradientPath(p_calc_);
    //发布一个make_plan的service
    make_plan_srv_ = private_nh.advertiseService("make_plan", &GlobalPlanner::makePlanService, this);

}

bool GlobalPlanner::makePlanService(nav_msgs::GetPlan::Request& req, nav_msgs::GetPlan::Response& resp) {
    makePlan(req.start, req.goal, resp.plan.poses);
}

bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,