Ask Your Question
1

Move base default tolerance

asked 2017-04-27 11:49:24 -0600

juanlu gravatar image

updated 2017-04-27 12:04:42 -0600

Hi,

Im implementing an explorer algorithm that takes the map from a SLAM algorithm, processes it and sends a goal to move base. The problem is that sometimes the goal is not away enough from a wall and move base takes a long time to stop replanning and reject the goal (up to several minutes).

I saw the parameter "default_tolerance", by planning to a goal within a tolerance raidus, should be the answer to my prayers but a bit of investigating here in ros answers and in the code showed me that the parameter has no effect whatsoever in the execution of the code.

Does anybody have any suggestion on how to fix this?

Thanks in advance.

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
3

answered 2017-04-27 12:19:10 -0600

emielke gravatar image

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 ...
(more)
edit flag offensive delete link more

Comments

1

You ought to make a pull request to fix this, IMO. It's an annoying issue.

AndyZe gravatar imageAndyZe ( 2017-04-27 17:43:48 -0600 )edit

Hi emielke, thanks for fixing this! It really is annoying. Would you mind sharing your modifications to plan_node.cpp and planner_core.h as well? They are probably minor, but I'm having some trouble compiling the updated code, so I'm probably missing something somewhere. Thanks!

Sherry Haibara gravatar imageSherry Haibara ( 2017-06-14 07:21:38 -0600 )edit

I'll include a link to my repo that has all the changes made to it, and you can look there. It was mostly including the navfn stuff that took some work. Here is the link

emielke gravatar imageemielke ( 2017-06-14 13:02:26 -0600 )edit

Thanks for the link! If you're not aware, there seems to be a bug on line 285 in gradient_path.cpp where xs_+1 should probably be n+xs_. They discussed the bug here, not sure about nx vs xs...

Sherry Haibara gravatar imageSherry Haibara ( 2017-06-14 13:52:20 -0600 )edit
1

answered 2017-04-28 14:50:46 -0600

AndyZe gravatar image

updated 2017-04-28 14:53:17 -0600

You'll probably have to tweak your local planner to fix this. I'm still using pose_follower, and I made some changes just after:

while(fabs(diff.linear.x) <= tolerance_trans_ &&
      fabs(diff.linear.y) <= tolerance_trans_ &&
      fabs(diff.angular.z) <= tolerance_rot_)
{

^This is where the tolerances are checked. Originally there was:

if(current_waypoint_ < global_plan_.size() - 1)
{
  current_waypoint_++;
  tf::poseStampedMsgToTF(global_plan_[current_waypoint_], target_pose);
  diff = diff2D(target_pose, robot_pose);
}

^Notice, no stop command unless it's the last waypoint. Change the code so it does stop, regardless of whether it's the last waypoint:

    goal_position = false;
    while(fabs(diff.linear.x) <= tolerance_trans_ &&
          fabs(diff.linear.y) <= tolerance_trans_ &&
          fabs(diff.angular.z) <= tolerance_rot_)
    {
      // We're close enough, despite not being at the final waypoint. Stop here.
      if(current_waypoint_ < global_plan_.size() - 1)
      {
        current_waypoint_++;
        tf::poseStampedMsgToTF(global_plan_[current_waypoint_], target_pose);
        diff = diff2D(target_pose, robot_pose);
        geometry_msgs::Twist empty_twist;
        cmd_vel = empty_twist; // Stop
      }
      else
      {
        ROS_INFO("Reached goal: %d", current_waypoint_);
        in_goal_position = true;
        break;
      }
    }

I'll try to make a PR for this.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2017-04-27 11:49:24 -0600

Seen: 776 times

Last updated: Apr 28 '17