全局规划器 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 <a_global_planner/planner_core.h>
#include <navfn/MakeNavPlan.h>
#include <boost/shared_ptr.hpp>
#include <costmap_2d/costmap_2d_ros.h>
#include <tf2_ros/transform_listener.h>namespace cm = costmap_2d;
namespace rm = geometry_msgs;using std::vector;
using rm::PoseStamped;
using std::string;
using cm::Costmap2D;
using cm::Costmap2DROS;namespace a_global_planner {// 创建一个继承自AGlobalPlanner的类，名为PlannerWithCostmap
class PlannerWithCostmap : public AGlobalPlanner {public:PlannerWithCostmap(string name, Costmap2DROS* cmap);bool makePlanService(navfn::MakeNavPlan::Request& req, navfn::MakeNavPlan::Response& resp);private:void poseCallback(const rm::PoseStamped::ConstPtr& goal);Costmap2DROS* cmap_;ros::ServiceServer make_plan_service_;ros::Subscriber pose_sub_;
};// 2、实现makePlanService服务
bool PlannerWithCostmap::makePlanService(navfn::MakeNavPlan::Request& req, navfn::MakeNavPlan::Response& resp) {vector<PoseStamped> path;req.start.header.frame_id = "map";req.goal.header.frame_id = "map";// 调用makePlan函数进行路径规划bool success = makePlan(req.start, req.goal, path);resp.plan_found = success;if (success) {resp.path = path;}return true;
}// 3、处理目标位姿的回调函数
void PlannerWithCostmap::poseCallback(const rm::PoseStamped::ConstPtr& goal) {geometry_msgs::PoseStamped global_pose;// 通过Costmap2DROS对象获取机器人当前的全局位姿信息cmap_->getRobotPose(global_pose);// 创建一个vector<PoseStamped>类型的变量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);
}} // namespaceint 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;elseconvert_offset_ = 0.0;bool use_quadratic; // 是否二次逼近获取路径private_nh.param("use_quadratic", use_quadratic, true);if (use_quadratic)p_calc_ = new QuadraticCalculator(cx, cy);elsep_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_ 实例，从可行点中提取路径elsepath_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;} elseROS_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<
 方法)
