move_base的全局路径规划代码研究
algorithmn
其实就是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的配置，具体的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,
std::vector<geometry_msgs::PoseStamped>& plan) {
return makePlan(start, goal, default_tolerance_, plan);
}
bool GlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
double tolerance, std::vector<geometry_msgs::PoseStamped>& plan) {
double wx = start.pose.position.x;
double wy = start.pose.position.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_){
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
//设置起点为FREE_SPACE
clearRobotCell(start_pose, 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];
//将costmap的四周(边界)变为LETHAL_OBSTACLE
outlineMap(costmap_->getCharMap(), nx, ny, costmap_2d::LETHAL_OBSTACLE);
// 寻找potential array
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，提取路径
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(); nx * ny * 2, potential_array_);
}
主要是以下三个函数
可能不同的配置有不同的实现，但是每个函数的实现功能是一样的。
计算所有的可行点
namespace global_planner {
bool DijkstraExpansion::calculatePotentials(unsigned char* costs, double start_x, double start_y, double end_x, double end_y,
int cycles, float* potential) {
cells_visited_ = 0;
// priority buffers
threshold_ = lethal_cost_;
currentBuffer_ = buffer1_;
currentEnd_ = 0;
nextBuffer_ = buffer2_;
nextEnd_ = 0;
overBuffer_ = buffer3_;
overEnd_ = 0;
memset(pending_, 0, ns_ * sizeof(bool));
std::fill(potential, potential + ns_, POT_HIGH);
// set goal
int k = toIndex(start_x, start_y);
if(precise_)
{
double dx = start_x - (int)start_x, dy = start_y - (int)start_y;
dx = floorf(dx * 100 + 0.5) / 100;
dy = floorf(dy * 100 + 0.5) / 100;
potential[k] = neutral_cost_ * 2 * dx * dy;
potential[k+1] = neutral_cost_ * 2 * (1-dx)*dy;
potential[k+nx_] = neutral_cost_*2*dx*(1-dy);
potential[k+nx_+1] = neutral_cost_*2*(1-dx)*(1-dy);//*/
push_cur(k+2);
push_cur(k-1);
push_cur(k+nx_-1);
push_cur(k+nx_+2);
push_cur(k-nx_);
push_cur(k-nx_+1);
push_cur(k+nx_*2);
push_cur(k+nx_*2+1);
}else{
potential[k] = 0;
push_cur(k+1);
push_cur(k-1);
push_cur(k-nx_);
push_cur(k+nx_);
}
int nwv = 0; // max priority block size
int nc = 0; // number of cells put into priority blocks
int cycle = 0; // which cycle we're on
// set up start cell
int startCell = toIndex(end_x, end_y);
for (; cycle < cycles; cycle++) // go for this many cycles, unless interrupted
{
//
if (currentEnd_ == 0 && nextEnd_ == 0) // priority blocks empty
return false;
// stats
nc += currentEnd_;
if (currentEnd_ > nwv)
nwv = currentEnd_;
// reset pending_ flags on current priority buffer
int *pb = currentBuffer_;
int i = currentEnd_;
while (i-- > 0)
pending_[*(pb++)] = false;
// process current priority buffer
pb = currentBuffer_;
i = currentEnd_;
while (i-- > 0)
updateCell(costs, potential, *pb++);
// swap priority blocks currentBuffer_ <=> nextBuffer_
currentEnd_ = nextEnd_;
nextEnd_ = 0;
pb = currentBuffer_; // swap buffers
currentBuffer_ = nextBuffer_;
nextBuffer_ = pb;
// see if we're done with this priority level
if (currentEnd_ == 0) {
threshold_ += priorityIncrement_; // increment priority threshold
currentEnd_ = overEnd_; // set current to overflow block
overEnd_ = 0;
pb = currentBuffer_; // swap buffers
currentBuffer_ = overBuffer_;
overBuffer_ = pb;
}
// check if we've hit the Start cell
if (potential[startCell] < POT_HIGH)
break;
}
//ROS_INFO("CYCLES %d/%d ", cycle, cycles);
if (cycle < cycles)
return true; // finished up here
else
return false;计算路径path
}
}
怎么计算一个点的可行点
namespace global_planner {
float QuadraticCalculator::calculatePotential(float* potential, unsigned char cost, int n, float prev_potential) {
// get neighbors
float u, d, l, r;namespace
l = potential[n - 1];
r = potential[n + 1];
u = potential[n - nx_];
d = potential[n + nx_];
// ROS_INFO("[Update] c: %f l: %f r: %f u: %f d: %f\n",
// potential[n], l, r, u, d);
// ROS_INFO("[Update] cost: %d\n", costs[n]);
// find lowest, and its lowest neighbor
float ta, tc;
if (l < r)
tc = l;
else
tc = r;
if (u < d)
ta = u;
else
ta = d;
float hf = cost; // traversability factor
float dc = tc - ta; // relative cost between ta,tc
if (dc < 0) // tc is lowest
{
dc = -dc;
ta = tc;
}
// calculate new potential
if (dc >= hf) // if too large, use ta-only update
return ta + hf;
else // two-neighbor interpolation update
{
// use quadratic approximation
// might speed this up through table lookup, but still have to
// do the divide
float d = dc / hf;
float v = -0.2301 * d * d + 0.5307 * d + 0.7040;
return ta + hf * v;
}
}
};
从可行点中计算路径path
bool GradientPath::getPath(float* potential, double start_x, double start_y, double goal_x, double goal_y, std::vector<std::pair<float, float> >& path) {
std::pair<float, float> current;
int stc = getIndex(goal_x, goal_y);
// set up offset
float dx = goal_x - (int)goal_x;
float dy = goal_y - (int)goal_y;
int ns = xs_ * ys_;
memset(gradx_, 0, ns * sizeof(float));
memset(grady_, 0, ns * sizeof(float));
int c = 0;
while (c++<ns*4) {
// check if near goal
double nx = stc % xs_ + dx, ny = stc / xs_ + dy;
if (fabs(nx - start_x) < .5 && fabs(ny - start_y) < .5) {
current.first = start_x;
current.second = start_y;
path.push_back(current);
return true;
}
if (stc < xs_ || stc > xs_ * ys_ - xs_) // would be out of bounds
{
printf("[PathCalc] Out of bounds\n");
return false;
}
current.first = nx;
current.second = ny;
//ROS_INFO("%d %d | %f %f ", stc%xs_, stc/xs_, dx, dy);
path.push_back(current);
bool oscillation_detected = false;
int npath = path.size();
if (npath > 2 && path[npath - 1].first == path[npath - 3].first
&& path[npath - 1].second == path[npath - 3].second) {
ROS_DEBUG("[PathCalc] oscillation detected, attempting fix.");
oscillation_detected = true;
}
int stcnx = stc + xs_;
int stcpx = stc - xs_;
// check for potentials at eight positions near cell
if (potential[stc] >= POT_HIGH || potential[stc + 1] >= POT_HIGH || potential[stc - 1] >= POT_HIGH
|| potential[stcnx] >= POT_HIGH || potential[stcnx + 1] >= POT_HIGH || potential[stcnx - 1] >= POT_HIGH
|| potential[stcpx] >= POT_HIGH || potential[stcpx + 1] >= POT_HIGH || potential[stcpx - 1] >= POT_HIGH
|| oscillation_detected) {
ROS_DEBUG("[Path] Pot fn boundary, following grid (%0.1f/%d)", potential[stc], (int) path.size());
// check eight neighbors to find the lowest
int minc = stc;
int minp = potential[stc];
int st = stcpx - 1;
if (potential[st] < minp) {
minp = potential[st];
minc = st;
}
st++;
if (potential[st] < minp) {
minp = potential[st];
minc = st;
}
st++;
if (potential[st] < minp) {
minp = potential[st];
minc = st;
}
st = stc - 1;
if (potential[st] < minp) {
minp = potential[st];
minc = st;
}
st = stc + 1;
if (potential[st] < minp) {
minp = potential[st];
minc = st;
}
st = stcnx - 1;
if (potential[st] < minp) {
minp = potential[st];
minc = st;
}
st++;
if (potential[st] < minp) {
minp = potential[st];
minc = st;
}
st++;
if (potential[st] < minp) {
minp = potential[st];
minc = st;
}
stc = minc;
dx = 0;
dy = 0;
//ROS_DEBUG("[Path] Pot: %0.1f pos: %0.1f,%0.1f",
// potential[stc], path[npath-1].first, path[npath-1].second);
if (potential[stc] >= POT_HIGH) {
ROS_DEBUG("[PathCalc] No path found, high potential");
//savemap("navfn_highpot");
return 0;
}
}
// have a good gradient here
else {
// get grad at four positions near cell
gradCell(potential, stc);
gradCell(potential, stc + 1);
gradCell(potential, stcnx);
gradCell(potential, stcnx + 1);
// get interpolated gradient
float x1 = (1.0 - dx) * gradx_[stc] + dx * gradx_[stc + 1];
float x2 = (1.0 - dx) * gradx_[stcnx] + dx * gradx_[stcnx + 1];
float x = (1.0 - dy) * x1 + dy * x2; // interpolated x
float y1 = (1.0 - dx) * grady_[stc] + dx * grady_[stc + 1];
float y2 = (1.0 - dx) * grady_[stcnx] + dx * grady_[stcnx + 1];
float y = (1.0 - dy) * y1 + dy * y2; // interpolated y
// show gradients
ROS_DEBUG(
"[Path] %0.2f,%0.2f %0.2f,%0.2f %0.2f,%0.2f %0.2f,%0.2f; final x=%.3f, y=%.3f\n", gradx_[stc], grady_[stc], gradx_[stc+1], grady_[stc+1], gradx_[stcnx], grady_[stcnx], gradx_[stcnx+1], grady_[stcnx+1], x, y);
// check for zero gradient, failed
if (x == 0.0 && y == 0.0) {
ROS_DEBUG("[PathCalc] Zero gradient");
return 0;
}
// move in the right direction
float ss = pathStep_ / hypot(x, y);
dx += x * ss;
dy += y * ss;
// check for overflow
if (dx > 1.0) {
stc++;
dx -= 1.0;
}
if (dx < -1.0) {
stc--;
dx += 1.0;
}
if (dy > 1.0) {
stc += xs_;
dy -= 1.0;
}
if (dy < -1.0) {
stc -= xs_;
dy += 1.0;
}
}
//printf("[Path] Pot: %0.1f grad: %0.1f,%0.1f pos: %0.1f,%0.1f\n",
// potential[stc], dx, dy, path[npath-1].first, path[npath-1].second);
}
return false;
}
