本文共 3346 字，大约阅读时间需要 11 分钟。
4.2.3 GlobalPlanner工作过程
MoveBase的全局规划线程通过调用配置的实际全局规划器的makePlan方法来计算全局路径规划。基类是class BaseGlobalPlanner（navigation-kinetic\nav_core\include\nav_core\ base_global_planner.h）， 具体的全局规划器需要继承此基类。 例如，GlobalPlanner就继承此基类，并实现相应的方法。
class GlobalPlanner : public nav_core::BaseGlobalPlanner
(navigation-kinetic\global_planner\include\global_planner\planner_core.h)
ROS可用的全局规划器有navfn/NavfnROS，global_planner/GlobalPlanner 和carrot_planner。类图如下图所示：
这里主要分析GlobalPlanner全局规划器。
在4.2.0节中已经介绍了创建和初始化global planner的过程。它是在MoveBase的构造函数中创建和初始化。如下:
boost::shared_ptr<nav_core::BaseGlobalPlanner> planner_;
planner_ = bgp_loader_.createInstance(global_planner);
//使用的planner为 global_planner/GlobalPlanner
planner_->initialize(bgp_loader_.getName(global_planner), planner_costmap_ros_);
//加载和初始化相应的动态库/opt/ros/kinetic/lib/libglobal_planner.so
4.2.3.1 GlobalPlanner initialize过程
实现是在void GlobalPlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros) 函数中完成，需要costmap作为入参。
initialize(name, costmap_ros->getCostmap(), costmap_ros->getGlobalFrameID());
1）保存costmap对象的指针和frameid
costmap_ = costmap;
frame_id_ = frame_id;
2）获取costmap的CellsX, CellsY即地图的长和宽
unsigned int cx = costmap->getSizeInCellsX(),
cy = costmap->getSizeInCellsY();
3） 根据old_navfn_behavior的值设定convert_offset_
如果old_navfn_behavior为true，convert_offset_ = 0.0
如果old_navfn_behavior为false，convert_offset_ = 0.5
我们这里设置为false，所以convert_offset_的值为0.5
4） 根据use_quadratic的值分配计算器
PotentialCalculator* p_calc_；
我们设置为true，所以p_calc_ = new QuadraticCalculator(cx, cy);
5） 根据use_dijkstra设置最短路径算法。
如果为true，则使用地杰斯特拉算法，否则使用A Star算法
Expander* planner_;
6） 根据use_grid_path设置路径生成器
若为true，使用网格边界，否则，使用梯度下降法。
我们设置为false，所以使用梯度下降法。
Traceback* path_maker_;
7）发布topic和service
//发布topic /move_base/GlobalPlanner/plan
plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);
//发布topic /move_base/GlobalPlanner/potential
potential_pub_ = private_nh.advertise<nav_msgs::OccupancyGrid>("potential", 1);
//发布service /move_base/GlobalPlanner/make_plan
make_plan_srv_ = private_nh.advertiseService("", &GlobalPlanner::makePlanService, this);
4.2.3.2 makeplan的过程
Global planner线程收到action server的请求后，会调用makePlan()函数做全局路径规划。最终调用的函数为：
GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
double tolerance, std::vector<geometry_msgs::PoseStamped>& plan)
具体的工作流程为：
1) 将起点坐标和终点坐标从世界坐标系转到地图坐标系。
这里的世界坐标指的是坐标使用米为单位
地图坐标指的是将米转换为数组的下标。
怎么理解呢？
实际存储地图时，使用的是二维数组。根据地图的长和宽以及分辨率计算出二维数据的大小。
举例：地图的大小为3.0X4.0米，分辨率为0.1米/像素。则实际存储地图的二维数组大小为
30X40. 如果一个点的世界坐标为（1.0, 2.0）, 在实际二维数组中的下标就为（10, 20）
这一步就是做这个转换工作。
costmap_->worldToMap(wx, wy, start_x_i, start_y_i)
worldToMap(wx, wy, start_x, start_y)
costmap_->worldToMap(wx, wy, goal_x_i, goal_y_i)
worldToMap(wx, wy, goal_x, goal_y)
2) 将起始点的costmap标为空闲。因为机器人就在起始点，所以此点肯定没有障碍物。
clearRobotCell(start_pose, start_x_i, start_y_i);
3）重新设置相关数组的size
//make sure to resize the underlying array that Navfn uses
p_calc_->setSize(nx, ny);
planner_->setSize(nx, ny);
path_maker_->setSize(nx, ny);
potential_array_ = new float[nx * ny];
4）将costmap的所有点都标为有障碍物
outlineMap(costmap_->getCharMap(), nx, ny, costmap_2d::LETHAL_OBSTACLE);
5) 计算可用路径
bool found_legal = planner_->calculatePotentials(costmap_->getCharMap(), start_x, start_y,
goal_x, goal_y, nx * ny * 2, potential_array_);
6) 生成路径规划
getPlanFromPotential(start_x, start_y, goal_x, goal_y, goal, plan)
转载地址：https://blog.csdn.net/lclfans1983/article/details/106796227 如侵犯您的版权，请留言回复原文章的地址，我们会给您删除此文章，给您带来不便请您谅解！
发表评论
最新留言
关于作者
