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;
}