I'm using a SimpleActionClient to issue MoveBaseGoals to an instance of /move_base in a flat outdoor environment. If I have the "xy_tolerance" parameter for baselocalplanner set to ~0.35m or larger then everything works well without any problems. However, If I reduce the tolerance to 0.25m in an effort to improve navigational accuracy, actionlib stops recognising that it has successfully reached the goal - even if it is repeatably within this tolerance in both the X,Y and Z axis (not that there is a vertical tolerance from what I understand). See below for an example bag file

In this case, the state of the goal remains stuck on '1' (labelled as ACTIVE) and will remain that way indefinitely until I manually cancel it.

So my question is: since all I have changed to cause this behaviour is "xy_tolerance", are there any hidden conditions that must be fulfilled for actionlib to move the state from 1 (ACTIVE) to 3 (SUCCEEDED) other than getting the robot within this distance of the goal point? Or am I likely just hitting the limit on how accurate ROS can be in an outdoor environment? The GPS I'm using is accurate to 20mm, as is the global fused data (verified experimentally).

I'm aware that there are a few other questions that address this issue. However, for the most part they seem to experience this problem regardless, whereas I only see it if I lower "xy_tolerance". So I believe the causes may be different.

Any help or advice is appreciated.

UPDATE 1: I must have been using a slightly older version of move_base and actionlib because I recently updated all my ROS packages and now the goal detection is significantly worse, to the point where it almost never correctly detects if it's reached the goal point, even if it's well within tolerance.

UPDATE 2: As suggested by @David Lu I increased the yaw to Pi (3.14) and conducted a test run. While the larger yaw tolerance significantly increased the rate of goals being successfully recognised, there were still several instances where the robot was within both the X,Y and yaw tolerances and yet it still didn't correctly recognise reaching the goal. I'll upload the bag file shortly.

I've looked through the source code for the internal goal check (goal_functions.cpp) and I noticed that the function checks the last pose in the global path (which is just an array of poses) against the current global position, whereas I'm using the original goal sent to the robot vs the current global position. So is it possible that the global path could have been re-calculated and ended up with a goal pose in a different place to the original?

UPDATE 3: As per @David Lu's suggestion (thanks for the help again!) I checked the end of the global path and the current goal:

• /movebase/currentgoal
• /movebase/TrajectoryPlannerROS/globalplan

Both almost perfectly match the goal I originally send to the SimpleActionServer (maybe out by 1mm) and if I've read the source code right, actionlib checks the global path to see if it's reached the goal (rather than say checking the local path). So it's definitely reaching the goal, even by actionlib's own standards.

UPDATE 4: I don't think I've solved the problem, but I've learned a bit more about what's going on with my specific system. As mentioned, I'm using a SimpleActionServer/Client to issue move goals to an instance of the /movebase node. All my goals are issued in the global /map frame (which fuses GPS data so It's accurate over long distances). By default the /movebase node plots a global path in the /map frame, but it plots the local path and issues move commands in the /odom frame, which is inaccurate and can drift quite badly over long distances.

The kicker is, even if you issue a move goal in the /map frame, that will not necessarily be the goal the robot attempts to reach. The /move_base node deals with two goal related topics:

• /move_base/goal The /movebase node publishes and subscribes to this topic, and this is the topic you or the SimpleActionServer can publish to in order to issue a move goal to the /movebase node. This should therefore always match the goal you as the user want the robot to reach.
• /movebase/currentgoal This is a PoseStamped message that contains the goal the robot is actually trying to reach after it has been converted to the /odom frame. So if there is any drift between the /map and /odom frame, this goal will actually be in a different location. Also, from what I've seen, this is the point that move_base node and the SimpleActionServer check to see if they got to within the required tolerance.

So if the robot is within the tolerance of the goal specified in /movebase/currentgoal it will mark the goal as succeeded, regardless of the distance to the goal you actually specified in /move_base/goal. I've only roughly checked all this so I could be wrong or it could be something specific to my system. But hopefully it helps.

The navigation parameters can be changed, so I've changed the globalframeid for the baselocalplanner from /odom to /map. This forces the local planner to operate in the /map frame, so the goal should be the same in both topics. After making this change I've noticed a big improvement, its not perfect but most of the time it correctly marks the goal as succeeded if it got within tolerance. I still find that the local path drifts quite a bit, so my robot no longer travels in straight lines but that problem may be unrelated.

System Information

• OS: Ubuntu 14.04 (laptop), ROS Indigo Igloo (robot)
• Robot: Clearpath Robotics Jackal UGV (real, not simulated)
• Sensors: IMU, wheel encoders, high accuracy RTK GPS
• Localization: package used is robot_localization. IMU and encoder data is fused in the /odom frame. IMU, encoder and GPS data is fused in the /map frame - this is the frame /move_base uses for navigation. I can provide the specific launch files if requested.
• Navigation: I use the jackal_navigation package (version 2.2.2) to set up my navigation stack. I use the odomnavigationdemo.launch file with the only changes being that I've changed "xy_tolerance" as described above, and increased the size of my costmap to 120x120m. I've also written my own SimpleActionClient which I use to issue move goals in the /map frame.
• Frame tree: tree
• Example of error: example_data.bag, I can provide an example of working data if requested.