`navigate_to_pose` aborts due to Path is empty.

[Tacha-S]

Bug report

Required Info:

• Operating System:
  • Ubuntu24.04
• Computer:
  • 13th Gen Intel NUC
• ROS2 Version:
  • Jazzy binaries
• Version or commit hash:
  • ros-jazzy-navigation2 1.3.4-1noble.20250124.144513
• DDS implementation:
  • Cyclone DDS

Steps to reproduce issue

Attempting to pass through a very narrow passage using MPPI and NavFn. The tree is based on the default navigate_to_pose_w_replanning_and_recovery.xml, with the robot placed in the left space and the goal set in the right space.

Occasionally, when follow_path fails, the local costmap is cleared as part of recovery. However, if NavFn, executed by rate control, fails to find a path afterward, the global costmap is then cleared as a recovery measure. Following this, follow_path halts with "path is empty."

[component_container-1] [INFO][robot.navigation.controller_server]: Optimizer reset
[component_container-1] [WARN][robot.navigation.controller_server]: Optimizer fail to compute path
[component_container-1] [INFO][robot.navigation.controller_server]: Optimizer reset
[component_container-1] [INFO][robot.navigation.controller_server]: Optimizer reset
[component_container-1] [WARN][robot.navigation.controller_server]: Optimizer fail to compute path
[component_container-1] [INFO][robot.navigation.controller_server]: Passing new path to controller.
[component_container-1] [ERROR][robot.navigation.controller_server]: Failed to make progress
[component_container-1] [INFO][robot.navigation.controller_server]: Optimizer reset
[component_container-1] [WARN][robot.navigation.controller_server]: [follow_path] [ActionServer] Aborting handle.
[component_container-1] [INFO][robot.navigation.local_costmap.local_costmap]: Received request to clear entirely the local_costmap
[component_container-1] [INFO][robot.navigation.local_costmap.local_costmap]: KeepoutFilter: Subscribing to "/robot/navigation/costmap_filter_info" topic for filter info...
[component_container-1] [INFO][robot.navigation.controller_server]: Received a goal, begin computing control effort.
[component_container-1] [INFO][robot.navigation.local_costmap.local_costmap]: KeepoutFilter: Received filter info from /robot/navigation/costmap_filter_info topic.
[component_container-1] [INFO][robot.navigation.local_costmap.local_costmap]: KeepoutFilter: Subscribing to "/robot/navigation/keepout_mask" topic for filter mask...
[component_container-1] [WARN][robot.navigation.local_costmap.local_costmap]: KeepoutFilter: New filter mask arrived from /robot/navigation/keepout_mask topic. Updating old filter mask.
[component_container-1] [INFO][robot.navigation.controller_server]: Passing new path to controller.
[component_container-1] [INFO][robot.navigation.controller_server]: Passing new path to controller.
[component_container-1] [WARN][robot.navigation.planner_server]: GridBased plugin failed to plan from (-3.34, -4.51) to (-5.73, -2.67): "Failed to create plan with tolerance of: 0.000000"
[component_container-1] [WARN][robot.navigation.planner_server]: [compute_path_to_pose] [ActionServer] Aborting handle.
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: Received request to clear entirely the global_costmap
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: Subscribing to "/robot/navigation/costmap_filter_info" topic for filter info...
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: Received filter info from /robot/navigation/costmap_filter_info topic.
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: Subscribing to "/robot/navigation/keepout_mask" topic for filter mask...
[component_container-1] [WARN][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: New filter mask arrived from /robot/navigation/keepout_mask topic. Updating old filter mask.
[component_container-1] [INFO][robot.navigation.controller_server]: Passing new path to controller.
[component_container-1] [ERROR][robot.navigation.controller_server]: Path is empty.
[component_container-1] [INFO][robot.navigation.controller_server]: Optimizer reset
[component_container-1] [WARN][robot.navigation.controller_server]: [follow_path] [ActionServer] Aborting handle.
[component_container-1] [ERROR][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: Failed to get result for compute_path_to_pose in node halt!
[component_container-1] [WARN][robot.navigation.bt_navigator]: [navigate_to_pose] [ActionServer] Aborting handle.
[component_container-1] [ERROR][robot.navigation.bt_navigator]: Goal failed

This issue does not occur when a global plan cannot be found from the beginning; in that case, spin, wait, and backup are executed correctly.

[component_container-1] [INFO][robot.navigation.amcl]: Setting pose (2253.600000): -5.113 1.551 -3.116
[component_container-1] [INFO][robot.navigation.bt_navigator]: Begin navigating from current location (-5.18, 1.51) to (-5.72, -1.86)
[component_container-1] [WARN][robot.navigation.planner_server]: GridBased plugin failed to plan from (-5.18, 1.51) to (-5.72, -1.86): "Failed to create plan with tolerance of: 0.000000"
[component_container-1] [WARN][robot.navigation.planner_server]: [compute_path_to_pose] [ActionServer] Aborting handle.
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: Received request to clear entirely the global_costmap
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: Subscribing to "/robot/navigation/costmap_filter_info" topic for filter info...
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: Received filter info from /robot/navigation/costmap_filter_info topic.
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: Subscribing to "/robot/navigation/keepout_mask" topic for filter mask...
[component_container-1] [WARN][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: New filter mask arrived from /robot/navigation/keepout_mask topic. Updating old filter mask.
[component_container-1] [WARN][robot.navigation.planner_server]: GridBased plugin failed to plan from (-5.18, 1.51) to (-5.72, -1.86): "Failed to create plan with tolerance of: 0.000000"
[component_container-1] [WARN][robot.navigation.planner_server]: [compute_path_to_pose] [ActionServer] Aborting handle.
[component_container-1] [INFO][robot.navigation.local_costmap.local_costmap]: Received request to clear entirely the local_costmap
[component_container-1] [INFO][robot.navigation.local_costmap.local_costmap]: KeepoutFilter: Subscribing to "/robot/navigation/costmap_filter_info" topic for filter info...
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: Received request to clear entirely the global_costmap
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: Subscribing to "/robot/navigation/costmap_filter_info" topic for filter info...
[component_container-1] [INFO][robot.navigation.local_costmap.local_costmap]: KeepoutFilter: Received filter info from /robot/navigation/costmap_filter_info topic.
[component_container-1] [INFO][robot.navigation.local_costmap.local_costmap]: KeepoutFilter: Subscribing to "/robot/navigation/keepout_mask" topic for filter mask...
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: Received filter info from /robot/navigation/costmap_filter_info topic.
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: Subscribing to "/robot/navigation/keepout_mask" topic for filter mask...
[component_container-1] [WARN][robot.navigation.local_costmap.local_costmap]: KeepoutFilter: New filter mask arrived from /robot/navigation/keepout_mask topic. Updating old filter mask.
[component_container-1] [WARN][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: New filter mask arrived from /robot/navigation/keepout_mask topic. Updating old filter mask.
[component_container-1] [WARN][robot.navigation.planner_server]: GridBased plugin failed to plan from (-5.18, 1.51) to (-5.72, -1.86): "Failed to create plan with tolerance of: 0.000000"
[component_container-1] [WARN][robot.navigation.planner_server]: [compute_path_to_pose] [ActionServer] Aborting handle.
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: Received request to clear entirely the global_costmap
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: Subscribing to "/robot/navigation/costmap_filter_info" topic for filter info...
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: Received filter info from /robot/navigation/costmap_filter_info topic.
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: Subscribing to "/robot/navigation/keepout_mask" topic for filter mask...
[component_container-1] [WARN][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: New filter mask arrived from /robot/navigation/keepout_mask topic. Updating old filter mask.
[component_container-1] [WARN][robot.navigation.planner_server]: GridBased plugin failed to plan from (-5.18, 1.51) to (-5.72, -1.86): "Failed to create plan with tolerance of: 0.000000"
[component_container-1] [WARN][robot.navigation.planner_server]: [compute_path_to_pose] [ActionServer] Aborting handle.
[component_container-1] [INFO][robot.navigation.behavior_server]: Running spin
[component_container-1] [INFO][robot.navigation.behavior_server]: Turning 1.57 for spin behavior.
[component_container-1] [INFO][robot.navigation.behavior_server]: spin completed successfully
[component_container-1] [WARN][robot.navigation.planner_server]: GridBased plugin failed to plan from (-5.20, 1.43) to (-5.72, -1.86): "Failed to create plan with tolerance of: 0.000000"
[component_container-1] [WARN][robot.navigation.planner_server]: [compute_path_to_pose] [ActionServer] Aborting handle.
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: Received request to clear entirely the global_costmap
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: Subscribing to "/robot/navigation/costmap_filter_info" topic for filter info...
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: Received filter info from /robot/navigation/costmap_filter_info topic.
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: Subscribing to "/robot/navigation/keepout_mask" topic for filter mask...
[component_container-1] [WARN][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: New filter mask arrived from /robot/navigation/keepout_mask topic. Updating old filter mask.
[component_container-1] [WARN][robot.navigation.planner_server]: GridBased plugin failed to plan from (-5.20, 1.43) to (-5.72, -1.86): "Failed to create plan with tolerance of: 0.000000"
[component_container-1] [WARN][robot.navigation.planner_server]: [compute_path_to_pose] [ActionServer] Aborting handle.
[component_container-1] [INFO][robot.navigation.behavior_server]: Running wait
[component_container-1] [INFO][robot.navigation.behavior_server]: wait completed successfully
[component_container-1] [WARN][robot.navigation.planner_server]: GridBased plugin failed to plan from (-5.09, 1.47) to (-5.72, -1.86): "Failed to create plan with tolerance of: 0.000000"
[component_container-1] [WARN][robot.navigation.planner_server]: [compute_path_to_pose] [ActionServer] Aborting handle.
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: Received request to clear entirely the global_costmap
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: Subscribing to "/robot/navigation/costmap_filter_info" topic for filter info...
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: Received filter info from /robot/navigation/costmap_filter_info topic.
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: Subscribing to "/robot/navigation/keepout_mask" topic for filter mask...
[component_container-1] [WARN][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: New filter mask arrived from /robot/navigation/keepout_mask topic. Updating old filter mask.
[component_container-1] [WARN][robot.navigation.planner_server]: GridBased plugin failed to plan from (-5.09, 1.47) to (-5.72, -1.86): "Failed to create plan with tolerance of: 0.000000"
[component_container-1] [WARN][robot.navigation.planner_server]: [compute_path_to_pose] [ActionServer] Aborting handle.
[component_container-1] [INFO][robot.navigation.behavior_server]: Running backup
[component_container-1] [INFO][robot.navigation.behavior_server]: backup completed successfully
[component_container-1] [WARN][robot.navigation.planner_server]: GridBased plugin failed to plan from (-5.17, 1.77) to (-5.72, -1.86): "Failed to create plan with tolerance of: 0.000000"
[component_container-1] [WARN][robot.navigation.planner_server]: [compute_path_to_pose] [ActionServer] Aborting handle.
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: Received request to clear entirely the global_costmap
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: Subscribing to "/robot/navigation/costmap_filter_info" topic for filter info...
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: Received filter info from /robot/navigation/costmap_filter_info topic.
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: Subscribing to "/robot/navigation/keepout_mask" topic for filter mask...
[component_container-1] [WARN][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: New filter mask arrived from /robot/navigation/keepout_mask topic. Updating old filter mask.
[component_container-1] [WARN][robot.navigation.planner_server]: GridBased plugin failed to plan from (-5.18, 1.79) to (-5.72, -1.86): "Failed to create plan with tolerance of: 0.000000"
[component_container-1] [WARN][robot.navigation.planner_server]: [compute_path_to_pose] [ActionServer] Aborting handle.
[component_container-1] [INFO][robot.navigation.local_costmap.local_costmap]: Received request to clear entirely the local_costmap
[component_container-1] [INFO][robot.navigation.local_costmap.local_costmap]: KeepoutFilter: Subscribing to "/robot/navigation/costmap_filter_info" topic for filter info...
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: Received request to clear entirely the global_costmap
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: Subscribing to "/robot/navigation/costmap_filter_info" topic for filter info...
[component_container-1] [INFO][robot.navigation.local_costmap.local_costmap]: KeepoutFilter: Received filter info from /robot/navigation/costmap_filter_info topic.
[component_container-1] [INFO][robot.navigation.local_costmap.local_costmap]: KeepoutFilter: Subscribing to "/robot/navigation/keepout_mask" topic for filter mask...
[component_container-1] [WARN][robot.navigation.local_costmap.local_costmap]: KeepoutFilter: New filter mask arrived from /robot/navigation/keepout_mask topic. Updating old filter mask.
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: Received filter info from /robot/navigation/costmap_filter_info topic.
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: Subscribing to "/robot/navigation/keepout_mask" topic for filter mask...
[component_container-1] [WARN][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: New filter mask arrived from /robot/navigation/keepout_mask topic. Updating old filter mask.
[component_container-1] [WARN][robot.navigation.planner_server]: GridBased plugin failed to plan from (-5.18, 1.79) to (-5.72, -1.86): "Failed to create plan with tolerance of: 0.000000"
[component_container-1] [WARN][robot.navigation.planner_server]: [compute_path_to_pose] [ActionServer] Aborting handle.
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: Received request to clear entirely the global_costmap
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: Subscribing to "/robot/navigation/costmap_filter_info" topic for filter info...
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: Received filter info from /robot/navigation/costmap_filter_info topic.
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: Subscribing to "/robot/navigation/keepout_mask" topic for filter mask...
[component_container-1] [WARN][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: New filter mask arrived from /robot/navigation/keepout_mask topic. Updating old filter mask.
[component_container-1] [WARN][robot.navigation.planner_server]: GridBased plugin failed to plan from (-5.18, 1.79) to (-5.72, -1.86): "Failed to create plan with tolerance of: 0.000000"
[component_container-1] [WARN][robot.navigation.planner_server]: [compute_path_to_pose] [ActionServer] Aborting handle.
[component_container-1] [INFO][robot.navigation.behavior_server]: Running spin
[component_container-1] [INFO][robot.navigation.behavior_server]: Turning 1.57 for spin behavior.
[component_container-1] [INFO][robot.navigation.behavior_server]: spin completed successfully
[component_container-1] [WARN][robot.navigation.planner_server]: GridBased plugin failed to plan from (-5.15, 1.81) to (-5.72, -1.86): "Failed to create plan with tolerance of: 0.000000"
[component_container-1] [WARN][robot.navigation.planner_server]: [compute_path_to_pose] [ActionServer] Aborting handle.
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: Received request to clear entirely the global_costmap
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: Subscribing to "/robot/navigation/costmap_filter_info" topic for filter info...
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: Received filter info from /robot/navigation/costmap_filter_info topic.
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: Subscribing to "/robot/navigation/keepout_mask" topic for filter mask...
[component_container-1] [WARN][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: New filter mask arrived from /robot/navigation/keepout_mask topic. Updating old filter mask.
[component_container-1] [WARN][robot.navigation.planner_server]: GridBased plugin failed to plan from (-5.15, 1.81) to (-5.72, -1.86): "Failed to create plan with tolerance of: 0.000000"
[component_container-1] [WARN][robot.navigation.planner_server]: [compute_path_to_pose] [ActionServer] Aborting handle.
[component_container-1] [WARN][robot.navigation.bt_navigator]: [navigate_to_pose] [ActionServer] Aborting handle.
[component_container-1] [ERROR][robot.navigation.bt_navigator]: Goal failed

Expected behavior

The recovery behavior is executed, and the navigate_to_pose action continues.

Actual behavior

When an empty path is shared with follow_path, the execution of follow_path is interrupted, and it aborts without executing the recovery behavior.

Reproduction instructions

Additional information

[Tacha-S]

I believe that the behavior of PipelineSequence is influencing this issue. It may occur when the timing of follow_path being ticked and compose_path_to_pose updating the path (failing to find a path) does not align properly.

[SteveMacenski]

That generally seems to me to be sensible behavior, as at least a default of Nav2. The beauty of Nav2's BT Navigator is that you can create your own navigation behavior and logic using a Behavior Tree that is run-time configurable/selectable as XML.

It seems like your robot drives and/or localized itself into the lethal zone given how confined the situation is. Due to this, the robot cannot generate a new path plan because there isn't a valid way out of the situation. This can be resolved by setting clear_under_footprint for the appropriate costmap layers so that the robot's current pose is thought to be "valid" for the purposes of getting it out of that tight spot.

However, that only works if you're barely in the lethal area, which should be correct if the costmap/footprint settings are setup correctly and not annotated to be larger than physical. Also mostly-only works if using a non-feasible planner that is grid-search based, or a feasible planner with a very liberal set of control capabilities.

[component_container-1] [WARN][robot.navigation.planner_server]: GridBased plugin failed to plan from (-3.34, -4.51) to (-5.73, -2.67): "Failed to create plan with tolerance of: 0.000000"

This is failing to compute a new path, so the path given to the controller would be empty. Though, you shouldn't be handing paths to the controller when you fail to plan and trigger recovery actions... It seems like something odd is happening in that situation:

[component_container-1] [ERROR][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: Failed to get result for compute_path_to_pose in node halt!

I'm not following you comment on the PipelineSequence, did you test something and find a bug somewhere in that? The failure to get results makes me think something's happening at the lower level of the action server or action client where results aren't being properly communicated, perhaps

[Tacha-S]

I am fully aware that the behavior server can freely modify the tree, and I have never explicitly stated that I want to follow the specific path.

However, what I want to point out is that when constructing a tree in this way, the strange transition where it fails to follow the path and does not attempt any recovery is still a bug, regardless of whether the tree can be modified.

Ideally, it should attempt a recovery behavior, think through the maximum number of retries, and then stop.

I only stated my hypothesis about PipelineSequence.
I haven’t conducted any experiments.

[SteveMacenski]

Got it - please do a bit of debugging to see if we can identify where this issue is coming from in order to know how to resolve it.

It sounds to me like potentially:

[component_container-1] [WARN][robot.navigation.planner_server]: GridBased plugin failed to plan from (-3.34, -4.51) to (-5.73, -2.67): "Failed to create plan with tolerance of: 0.000000"

This failed to find a path

[component_container-1] [WARN][robot.navigation.planner_server]: [compute_path_to_pose] [ActionServer] Aborting handle.

Thus we abort.

[component_container-1] [ERROR][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: Failed to get result for compute_path_to_pose in node halt!

(out of order), we see that we fail to get a result in the node halt when we exit a part of the BT where the nodes are being halt()-ed. I'm wondering in this case if the BT node isn't being communicated the failure of planning due to a dropped result message for some reason. Something's not right here, but without more debugging / logging I can't say for sure what at this moment.

Is this happening in a high compute / network load environment? What's your CPU, memory load? Any really specific DDS tuning?

[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: Received request to clear entirely the global_costmap

We clear the costmap, so that means that the BT is aware of the server failure, so maybe the result message is received, but not properly processed by the client? Or perhaps the workflow in the pipeline sequence is trying to terminate something already terminated due to a workflow issue?

[component_container-1] [INFO][robot.navigation.controller_server]: Passing new path to controller.

Clearly in error when the planner fails.

---

I don't discount the option it could be an edge-case of how the pipeline sequence is implemented. If you think it is this, can you highlight a workflow example of the BT node and how we could end up in that state? I just want a plausible explanation of how this could happen - so that we know what gap we need to fill.

Maybe we need some guards around the FollowPath node due to it (i.e. have the FollowPath node check if the path is non-empty before passing to the task server; if empty, itself returning false without calling the task server directly).

[Tacha-S]

[component_container-1] [INFO][robot.navigation.controller_server]: Passing new path to controller.
[component_container-1] [WARN][robot.navigation.planner_server]: GridBased plugin failed to plan from (-4.62, -4.43) to (-1.27, -4.67): "Failed to create plan with tolerance of: 0.000000"
[component_container-1] [WARN][robot.navigation.planner_server]: [compute_path_to_pose] [ActionServer] Aborting handle.
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [ComputePathToPose] RecoveryNode retrying child [1]
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: Executing "global_costmap/clear_entirely_global_costmap" service call # https://github.com/ros-navigation/navigation2/blob/ee96246985a9a85313da00e4488213ba352bdb58/nav2_behavior_tree/include/nav2_behavior_tree/bt_service_node.hpp#L160
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: Waiting for "global_costmap/clear_entirely_global_costmap" service response
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: Received request to clear entirely the global_costmap
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: Subscribing to "/soar/navigation/costmap_filter_info" topic for filter info...
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: "global_costmap/clear_entirely_global_costmap" service call successful
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [ComputePathToPose] RecoveryNode recovery successful
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: Received filter info from /soar/navigation/costmap_filter_info topic.
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: Subscribing to "/soar/navigation/keepout_mask" topic for filter mask...
[component_container-1] [WARN][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: New filter mask arrived from /soar/navigation/keepout_mask topic. Updating old filter mask.
[component_container-1] [INFO][robot.navigation.controller_server]: Passing new path to controller.
[component_container-1] [ERROR][robot.navigation.controller_server]: Path is empty.
[component_container-1] [INFO][robot.navigation.controller_server]: Optimizer reset
[component_container-1] [WARN][robot.navigation.controller_server]: [follow_path] [ActionServer] Aborting handle.
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [FollowPath] RecoveryNode retrying child [1] # https://github.com/ros-navigation/navigation2/blob/ee96246985a9a85313da00e4488213ba352bdb58/nav2_behavior_tree/plugins/control/recovery_node.cpp#L65
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [FollowPath] RecoveryNode failed # https://github.com/ros-navigation/navigation2/blob/ee96246985a9a85313da00e4488213ba352bdb58/nav2_behavior_tree/plugins/control/recovery_node.cpp#L105
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: ComputePathToPose: halt # https://github.com/ros-navigation/navigation2/blob/ee96246985a9a85313da00e4488213ba352bdb58/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp#L74
[component_container-1] [ERROR][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: Failed to get result for compute_path_to_pose in node halt!
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [NavigateWithReplanning] PipelineSequence halt at child [3]: FollowPath
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [NavigateRecovery] RecoveryNode retrying child [1]
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [NavigateRecovery] RecoveryNode failed
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: ComputePathToPose: halt
[component_container-1] [WARN][robot.navigation.bt_navigator]: [navigate_to_pose] [ActionServer] Aborting handle.
[component_container-1] [ERROR][robot.navigation.bt_navigator]: Goal failed
[component_container-1] [INFO][robot.navigation.planner_server]: Goal was canceled. Canceling planning action.
[component_container-1] [INFO][robot.navigation.planner_server]: [compute_path_to_pose] [ActionServer] Client requested to cancel the goal. Cancelling.

I have added some additional logging. I have also provided URLs indicating the specific lines where the logs were added, focusing on key areas of interest.

It seems that the recovery behavior for clearing the local costmap fails immediately. However, for the global costmap, I can see the log message:

"Executing 'global_costmap/clear_entirely_global_costmap' service call"

but for the local costmap, this message does not appear. This suggests that the clearing process for the local costmap is not being ticked at all and is instead returning a failure status right away.

I am not very familiar with this, but is it possible for executeTick() to be called while tick() does not run, resulting in a failure status being returned?

[SteveMacenski]

but for the local costmap, this message does not appear.

If you look at the BT, you can see that we have a node WouldAControllerRecoveryHelp, which checks the error code from the controller and only clears the local costmap if it thinks that it would actually help for that particular error code. Given the fact that the input path is invalid (i.e. empty), it wouldn't trigger the costmap clearing since that won't help if the input path is empty. That should explain that behavior, at least

[Tacha-S]

This part seems to be correct.

That means the issue is likely that an empty path is being provided.
I added additional logs, and according to them, compute_path_to_pose is being called and remains in the RUNNING state. However, the logs I placed in on_success, on_aborted, and on_cancelled() are not displayed. Instead, follow_path is receiving a new path (which is an empty path).

I assume that PipelineSequence running compute_path_to_pose and follow_path simultaneously is expected behavior. However, isn't it a bug that the intermediate result is being shared in the blackboard variable before compute_path_to_pose has produced a final result?

[component_container-1] [WARN][robot.navigation.planner_server]: GridBased plugin failed to plan from (-2.97, -4.43) to (-5.98, -2.66): "Failed to create plan with tolerance of: 0.000000"
[component_container-1] [WARN][robot.navigation.planner_server]: [compute_path_to_pose] [ActionServer] Aborting handle.
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [NavigateWithReplanning] PipelineSequence running at child [2]: RateController
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [NavigateWithReplanning] PipelineSequence running at child [3]: FollowPath
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: ComputePathToPose: Aborted
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [ComputePathToPose] RecoveryNode retrying child [1]
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: Executing "global_costmap/clear_entirely_global_costmap" service call
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: Waiting for "global_costmap/clear_entirely_global_costmap" service response
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: Received request to clear entirely the global_costmap
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: Subscribing to "/soar/navigation/costmap_filter_info" topic for filter info...
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: "global_costmap/clear_entirely_global_costmap" service call successful
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [ComputePathToPose] RecoveryNode recovery successful
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: Starting Action: compute_path_to_pose
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [NavigateWithReplanning] PipelineSequence running at child [2]: RateController
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [NavigateWithReplanning] PipelineSequence running at child [3]: FollowPath
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [NavigateWithReplanning] PipelineSequence running at child [2]: RateController
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [NavigateWithReplanning] PipelineSequence running at child [3]: FollowPath
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: Received filter info from /soar/navigation/costmap_filter_info topic.
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: Subscribing to "/soar/navigation/keepout_mask" topic for filter mask...
[component_container-1] [WARN][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: New filter mask arrived from /soar/navigation/keepout_mask topic. Updating old filter mask.
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [NavigateWithReplanning] PipelineSequence running at child [2]: RateController
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [NavigateWithReplanning] PipelineSequence running at child [3]: FollowPath
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [NavigateWithReplanning] PipelineSequence running at child [2]: RateController
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [NavigateWithReplanning] PipelineSequence running at child [3]: FollowPath
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [NavigateWithReplanning] PipelineSequence running at child [2]: RateController
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [NavigateWithReplanning] PipelineSequence running at child [3]: FollowPath
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [NavigateWithReplanning] PipelineSequence running at child [2]: RateController
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [NavigateWithReplanning] PipelineSequence running at child [3]: FollowPath
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [NavigateWithReplanning] PipelineSequence running at child [2]: RateController
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [NavigateWithReplanning] PipelineSequence running at child [3]: FollowPath
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [NavigateWithReplanning] PipelineSequence running at child [2]: RateController
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [NavigateWithReplanning] PipelineSequence running at child [3]: FollowPath
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [NavigateWithReplanning] PipelineSequence running at child [2]: RateController
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [NavigateWithReplanning] PipelineSequence running at child [3]: FollowPath
[component_container-1] [INFO][robot.navigation.controller_server]: Passing new path to controller.
[component_container-1] [ERROR][robot.navigation.controller_server]: Path is empty.
[component_container-1] [INFO][robot.navigation.controller_server]: Optimizer reset
[component_container-1] [WARN][robot.navigation.controller_server]: [follow_path] [ActionServer] Aborting handle.
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [NavigateWithReplanning] PipelineSequence running at child [2]: RateController
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [NavigateWithReplanning] PipelineSequence running at child [3]: FollowPath
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [NavigateWithReplanning] PipelineSequence running at child [2]: RateController
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [NavigateWithReplanning] PipelineSequence running at child [3]: FollowPath
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [NavigateWithReplanning] PipelineSequence running at child [2]: RateController
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [FollowPath] RecoveryNode retrying child [1]
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [FollowPath] RecoveryNode failed
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: ComputePathToPose: halt
[component_container-1] [ERROR][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: Failed to get result for compute_path_to_pose in node halt!
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: ComputePathToPose: Cancelled
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [NavigateWithReplanning] PipelineSequence halt at child [3]: FollowPath
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [NavigateRecovery] RecoveryNode retrying child [1]
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [NavigateRecovery] RecoveryNode failed
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: ComputePathToPose: halt
[component_container-1] [WARN][robot.navigation.bt_navigator]: [navigate_to_pose] [ActionServer] Aborting handle.
[component_container-1] [ERROR][robot.navigation.bt_navigator]: Goal failed
[component_container-1] [INFO][robot.navigation.planner_server]: Goal was canceled. Canceling planning action.

[SteveMacenski]

I assume that PipelineSequence running compute_path_to_pose and follow_path simultaneously is expected behavior.

Correct, the FollowPath is a long-running process that is essentially aways running after the first plan is provided til the end of navigation. Whenever a new path is available, it is sent to the controller and preempts the current path being tracked.

However, isn't it a bug that the intermediate result is being shared in the blackboard variable before compute_path_to_pose has produced a final result?

The path is only updated in one of these methods:

• on_success
• on_aborted
• on_canceled
• halt

If you added logging to all 4, you would have to find a log in one of them, else the path would never have been updated. I see that in the following which seems proper to me, since aborted is when the server itself cancels the action due to failure, such as when no path can be found:

[component_container-1] [WARN][robot.navigation.planner_server]: GridBased plugin failed to plan from (-2.97, -4.43) to (-5.98, -2.66): "Failed to create plan with tolerance of: 0.000000"
[component_container-1] [WARN][robot.navigation.planner_server]: [compute_path_to_pose] [ActionServer] Aborting handle.
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: ComputePathToPose: Aborted

That all looks fine to me on the failure front. In abort, we set the path to empty, so that is where the empty path comes from that is passed to the controller for some reason.

Where is Starting Action: compute_path_to_pose in your prints? I don't see at that point any actual call to the planner server in your logs at that point - so I assume its not on tick. Or, if its on tick, the planner is still computing a path so its in the RUNNING state so the path hasn't been updated but why don't we see any planner server logs? So, please add logs for action server calls to see if we call something and for the return state of ComputePathToPose to know if its in the RUNNING state. Maybe that silence is due to lack of logging rather than lack of action.

We see some of the RateController prints which makes sense if we're ticking waiting for the rate controller to trigger the compute_path_to_pose again on its set rate. What's different about that 9th FollowPath where it now magically takes the new empty path value that should have been in the path blackboard for some time of the previous 8x calls?

Can you log inside of FollowPath what its doing there / thinks the path is? Is it updating the path, does it think the path is updated? Maybe what we're seeing in logs is latency related where it takes 50ms or something to process the new goal so we see a few FollowPath prints but the first instance of it is what actually sent it to the controller server and it just takes that long to react and see the results in the logs.

That would resolve one current mystery, if true. As to the second:

The FollowPath shouldn't be called in the PipelineSequence until the RateController returns SUCCESS to move on in the pipe. As long as the rate controller and compute path to pose is returning RUNNING, we shouldn't be moving onto the FollowPath at all, I don't think. Can you log the BT node running/success/failure/idle states and figure out why its being ticked at all?

Frankly I find issues in the BT (PipelineSequence, RateController, RecoveryNode interactions) easier to debug from logging after running than static analysis of the code.

---

Change 1

The following log is interesting to me

[component_container-1] [ERROR][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: Failed to get result for compute_path_to_pose in node halt!

I see that happening when we don't fully complete the action cancelation within the BT server timeout. That timeout by default is set to 20ms - which might be too short for the planner to actually cancel and return within. I think maybe there should be a cancel_server_timeout value used here. This also indicates to me that the planner server is still working on computing a path at this point -- the return from ComputePathToPose should be RUNNING, no?

---

Change 2

It seems reasonable to me to adjust https://github.com/ros-navigation/navigation2/blob/main/nav2_behavior_tree/plugins/action/follow_path_action.cpp#L69-L73 to check goal_.path != new_path && new_path != nav_msgs::msg::Path() so that we know its not an empty, placeholder path.

That may well fully resolve the issue, though I'd like to understand why this is happening to convince myself that it isn't masking some deeper issues we should resolve more elegantly.

[Tacha-S]

The FollowPath shouldn't be called in the PipelineSequence until the RateController returns SUCCESS to move on in the pipe. As long as the rate controller and compute path to pose is returning RUNNING, we shouldn't be moving onto the FollowPath at all, I don't think. Can you log the BT node running/success/failure/idle states and figure out why its being ticked at all?

PipelineSequence executes RateController even when FollowPath is running because last_child_ticked_ remains FollowPath. As a result, RateController and FollowPath are ticked simultaneously. This behavior is also confirmed by the logs, where log PipelineSequence running at child [2]: RateController is output in

navigation2/nav2_behavior_tree/plugins/control/pipeline_sequence.cpp

Line 54 in c7a5cc5

case BT::NodeStatus::RUNNING:

.

The path is only updated in one of these methods:

• on_success
• on_aborted
• on_canceled
• halt

If you added logging to all 4, you would have to find a log in one of them, else the path would never have been updated. I see that in the following which seems proper to me, since aborted is when the server itself cancels the action due to failure, such as when no path can be found:

Can you log inside of FollowPath what its doing there / thinks the path is? Is it updating the path, does it think the path is updated? Maybe what we're seeing in logs is latency related where it takes 50ms or something to process the new goal so we see a few FollowPath prints but the first instance of it is what actually sent it to the controller server and it just takes that long to react and see the results in the logs.

I have inserted logs into all of these and made them output statuses such as ComputePathToPose: Aborted.
Additionally, when I added the log follow_path: New path received. to the path update section of FollowPath, the following occurred.

From this, it can be inferred that after ComputePathToPose: Aborted, a recovery for ComputePathToPose occurs. The recovery itself succeeds, and ComputePathToPose is executed again, but it is still in the process of computing the first path after recovery.
During this computation, FollowPath is executed and receives the result of the previous ComputePathToPose: Aborted (an empty path), leading to the Path is empty. status.

[component_container-1] [WARN][robot.navigation.planner_server]: GridBased plugin failed to plan from (-5.17, -4.24) to (-2.16, -2.93): "Failed to create plan with tolerance of: 0.000000"
[component_container-1] [WARN][robot.navigation.planner_server]: [compute_path_to_pose] [ActionServer] Aborting handle.
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [NavigateWithReplanning] PipelineSequence running at child [2]: RateController
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [NavigateWithReplanning] PipelineSequence running at child [3]: FollowPath
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: ComputePathToPose: Aborted
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [ComputePathToPose] RecoveryNode retrying child [1]
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: Executing "global_costmap/clear_entirely_global_costmap" service call
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: Waiting for "global_costmap/clear_entirely_global_costmap" service response
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: Received request to clear entirely the global_costmap
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: Subscribing to "/robot/navigation/costmap_filter_info" topic for filter info...
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: "global_costmap/clear_entirely_global_costmap" service call successful
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [ComputePathToPose] RecoveryNode recovery successful
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: Starting Action: compute_path_to_pose
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: Action: compute_path_to_pose result is resetted
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [NavigateWithReplanning] PipelineSequence running at child [2]: RateController
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: follow_path: New path received.
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [NavigateWithReplanning] PipelineSequence running at child [3]: FollowPath
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [NavigateWithReplanning] PipelineSequence running at child [2]: RateController
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [NavigateWithReplanning] PipelineSequence running at child [3]: FollowPath
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: Received filter info from /robot/navigation/costmap_filter_info topic.
[component_container-1] [INFO][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: Subscribing to "/robot/navigation/keepout_mask" topic for filter mask...
[component_container-1] [WARN][robot.navigation.global_costmap.global_costmap]: KeepoutFilter: New filter mask arrived from /robot/navigation/keepout_mask topic. Updating old filter mask.
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [NavigateWithReplanning] PipelineSequence running at child [2]: RateController
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [NavigateWithReplanning] PipelineSequence running at child [3]: FollowPath
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [NavigateWithReplanning] PipelineSequence running at child [2]: RateController
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [NavigateWithReplanning] PipelineSequence running at child [3]: FollowPath
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [NavigateWithReplanning] PipelineSequence running at child [2]: RateController
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [NavigateWithReplanning] PipelineSequence running at child [3]: FollowPath
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [NavigateWithReplanning] PipelineSequence running at child [2]: RateController
[component_container-1] [INFO][robot.navigation.bt_navigator_navigate_to_pose_rclcpp_node]: [NavigateWithReplanning] PipelineSequence running at child [3]: FollowPath
[component_container-1] [INFO][robot.navigation.controller_server]: Passing new path to controller.
[component_container-1] [ERROR][robot.navigation.controller_server]: Path is empty.
[component_container-1] [INFO][robot.navigation.controller_server]: Optimizer reset
[component_container-1] [WARN][robot.navigation.controller_server]: [follow_path] [ActionServer] Aborting handle.

[SteveMacenski]

That makes sense - what do you feel is the right solution to this?

My comment in Change 2 could resolve:

It seems reasonable to me to adjust https://github.com/ros-navigation/navigation2/blob/main/nav2_behavior_tree/plugins/action/follow_path_action.cpp#L69-L73 to check goal_.path != new_path && new_path != nav_msgs::msg::Path() so that we know its not an empty, placeholder path.

I'm also thinking we could adjust the return value of the RecoveryNode / RateController in this situation so that the PipelineSequence state is reset and we start from a clean place again so that FollowPath isn't considered running. That is the most clean solution if possible.

[Tacha-S]

Adding the content of Change 2 improve the situation.
I also agree that it would be better if we could address this through RecoveryNode, RateController, or PipelineSequence, but I couldn't come up with a good solution for those. So, I'm planning to create a PR with the content of Change 2.

[SteveMacenski]

OK! I merged and backported that FollowPath change, which should get the problem's symptoms dealt with. That may well be really the best solution, but I'm going to leave this open because I'd like to go down that other direction at a later time and see if there's a 'better' solution.

Later this year I'm going to work on BT behavior and default trees, I'll add this to that list to consider