behavior tree "..only_if_path_becomes_invalid.xml" leads to navigation on cancelled goal

[automech-rb]

Bug report

Required Info:

• Operating System:
  • Docker 20.04 Ubuntu
• ROS2 Version:
  • rolling
• Version or commit hash:
  • latest 104bdc5
• DDS implementation:
  • rmw_cyclonedds_cpp

Steps to reproduce issue

Use SMAC lattice planner and RPP
Send a goal which is easily (quickly) calculable by SMAC lattice
Cancel the above goal
Send a goal which is difficult to calculate (takes time)

Expected behavior

Local planner (controller) waits until new path is calculated.

Actual behavior

Controller navigates on old path which is already canceled.

Additional information

[jwallace42]

How often does this error occur for you?

[automech-rb]

HI @jwallace42

This error is very repeatable and occurs every time when a current navigation is cancelled and next goal is difficult or non-navigable (very close to obstacle). It is also very easy to emulate on any map (small or big). For a rectangular footprint robot, set goal pose very near to obstacle facing into it.

[jwallace42]

Does this only happen with SMAC and RRP? I ran with the default planners for about 10 minutes and couldn't reproduce your error.

[automech-rb]

Yeah, that is what I think. I didn't saw this error with other planners.
I will try again tomorrow to check with others (smac Hybrid-A* Planner and Navfn)

[jwallace42]

Okay,

That is pretty odd. I would assume this issue would be for any combination of planner and controller. Let me know how the testing goes. :)

[automech-rb]

Hi @jwallace42

This time I used publicly available Amazon warehouse with gazebo. Here are the results:

• SMAC Lattice + RPP :: Leads to error
• SMAC Lattice + DWB :: Leads to error
• Navfn + RPP :: Leads to error

Also, I am using navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml.

Map files are here to reproduce.

Here is the video with Navfn + RPP!

[padhupradheep]

what does the terminal outputs say?

[automech-rb]

Here is the output from cancelling to reaching wrong (previous) goal.

[component_container_isolated-2] [INFO] [1659604458.696972564] [controller_server]: Goal was canceled. Stopping the robot.
[component_container_isolated-2] [WARN] [1659604458.697013723] [controller_server]: [follow_path] [ActionServer] Client requested to cancel the goal. Cancelling.
[rviz2-1] Start navigation
[rviz2-1] [INFO] [1659604463.863938074] [_]: NavigateToPose will be called using the BT Navigator's default behavior tree.
[component_container_isolated-2] [INFO] [1659604463.864956320] [bt_navigator]: Begin navigating from current location to (5.82, -2.34)
[component_container_isolated-2] [WARN] [1659604466.781395567] [planner_server]: GridBased: failed to create plan, no valid path found.
[component_container_isolated-2] [WARN] [1659604466.781456281] [planner_server]: Planning algorithm GridBased failed to generate a valid path to (5.82, -2.34)
[component_container_isolated-2] [WARN] [1659604466.781477986] [planner_server]: [compute_path_to_pose] [ActionServer] Aborting handle.
[component_container_isolated-2] [INFO] [1659604466.795282971] [global_costmap.global_costmap]: Received request to clear entirely the global_costmap
[component_container_isolated-2] [INFO] [1659604466.796883359] [global_costmap.global_costmap]: KeepoutFilter: Subscribing to "/costmap_filter_info" topic for filter info...
[component_container_isolated-2] [INFO] [1659604466.797919041] [controller_server]: Received a goal, begin computing control effort.
[component_container_isolated-2] [INFO] [1659604466.798207755] [global_costmap.global_costmap]: KeepoutFilter: Received filter info from /costmap_filter_info topic.
[component_container_isolated-2] [INFO] [1659604466.798231274] [global_costmap.global_costmap]: KeepoutFilter: Subscribing to "/keepout_filter_mask" topic for filter mask...
[component_container_isolated-2] [WARN] [1659604466.800309296] [global_costmap.global_costmap]: KeepoutFilter: New filter mask arrived from /keepout_filter_mask topic. Updating old filter mask.
[component_container_isolated-2] [INFO] [1659604479.899863867] [collision_monitor]: Robot to slowdown for 50.000000 percents due to PolygonSlow polygon
[component_container_isolated-2] [INFO] [1659604479.999834940] [collision_monitor]: Robot to continue normal operation
[component_container_isolated-2] [INFO] [1659604487.998196860] [controller_server]: Reached the goal!
[component_container_isolated-2] [INFO] [1659604488.035258838] [bt_navigator]: Goal succeeded

[jwallace42]

So,

@automech-rb
I am pretty sure that this is a issue with this behavior tree. I have some ideas on fix. Can you verify that I am correct by running https://github.com/ros-planning/navigation2/blob/main/nav2_bt_navigator/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml and see if the same issue occurs? My guess is you won't see the issue.

@padhupradheep it looks like we don't have any logging setup for behavior trees. We should probably get this info the stack https://www.behaviortree.dev/tutorial_05_subtrees/ especially since our behavior trees are getting more complex. We can just hide the logging behind some flag.

[automech-rb]

Yeah. I think the same too. I will try to check with other xmls next week.

[jwallace42]

Alright,

I will add a pr for logging BT this weekend with will be able to verify my thoughts.

https://github.com/ros-planning/navigation2/blob/main/nav2_bt_navigator/behavior_trees/navigate_w_recovery_and_replanning_only_if_path_becomes_invalid.xml#L12

I am guessing that the flow goes as follows

1. Tries to compute path, fails clears costmap
2. Recovery node ticks again, compute path does not get ticked again because isPathValid == SUCCESS.
3. Causes success to propagate up to the pipeline sequence.
4. Follow path gets called and runs on the old path

[automech-rb]

Hi,

I tried navigate_through_poses_w_replanning_and_recovery.xml. As expected, this behaviour does not occur.

[jwallace42]

Hey,

sorry for the delay, been on vacation this past week.

I have a draft pr here for getting debug logs. This will be useful for getting more detailed logs from users in the future.
#3110.

This issue is caused by the blackboard cashe of path. On preemption we need to make sure the previous path gets invalidated so isPathValid is checking the empty path and not the previous path. I create a pr for the fix hopefully over this weekend.

Again sry for the delay :).

[automech-rb]

No problem. It's actually 1 week summer holiday next week here in Japan, so I will be out of office!
Take your time!

[SteveMacenski]

Awesome, it looks like @jwallace42 debugged and found the solution in #3114! That seems like a sane thing to do, please do test it out @automech-rb when you get a moment when back from vacation.

[automech-rb]

@jwallace42 @SteveMacenski @padhupradheep
Thank you everyone. The mentioned pr solves my issue. Tested on same gazebo environment.
Sorry for the wait.

[SteveMacenski]

No worries! Will merge.