【Navigation】global_planner 源码解析
全局规划器 global_planner 功能包
文章目录
- global_planner 功能包结构
- 1、plan_node.cpp
- 2、planner_core.cpp
- 3、astar.cpp
- 4、dijkstra.cpp
- 5、quadratic_calculator.cpp
- 6、grid_path.cpp
- 7、gradient_path.cpp
- 8、orientation_filter.cpp
全局规划大都基于静态地图进行规划，产生路径点，然后给到局部规划器进行局部规划，ROS 中常见的全局规划器功能包有 navfn、global_planner(Dijkstra、A*）、asr_navfn、Movelt! (常用于机械臂)等。
global_planner 功能包结构
- plan_node.cpp 文件是全局规划的入口；
- planner_core.cpp 是 global_planner 的核心，进行初始化，调用A*或者Dijkstra进行全局规划，生成搜索路径；
- astar.cpp
- dijkstra.cpp
- quadratic_calculator.cpp（二次逼近方式，常用） 和 potential_calculator（直接返回当前点代价最小值，累加前面的代价值）在生成搜索路径中用到；
- 搜索到路径后使用回溯 grid_path.cpp（栅格路径）、gradient_path.cpp（梯度路径）获得最终路径；
- 栅格路径：从终点开始找上下左右四个点中最小的栅格直到起点
- 梯度路径：从周围八个栅格中找到下降梯度最大的点
- 之后 orientation_filter.cpp 进行方向滤波。
根据nav_core提供的BaseGlobalPlanner接口：
- initialize(name, costmap) ——算法实例的选取
- makePlan(start, goal, plan)——两个步骤完成路径的生成（①计算可行点矩阵potential_array (planner_->calculatePotentials) → ②从可行点矩阵中提取路径plan (path_maker_->getPath)）
主要是以下三个实例:
- 计算“一个点”的可行性 —— p_calc_：PotentialCalculator::calculatePotential()、 QuadraticCalculator::calculatePotential()（quadratic_calculator.cpp）
- 计算“所有”的可行点 —— planner_：DijkstraExpansion::calculatePotentials()、 AStarExpansion::calculatePotentials()（astar.cpp、dijkstra.cpp）
- 从可行点中“提取路径” —— path_maker_：GridPath::getPath()、 GradientPath::getPath()（grid_path.cpp、gradient_path.cpp）
涉及到四个算法程序：A*, Dijkstra；gradient_path, grid_path
可以总结出global_planner框架：
1、plan_node.cpp
/*********************************************************************
* 这段代码是一个ROS节点，实现了一个基于代价地图（costmap）的全局路径规划器。
* 节点初始化了一个全局规划器对象 PlannerWithCostmap，并提供了ROS服务和订阅者，
* 允许外部调用路径规划服务或通过订阅目标位姿来触发路径规划。
* 其中使用了ROS中的TransformListener来获取机器人位姿的变换信息。整个节点在main函数中被初始化并启动。
*********************************************************************/
#include 类型的变量path，用于存储规划得到的路径
vector<PoseStamped> path;
// 调用makePlan函数进行路径规划，传递起始位姿为当前机器人位姿(global_pose)，目标位姿为传入的目标位姿(*goal)，规划结果存储在path中
makePlan(global_pose, *goal, path);
}
// 1、构造函数
PlannerWithCostmap::PlannerWithCostmap(string name, Costmap2DROS* cmap) :
AGlobalPlanner(name, cmap->getCostmap(), cmap->getGlobalFrameID()) {
ros::NodeHandle private_nh("~");
// 将传入的Costmap2DROS指针赋值给成员变量cmap_
cmap_ = cmap;
// 创建ROS服务，服务名为 "make_plan"，回调函数为 &PlannerWithCostmap::makePlanService，this指向当前对象
make_plan_service_ = private_nh.advertiseService("make_plan", &PlannerWithCostmap::makePlanService, this);
// 创建ROS订阅者，订阅名为 "goal" 的消息，消息类型为 rm::PoseStamped，回调函数为 &PlannerWithCostmap::poseCallback，this指向当前对象
pose_sub_ = private_nh.subscribe<rm::PoseStamped>("goal", 1, &PlannerWithCostmap::poseCallback, this);
}
} // namespace
int main(int argc, char** argv) {
ros::init(argc, argv, "a_global_planner");
tf2_ros::Buffer buffer(ros::Duration(10));
tf2_ros::TransformListener tf(buffer);
costmap_2d::Costmap2DROS lcr("costmap", buffer);
a_global_planner::PlannerWithCostmap pppp("planner", &lcr);
ros::spin();
return 0;
}
2、planner_core.cpp
//register this planner as a BaseGlobalPlanner plugin
// 首先，注册全局路径插件，使其成为ros插件，在ros中使用
PLUGINLIB_EXPORT_CLASS(a_global_planner::AGlobalPlanner, nav_core::BaseGlobalPlanner)
initialize()
// 1、
void AGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros) {
initialize(name, costmap_ros->getCostmap(), costmap_ros->getGlobalFrameID());
}
// 2、进行初始化
void AGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2D* costmap, std::string frame_id) {
if (!initialized_) {
ros::NodeHandle private_nh("~/" + name);
costmap_ = costmap;
frame_id_ = frame_id;
unsigned int cx = costmap->getSizeInCellsX(), cy = costmap->getSizeInCellsY();
// 参数：
private_nh.param("old_navfn_behavior", old_navfn_behavior_, false);
if(!old_navfn_behavior_)
convert_offset_ = 0.5;
else
convert_offset_ = 0.0;
bool use_quadratic; // 是否二次逼近获取路径
private_nh.param("use_quadratic", use_quadratic, true);
if (use_quadratic)
p_calc_ = new QuadraticCalculator(cx, cy);
else
p_calc_ = new PotentialCalculator(cx, cy);
bool use_dijkstra; // 是否使用dijkstra全局规划
private_nh.param("use_dijkstra", use_dijkstra, true);
if (use_dijkstra)
{
ROS_INFO("use_dijkstra");
DijkstraExpansion* de = new DijkstraExpansion(p_calc_, cx, cy);
if(!old_navfn_behavior_)
de->setPreciseStart(true);
planner_ = de;
}
else{
ROS_INFO("use_A_star");
planner_ = new AStarExpansion(p_calc_, cx, cy);
}
bool use_grid_path; // 是否使用栅格路径
private_nh.param("use_grid_path", use_grid_path, false);
if (use_grid_path)
path_maker_ = new GridPath(p_calc_); // new 出 path_maker_ 实例，从可行点中提取路径
else
path_maker_ = new GradientPath(p_calc_);
orientation_filter_ = new OrientationFilter(); // 方向滤波
// 路径发布
plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1);
// 视场显示，一般不用
potential_pub_ = private_nh.advertise<nav_msgs::OccupancyGrid>("potential", 1);
// 是否探索未知区域，flase--不可到达
private_nh.param("allow_unknown", allow_unknown_, true);
planner_->setHasUnknown(allow_unknown_);
// 窗口信息
private_nh.param("planner_window_x", planner_window_x_, 0.0);
private_nh.param("planner_window_y", planner_window_y_, 0.0);
private_nh.param("default_tolerance", default_tolerance_, 0.0);
private_nh.param("publish_scale", publish_scale_, 100);
private_nh.param("outline_map", outline_map_, true);
make_plan_srv_ = private_nh.advertiseService("make_plan", &AGlobalPlanner::makePlanService, this);
dsrv_ = new dynamic_reconfigure::Server<a_global_planner::GlobalPlannerConfig>(ros::NodeHandle("~/" + name));
dynamic_reconfigure::Server<a_global_planner::GlobalPlannerConfig>::CallbackType cb =
[this](auto& config, auto level){
reconfigureCB(config, level); };
dsrv_->setCallback(cb);
initialized_ = true;
} else
ROS_WARN("This planner has already been initialized, you can't call it twice, doing nothing");
}
makePlan 函数 —— 主要函数
通过输入起始位姿、目标点，返回 plan 路径结果，调用 makePlan 函数
关键方法是：
- calculatePotentials()
- 若全局规划器选择A* ，就去 astar.cpp 中找；选择 Dijkstra 就去 dijkstra.cpp 中找
- getPlanFromPotential()
// 3、
bool AGlobalPlanner::makePlan(const geometry_msgs<