Bug report

Required Info:

• Operating System:
  • Ubuntu 18.04
• Version or commit hash:
  current master branch
  ecc42fb
• RMW:
  CycloneDDS

Steps to reproduce issue

I've tested my robot in a 2x2 meter square area with current master branch. I intentionally make my robot stuck in a corner and the robot has no way to escape. Then I send a goal asking my robot to navigate to another corner. There is no valid path can be generated from the robot's position to the goal because my robot is stuck. However, the navigation system just tells me the robot reaches the goal even my robot is (1.5 meter) far from the goal.

It took me a while to figure out until I found the tolerance of navfn_planner is set to 2.0 meters according to this discussion. After I change the value back to 0.0, it looks reasonable because the planner_server failed to create a plan. But there is another issue: planner_server aborts its handle and no replanning or recovery happens.

Expected behavior

What I expected is the stuck robot should NOT be able to create a valid path and then going to replanning or recovery by following navigate_w_replanning_and_recovery.xml.

Actual behavior

When the tolerance of navfn_planner is set to 2.0:
Navigation succeed but actually the robot is still stuck at the same place.

When the tolerance of navfn_planner is set to 0.0:
ComputePathToPoseAction keeps ticking on_wait_for_result() even the planner server has aborted its handle. In this situation, new goal can't preempt the current goal until I restart navigation stack.