Move base default tolerance

edit

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 ...