Planner Server: Allow planning until last reachable pose before obstacle

[jpcastillo26]

Feature request

Feature description

Currently, the compute_path_through_poses action in the planner server fails when any obstacle completely blocks the path to any of the intermediate poses and there are no alternative paths, regardless of how far away the obstacle is. This all-or-nothing approach prevents the robot from navigating to any of the reachable poses in the sequence.

This feature request proposes implementing partial path planning for compute_path_through_poses, allowing the robot to navigate to the last reachable pose before encountering the obstacle. This would be useful in dynamic environments where temporary obstacles may block the path completely for a prolonged period of time.

The robot would successfully navigate through all poses it can reach and stop at the last valid pose, rather than failing while still far away from the obstacle on route to a distant goal.

Implementation considerations

Proposed Solution:
Modify the computePlanThroughPoses() function (or create new action) in planner_server.cpp to allow for partial path planning:

1. Sequential planning with failure tolerance: Continue planning through the pose sequence, but don't fail completely when a single segment fails
2. Partial path validation: Allow the action to succeed with a partial path that reaches the last accessible pose
3. First goal validation: Still fail if the very first goal is unreachable (no partial path possible)

Implementation Details:

• Replace immediate NoValidPathCouldBeFound exception with warning logging and loop continuation
• Add final validation to ensure at least one valid path segment exists
• Maintain backward compatibility - existing behavior for single-goal planning unchanged
• Preserve all existing error handling for other failure modes (TF errors, invalid planners, etc.)

Pros:

• Enables partial mission completion when full path is blocked

Cons:

• Changes behavior semantics (partial success vs complete failure)
• May require client applications to handle partial path results

Alternative Approaches:

1. New action type: Create a new action to avoid breaking existing behavior
2. Parameter-based: Add a parameter to enable/disable partial planning mode

[SteveMacenski]

Neat idea and actually this has been proposed before by a sponsor but they worked it out easily internal to their stack so it was never raised publicly.

Do you see this as a new action API request field or a parameter configuration? Either way, its easy to set the defaults to the current behavior prevails.

I would think we wouldn't want to skip poses, but return after the first failure the partial path possible (whereas the first segment is strictly required to provide back anything).

With those questions, I think we should be good to go for an implementation assuming you're open to contributing!

[armgits]

I'd like to contribute to this feature!

[SteveMacenski]

Please do! Do you have any questions?

[armgits]

Nothing right now, I will let you know

[armgits]

I'm back on working on this feature. I was going through the code and didn't see any unit tests specific to the function, but I saw this test in another package. I'm trying to understand how to update the tests to validate new changes? Should I write a unit test by adding a new cpp file within the planner server package or modify this system test file?

[SteveMacenski]

A unit test for this feature would be ideal!

[armgits]

Sounds great! I'm already on it and wanted to make sure that I was heading in the right way.

[armgits]

I got stuck here for a while when creating the unit tests:

I've created 2 test cases for the existing code, one without any obstacles and one with an obstacle in an attempt to recreate the case where there's no valid path. But in the test case with an obstacle, I still get no errors from the planner server and the path returned is exactly the same as the one without obstacles.

How I set the map/obstacles:
I've set the costmap to use only static layer. And I publish an occupancy grid with lethal cells for the static layer to subscribe and update the costmap. This part is entirely done before sending the action request to obtain the path through poses.

I can also share the code if anyone wants to take a look at it, what would be the best way to do that? I'll continue to keep working on this in the meanwhile...

Visual of the 2nd test case (first case is similar without obstacle in the middle):

The dashed blue line is what the planner server outputs regardless of the obstacle in middle for both the test cases.

[SteveMacenski]

Its hard to say without looking at the code. Are you sure the static layer received and processed the update with the obstacle before you attempted to plan with it? One way to deal with this would be to manually set the costmap values like we do in other unit tests rather than sending over a topic. If this is all done in a unit test with a single process, then you could be able to use the planner server test fixture to access the global costmap to set the values manually.

[armgits]

I did initially try updating the costmap values like in other unit tests. The issue with that approach here is the planner server doesn't allow planning unless costmap layer(s) have updated data by checking the current_ flag which is set to True only when the layer receives and processes the map.

And here's the log for that part and I do think that the map is received by static layer and updated before attempting to plan. This is supposed to fail while attempting to plan between (0.5, 0.3) and (0.5, 0.7) due to the obstacle.

[8.391s] 8: [INFO] [1763087848.007424411] [tester_node]: Adding obstacle at: 50, 50, with size 100, 10
[8.392s] 8: [INFO] [1763087848.008172312] [tester_node]: Publishing map...
[8.392s] 8: [DEBUG] [1763087848.008339262] [planner_server]: [compute_path_through_poses] [ActionServer] Received request for goal acceptance
[8.392s] 8: [DEBUG] [1763087848.008346666] [global_costmap.global_costmap]: StaticLayer: Process map
[8.392s] 8: [DEBUG] [1763087848.008403815] [global_costmap.global_costmap]: StaticLayer: Received a 100 X 100 map at 0.010000 m/pix
[8.392s] 8: [INFO] [1763087848.008420337] [global_costmap.global_costmap]: StaticLayer: Resizing costmap to 100 X 100 at 0.010000 m/pix
[8.392s] 8: [DEBUG] [1763087848.008396331] [planner_server.rclcpp_action]: Accepted goal c29e370c-a46e-3c43-bee5-a2466c647ab8
[8.392s] 8: [DEBUG] [1763087848.008482827] [planner_server]: [compute_path_through_poses] [ActionServer] Receiving a new goal
[8.393s] 8: [DEBUG] [1763087848.008495942] [planner_server]: [compute_path_through_poses] [ActionServer] Executing goal asynchronously.
[8.393s] 8: [DEBUG] [1763087848.008612525] [planner_server]: [compute_path_through_poses] [ActionServer] Executing the goal...
[8.393s] 8: [INFO] [1763087848.008633244] [planner_server]: Computing path through poses to goal.
[8.393s] 8: [DEBUG] [1763087848.008648243] [planner_server]: Attempting to a find path from (0.50, 0.10) to (0.50, 0.30).
[8.393s] 8: [DEBUG] [1763087848.008668562] [planner_server]: Making plan from (0.50,0.10) to (0.50,0.30)
[8.393s] 8: [DEBUG] [1763087848.008763454] [planner_server]: Path found, 38 steps, 5882445824.000000 cost
[8.393s] 8: 
[8.393s] 8: [DEBUG] [1763087848.008789193] [planner_server]: Found valid path of size 38 to (0.50, 0.30)
[8.394s] 8: [DEBUG] [1763087848.008824891] [planner_server]: Attempting to a find path from (0.50, 0.30) to (0.50, 0.70).
[8.394s] 8: [DEBUG] [1763087848.008837595] [planner_server]: Making plan from (0.50,0.30) to (0.50,0.70)
[8.394s] 8: [DEBUG] [1763087848.008976942] [planner_server]: Path found, 79 steps, 5882445824.000000 cost
[8.394s] 8: 
[8.394s] 8: [DEBUG] [1763087848.009007220] [planner_server]: Found valid path of size 79 to (0.50, 0.70)
[8.394s] 8: [DEBUG] [1763087848.009027879] [planner_server]: Attempting to a find path from (0.50, 0.70) to (0.50, 0.90).
[8.394s] 8: [DEBUG] [1763087848.009038460] [planner_server]: Making plan from (0.50,0.70) to (0.50,0.90)
[8.394s] 8: [DEBUG] [1763087848.009107021] [planner_server]: Path found, 38 steps, 5882445824.000000 cost
[8.394s] 8: 
[8.394s] 8: [DEBUG] [1763087848.009123142] [planner_server]: Found valid path of size 38 to (0.50, 0.90)
[8.395s] 8: [DEBUG] [1763087848.009165673] [planner_server]: [compute_path_through_poses] [ActionServer] Setting succeed on current goal.
[8.395s] 8: [DEBUG] [1763087848.009261456] [planner_server]: [compute_path_through_poses] [ActionServer] Blocking processing of new goal handles.
[8.395s] 8: [DEBUG] [1763087848.009278950] [planner_server]: [compute_path_through_poses] [ActionServer] Done processing available goals.
[8.395s] 8: [DEBUG] [1763087848.009284180] [planner_server]: [compute_path_through_poses] [ActionServer] Worker thread done.

[SteveMacenski]

5882445824

This is a bit suspicious. Why is this already the same for all of them? Also, that's a ... very large number. If you visualize the path, what does it look like?

It could be that since these are small (i.e. 20 cm apart) that if you have the tolerance of the planning set to non-zero or 20+ cm, then it is failing to find an exact path, but succeeds to find a path that meets the tolerances. What if you make these 2 meters apart instead of 20 cm?

[armgits]

I'll definitelty try increasing the spacing and reduce the overall map size to see if anything changes. And here are the path visualizations for both the cases:

Path without obstacles

Output path with obstacles

[armgits]

I just was able to fix it by adding a small delay using sleep in between publishing the map, letting the costmap to update and sending goal request. Now the planner fails to plan with the obstacle as expected. Now that this is clear, I'll make the required changes, update the unit test and create a pull request as soon as possible.