版权声明：本文为博主原创文章，未经博主允许不得转载。 https://blog.csdn.net/flyinsilence/article/details/82898797
ROS的navigation stack中nav_core包定义了机器人的全局规划、局部规划和恢复控制三种plugin接口，navigation并给出了相应的plugin。全局路径规划BaseGlobalPlanner的plugin有三种: navfn/NavfnROS，global_planner/GlobalPlanner, carrot_planner/CarrotPlanner. 其中常用的为global_planner/GlobalPlanner，它是navfn/NavfnROS的改进版本，包含Dijskstra和A*算法进行全局路径规划，本文将对其源码包global_planner进行分析。
全局规划器的重要函数主要是初始化函数initialize和路径规划函数makePlan， 在planner_core文件中。
初始化函数initialize
使用全局规划器必须要首先进行初始化，代码及注释如下：
void GlobalPlanner::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) // 使用二次差值近似的potential
p_calc_ = new QuadraticCalculator(cx, cy);
else
p_calc_ = new PotentialCalculator(cx, cy);
bool use_dijkstra;
private_nh.param("use_dijkstra", use_dijkstra, true);
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); // A*算法
bool use_grid_path;
private_nh.param("use_grid_path", use_grid_path, false);
if (use_grid_path) //路径方法
path_maker_ = new GridPath(p_calc_); // 栅格路径，从终点开始找上下或左右4个中最小的栅格直到起点
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);
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);
double costmap_pub_freq;
private_nh.param("planner_costmap_publish_frequency", costmap_pub_freq, 0.0);
make_plan_srv_ = private_nh.advertiseService("make_plan", &GlobalPlanner::makePlanService, this);
dsrv_ = new dynamic_reconfigure::Server<global_planner::GlobalPlannerConfig>(ros::NodeHandle("~/" + name));
dynamic_reconfigure::Server<global_planner::GlobalPlannerConfig>::CallbackType cb = boost::bind(
&GlobalPlanner::reconfigureCB, this, _1, _2);
dsrv_->setCallback(cb);
initialized_ = true;
} else
ROS_WARN("This planner has already been initialized, you can't call it twice, doing nothing");
}
规划函数makePlan
路径规划的主要函数是GlobalPlanner::makePlan，简化代码及注释如下：
bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
double tolerance, std::vector<geometry_msgs::PoseStamped>& plan) {
boost::mutex::scoped_lock lock(mutex_);
double wx = start.pose.position.x;
double wy = start.pose.position.y;
unsigned int start_x_i, start_y_i, goal_x_i, goal_y_i;
double start_x, start_y, goal_x, goal_y;
if (!costmap_->worldToMap(wx, wy, start_x_i, start_y_i)) {
ROS_WARN(
"The robot's start position is off the global costmap. Planning will always fail, are you sure the robot has been properly localized?");
return false;
}
if(old_navfn_behavior_){ // 兼容navfn
start_x = start_x_i;
start_y = start_y_i;
}else{
worldToMap(wx, wy, start_x, start_y);
}
wx = goal.pose.position.x;
wy = goal.pose.position.y;
if (!costmap_->worldToMap(wx, wy, goal_x_i, goal_y_i)) {
ROS_WARN_THROTTLE(1.0,
"The goal sent to the global planner is off the global costmap. Planning will always fail to this goal.");
return false;
}
if(old_navfn_behavior_){
goal_x = goal_x_i;
goal_y = goal_y_i;
}else{
worldToMap(wx, wy, goal_x, goal_y);
}
//clear the starting cell within the costmap because we know it can't be an obstacle
clearRobotCell(start, start_x_i, start_y_i);
int nx = costmap_->getSizeInCellsX(), ny = costmap_->getSizeInCellsY();
//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];
outlineMap(costmap_->getCharMap(), nx, ny, costmap_2d::LETHAL_OBSTACLE); // 地图外围轮廓设为障碍
// 计算代价
bool found_legal = planner_->calculatePotentials(costmap_->getCharMap(), start_x, start_y, goal_x, goal_y,
nx * ny * 2, potential_array_);
if(!old_navfn_behavior_)
planner_->clearEndpoint(costmap_->getCharMap(), potential_array_, goal_x_i, goal_y_i, 2);
if(publish_potential_)
publishPotential(potential_array_);
if (found_legal) {
//extract the plan
// 通过代价用path_maker_->getPath得到路径
if (getPlanFromPotential(start_x, start_y, goal_x, goal_y, goal, plan)) {
//make sure the goal we push on has the same timestamp as the rest of the plan
geometry_msgs::PoseStamped goal_copy = goal;
goal_copy.header.stamp = ros::Time::now();
plan.push_back(goal_copy);
} else {
ROS_ERROR("Failed to get a plan from potential when a legal potential was found. This shouldn't happen.");
}
}else{
ROS_ERROR("Failed to get a plan.");
}
// add orientations if needed 给路径加方向
orientation_filter_->processPath(start, plan);
//publish the plan for visualization purposes
publishPlan(plan);
delete potential_array_;
return !plan.empty();
}
A*算法
A*算法是启发式规划算法，比Dijskstra算法速度快，其算法思想可参考该博文，文中采用了堆排序对开启列表进行处理，具体代码及注释如下：
// 计算规划代价函数：
// costs为地图指针，potential为代价数组，cycles为循环次数，代码里值为2*nx*ny为地图栅格数的两倍
bool AStarExpansion::calculatePotentials(unsigned char* costs, double start_x, double start_y, double end_x, double end_y,
int cycles, float* potential) {
queue_.clear(); // queue_为启发式搜索到的向量队列：<i , cost>
int start_i = toIndex(start_x, start_y); // 起点对应的序号
queue_.push_back(Index(start_i, 0));
std::fill(potential, potential + ns_, POT_HIGH); // 代价数组值全设为∞
potential[start_i] = 0;
int goal_i = toIndex(end_x, end_y); // 终点对应的序号
int cycle = 0;
while (queue_.size() > 0 && cycle < cycles) {
Index top = queue_[0]; // 最小代价点
std::pop_heap(queue_.begin(), queue_.end(), greater1()); // 将向量front最小的代价Index放到放到最后
queue_.pop_back(); // 删除最小代价的点
int i = top.i;
if (i == goal_i) // 若是目标点则终止搜索，搜索成功
return true;
// 将代价最小点i周围点加入搜索队里并更新代价值
add(costs, potential, potential[i], i + 1, end_x, end_y);
add(costs, potential, potential[i], i - 1, end_x, end_y);
add(costs, potential, potential[i], i + nx_, end_x, end_y);
add(costs, potential, potential[i], i - nx_, end_x, end_y);
cycle++;
}
return false;
}
// 添加点并更新代价函数
void AStarExpansion::add(unsigned char* costs, float* potential, float prev_potential, int next_i, int end_x,
int end_y) {
if (next_i < 0 || next_i >= ns_) // 超出范围， ns_为栅格总数
return;
if (potential[next_i] < POT_HIGH) // 已经搜索过
return;
if(costs[next_i]>=lethal_cost_ && !(unknown_ && costs[next_i]==costmap_2d::NO_INFORMATION)) // 障碍物
return;
// p_calc_->calculatePotential() 采用简单方法计算值为costs[next_i] + neutral_cost_+ prev_potentia 地图代价+单格距离代价(初始化为50)+之前路径代价 为G
potential[next_i] = p_calc_->calculatePotential(potential, costs[next_i] + neutral_cost_, next_i, prev_potential);
int x = next_i % nx_, y = next_i / nx_;
float distance = abs(end_x - x) + abs(end_y - y);
// potential[next_i] + distance * neutral_cost_ 为总代价 F = G + H, H为到目标点距离代价
queue_.push_back(Index(next_i, potential[next_i] + distance * neutral_cost_)); // 加入搜索向量
std::push_heap(queue_.begin(), queue_.end(), greater1()); // 对加入的再进行堆排序， 把最小代价点放到front队头queue_[0]
}