Revision history [back]

So, here is what I did:

First, I cloned the global_planner package from source, and renamed the class to be global_replanner. So in the code that follows, if you didn't want to rename the package, you could just replace the "global_replanner" with the standard "global_planner".

Aside from the renaming changes, in the package, there were 3 files I changed: planner_core.h, planner_core.cpp, and plan_node.cpp. The main changes were in planner_core.cpp. In this file, there is a portion that is the main planning algorithm that starts with if (found_legal) { and ends with ROS_ERROR("Failed to get a plan.");. This is the portion that needs to change [Lines 306-318]. So I replaced this section with the following:

    // Start time for loop time
double start_time = ros::Time::now().toSec();

// Need functions from navfn planner for this
replanner_->setNavArr(nx,ny);
replanner_->setCostmap(costmap_->getCharMap(),true,allow_unknown_);

// Start loop for if goal is not the same as previous one
if((prev_goal_.x != goal.pose.position.x || prev_goal_.y != goal.pose.position.y)){
  // Set map start and goal in new planner
  int map_start[2];
  map_start[0] = start_x;
  map_start[1] = start_y;
  int map_goal[2];
  map_goal[0] = goal_x;
  map_goal[1] = goal_y;
  replanner_->setStart(map_goal);
  replanner_->setGoal(map_start);

  // Calculate potentials along path using Astar
  // replanner_->calcNavFnAstar();
  replanner_->setupNavFn(true);

  int astarint1 = replanner_->nx*replanner_->ny/1000;
  int astarint2 = replanner_->nx+replanner_->ny;
  // replanner_->propNavFnAstar(std::max(astarint1,astarint2));
  replanner_->propNavFnAstar(astarint2/2);

  int len = replanner_->calcPath(replanner_->nx*4);

  if (len > 0){
  ROS_DEBUG("path found");
}
  else{
ROS_DEBUG("no path found");
  }


  // Get resolution, tolerance (from parameters) and initialize some vars
  double resolution = costmap_->getResolution();
  double tol = tolerance;
  geometry_msgs::PoseStamped p,check_best_change;
  p = goal;
  check_best_change = best_pose;
  double best_sdist = DBL_MAX;

  // This will make the algorithm restart as long as it hasn't found a goal by increasing the tolerance
  bool tolerance_check = false;

  // Potential searching algorithm -- finds lowest potential closest to goal by cycling through a tolerance x tolerance sized square around the goal (if it fails, it increases the tolerance and goes through the loop again)
  while (!tolerance_check){
p.pose.position.y = goal.pose.position.y - tol;
while (p.pose.position.y <= goal.pose.position.y + tol){
  p.pose.position.x = goal.pose.position.x - tol;
  while (p.pose.position.x <= goal.pose.position.x + tol){
    unsigned int mx,my;
    double potential;
    if(!costmap_->worldToMap(p.pose.position.x,p.pose.position.y,mx,my)){
      potential = DBL_MAX;
    }
    else {
      unsigned int index = my * nx + mx;
      potential = replanner_->potarr[index];
    }
    double sdist = sq_distance(p,goal);
    if (potential < POT_HIGH && sdist < best_sdist){
      best_sdist = sdist;
      best_pose = p;
    }
    p.pose.position.x += resolution;
  }
  p.pose.position.y += resolution;
}
// Check to see if best_pose changed, if it didn't, set best pose as current position
if(check_best_change.pose.position.x == best_pose.pose.position.x && check_best_change.pose.position.y == best_pose.pose.position.y){
  tol += 1.0;
  ROS_WARN("Goal still in obstacle, increasing tolerance to: %.2f",tol);
}
else{
  tolerance_check = true;
}
  }
  // Push goal further away from obstacle by a factor of scale * distance between goal and best pose -- This is just because I felt it was getting too close to obstacles
  double diff_x,diff_y,scale;
  scale = 0.3;
  diff_x = best_pose.pose.position.x - goal.pose.position.x;
  diff_y = best_pose.pose.position.y - goal.pose.position.y;
  best_pose.pose.position.x += diff_x * scale;
  best_pose.pose.position.y += diff_y * scale;

  // If gone through this loop, that means you had a new goal
  prev_goal_.x = goal.pose.position.x;
  prev_goal_.y = goal.pose.position.y;
}

// Convert to cell coordinates
double gx,gy;
worldToMap(best_pose.pose.position.x,best_pose.pose.position.y,gx,gy);
goal_x = gx;
goal_y = gy;

// Clear the plan, just in case
plan.clear();

// Set new goal
geometry_msgs::PoseStamped new_goal = goal;
new_goal.pose.position.x = best_pose.pose.position.x;
new_goal.pose.position.y = best_pose.pose.position.y;

// Check if calculated potentials are legal and make plan if they are
bool found_legal = planner_->calculatePotentials(costmap_->getCharMap(), start_x, start_y, goal_x, goal_y, nx * ny * 2, potential_array_);
if (found_legal){
  if (getPlanFromPotential(start_x,start_y, goal_x, goal_y,new_goal,plan)){
// geometry_msgs::PoseStamped goal_copy = goal;
// goal_copy.header.stamp = ros::Time::now();
// plan.push_back(goal_copy);
  }
  else{
ROS_ERROR("Plan potential not legal. Will not plan path.");
  }
}

// Inform of total time loop took
double end_time = ros::Time::now().toSec();
double total_time = end_time - start_time;
if (total_time > 1.0){
  ROS_WARN("Total replan-loop time exceeded 1.0 s. Time was: %.2f",total_time);
}

This was essentially based on NavFN behavior, which was able to plan even when the goal was in an obstacle. The part that helps the planner escape from being in an obstacle is the //Potential searching algorithm. This takes the goal and searches in a tolerance x tolerance square for the best non-obstacle place to set as a new goal. In this way I was able to use tolerance to specify a distance to search around goal for a new obstacle-free goal.

If this doesn't make sense, or you need the other files to see what changes I have made, I can upload this to git and you can clone it.