# How does the local planner determine when the goal is reached?

Hey everyone

I have a maybe simple question, how does the local planner described in the navigation tutorial determine when the goal is reached? My question arises due to my testing of the local planner. I have 5 goals/offices in an simulated environment with odometry noise. Move_base tells whether the goal have been reached successfully but I also want the pose. So after each goal have been reached I publish the pose estimated by AMCL. I can see it fits very well with the xy goal tolerance but some times it is off by 1-3cm which it not much but I still find it very strange. The move_base should not confirm a succeeded reached goal if it is 1-3cm outside the defined tolerance. It have made me question how the local planner actually determines when a goal is reached but I can not find any detailed documentation on this and I hope someone here can help.
It might sound like nitpicking but for my master I can not omit such an obvious abnormality. My setup of the local planner is shown below:

• controller_frequency: 5.0

• TrajectoryPlannerROS:
• max_vel_x: 0.5
• min_vel_x: 0.0
• max_vel_theta: 1.0
• min_vel_theta: 0.0
• max_rotational_vel: 1.0
• min_in_place_rotational_vel: 0.75
• escape_vel: -0.20
• acc_lim_th: 1.5708
• acc_lim_x: 0.5
• acc_lim_y: 0.5

• holonomic_robot: false

• yaw_goal_tolerance: 0.7854
• xy_goal_tolerance: 0.10

• dwa: true

• simple_attractor: false
edit retag close merge delete

Sort by » oldest newest most voted

Generally, it's a good idea to have a look at the corresponding source code if the documentation is not sufficient. Have a look at the file goal_functions.cpp in base_local_planner/src, method isGoalReached (defined right at the end of the file). To me it seems like it is using the euclidean distance and the yaw of the robot orientation to check if the current pose is within the tolerances of the goal.

As you can see in the source code, the last point of the global plan is used as goal pose. I guess that all poses in the global plan are discretized with respect to the size of your global costmap which could explain the small errors you get.

more

Hey Lorenz. It seems to make sense. I tried to lower the the goal distance to 5cm (my grid size is 5cm) and this should keep the deviations under 10cm which it is also did. Out of 70 goals only one failed with a distance on 12.82cm instead of 10cm. But...

( 2012-08-21 04:52:49 -0500 )edit

I have a suspicion that AMCL is also part of the blame. If the AMCL is uncertain around the goal it will spread out particles. While it tries to converge towards the true pose of the robot an estimate might hit the boundaries of the goal and cause the planner to accept a success while the AMCL ...

( 2012-08-21 04:54:31 -0500 )edit

still converges. This would could also cause some of the deviations.

( 2012-08-21 04:56:49 -0500 )edit

I think AMCL uses the mean of the biggest particle cloud as robot pose. move_base doesn't know about these particle clouds at all. It might happen though that amcl causes 'jumps' in the robot pose, i.e. in one moment the robot is at the goal and in the next moment it is not anymore.

( 2012-08-21 04:57:42 -0500 )edit

Well, it have to happen in order to explain the results :D The dependency between grid size and goal accuracy makes perfect sense and a jumping AMCL just tops the remaining few problems. It is a rare case and the results I have also confirms it.

( 2012-08-21 05:13:02 -0500 )edit

Great. So if my answer is correct, please mark the question as answered.

( 2012-08-21 05:15:07 -0500 )edit