ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
3

Move base default tolerance

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

juanlu gravatar image

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

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
5

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

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 image AndyZe  ( 2017-04-27 17:43:48 -0500 )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 image Sherry Haibara  ( 2017-06-14 07:21:38 -0500 )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 image emielke  ( 2017-06-14 13:02:26 -0500 )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 image Sherry Haibara  ( 2017-06-14 13:52:20 -0500 )edit

Thanks! But is this just me or now move_base won't take into consideration yaw anymore? The robot arrives at correct XY, but always turns to yaw = 0, informing "goal reached".

felipeduque gravatar image felipeduque  ( 2020-05-05 10:12:52 -0500 )edit

When I made these changes I wasn't so concerned with yaw, so that behavior seems like what I was experiencing. Unfortunately I no longer have access to the code base I tested this on, so I can't confirm that is the behavior. Perhaps you could look into adding a check for yaw as well, as the code above only really checks the x and y locations of the pose and doesn't take any other dimensions into consideration.

emielke gravatar image emielke  ( 2020-05-14 11:27:57 -0500 )edit

I did that, and noticed that indeed the goal's quaternion was being set to (0, 0, 0, 1) in the original global_planner. I changed that and things are working fine now.

felipeduque gravatar image felipeduque  ( 2020-05-15 06:38:07 -0500 )edit
1

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

AndyZe gravatar image

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

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

Question Tools

2 followers

Stats

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

Seen: 2,155 times

Last updated: Apr 28 '17