# Move base default tolerance

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?

edit retag close merge delete

Sort by » oldest newest most voted

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

1

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

( 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!

( 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

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

( 2017-06-14 13:52:20 -0600 )edit

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.

more