follow_path action keeps on running after having canceled the goal #4659
Description
Bug report
Required Info:
- Operating System:
- Custom Linux distribution
- ROS2 Version:
- Iron 2024-07-12
- Version or commit hash:
- navigation2 1.2.9
- DDS implementation:
- CycloneDDS
Steps to reproduce issue
In some cases, the follow_path action keeps on running after having canceled a navigate_to_pose goal.
Expected behavior
After canceling the navigate_to_pose goal, we would expect the controller_server to properly interrupt its follow_path action.
[controller_server-8] [INFO]: Goal was canceled. Stopping the robot.
[controller_server-8] [WARN]: [follow_path] [ActionServer] Client requested to cancel the goal. Cancelling.
Actual behavior
The bt_navigator gets a Failed to get result for compute_path_to_pose in node halt! or a Failed to get result for follow_path in node halt! error, and the controller_server keeps on trying to follow the last received path.
[bt_navigator-12] [INFO]: Starting path following
[controller_server-8] [INFO]: Received a goal, begin computing control effort.
[planner_server-9] [WARN]: Map update loop missed its desired rate of 5.0000Hz. Current loop rate is 0.9686Hz
[bt_navigator-12] [WARN]: Behavior tree engine cycle is slower than expected: 114 ms (timeout is 50 ms)
[bt_navigator-12] [WARN]: Time required to tick the tree is 114 ms
[bt_navigator-12] [WARN]: Timed out while waiting for action server to acknowledge goal request for follow_path
[bt_navigator-12] [INFO]: Pausing path following due to slow path computation (>500ms)
[planner_server-9] [INFO]: Attempting to find a path from (-12.62, 3.38) to (-54.58, 11.29).
[planner_server-9] [INFO]: SmacPlannerHybrid: Start pose in world: (-12.624103, 3.381263), in costmap (452, 103)
[planner_server-9] [INFO]: SmacPlannerHybrid: Goal pose in world: (-54.581326, 11.286437), in costmap (32, 182)
[app-21] [INFO]: Request to cancel goals on /navigate_to_pose was accepted by server, waiting for result
[controller_server-8] [WARN]: Control loop missed its desired rate of 10.0000 Hz. Current loop rate is 9.2498 Hz.
[bt_navigator-12] [ERROR]: Failed to get result for compute_path_to_pose in node halt!
[bt_navigator-12] [INFO]: Goal canceled
[bt_navigator-12] [WARN]: [navigate_to_pose] [ActionServer] Client requested to cancel the goal. Cancelling.
[controller_server-8] [WARN]: Control loop missed its desired rate of 10.0000 Hz. Current loop rate is 9.5395 Hz.
[controller_server-8] [WARN]: Control loop missed its desired rate of 10.0000 Hz. Current loop rate is 10.9403 Hz.
Additional information
In the case reported above, before that new instance of the Starting path following, in a previous attempt in the same instance of the navigate_to_pose behavior tree, the bt_navigator already encountered the following error
[bt_navigator-12] [ERROR]: Failed to get result for follow_path in node halt!
what could indicate that the bt_navigator might somehow be desynchronized with the controller_server?
Also, the map is quite big, and the planner_server takes some time to compute a path. The goal canceling occurred while the compute_path_to_pose action was still running.