Stop planner if the goal is cancelled

[bektaskemal]

Feature request

Feature description

The current implementation of PlannerServer invokes the getPlan function in a blocking manner. This means that even if a goal is cancelled, the planner computation continues until completion, leading to potential delays, especially with slower planners like Hybrid A*.

Implementation considerations

Potential implementations could be either running planner in an async way and keep checking for isCancelRequested or having a cancel service. Both requires GlobalPlanner to have a cancel interface.

[SteveMacenski]

The planners generally shouldn't be all that long running of processes (Smac Hybrid-A* might be ~400ms in semi-normal large scale use-cases, but I suppose depending on parameterization it could increase more) where we felt that this might be an issue. But its true that especially if you're BOY-planner, then that planner could be, 5-10-20-N seconds long and we might want to kill that.

We do that right now in the Smac Planner and others with planning timeouts. The planner action API contains a timeout that if we exceed, the planner exits. Its different than a cancelation for sure, but helps a portion of this issue to make sure we don't plan indefinitely.

I'd be very open to an architectural update that would allow for the in-run preemption of planning algorithms based on cancellations.

Potential implementations could be either running planner in an async way and keep checking for isCancelRequested

That makes sense, basically pass in a function to the planning plugin's main request function who is a lambda that returns the bool value of the action server's is cancel requested API. We can check this on looping over the search or to other internal libraries that can take an external exit condition. I took a quick glance at a couple of the planners and I see where that could be used:

• https://github.com/ros-planning/navigation2/blob/main/nav2_smac_planner/src/a_star.cpp#L287-L295 either in the while condition or the if statement below it. Worth benchmarking performance changes in both locations to gauge it since perhaps running it every iteration in the while would be overkill. This would cover all Smac Planners all at once
• Similarly in Theta* https://github.com/ros-planning/navigation2/blob/main/nav2_theta_star_planner/src/theta_star.cpp#L60

NavFn is a little trickier, but should also be possible in the same concept. This is something you'd be willing to take the lead on implementing and opening a PR? This seems like a good contribution for sure!

[bektaskemal]

Sounds good, I will create a PR soon for it.