Planner Server: Add support for planning partial paths when planning through poses

[armgits]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#5535
Primary OS tested on	Ubuntu
Robotic platform tested on	--
Does this PR contain AI generated software?	No
Was this PR description generated by AI software?	No

---

Description of contribution in a few bullet points

• Planner server can now plan partial paths when planning through poses in the presence of obstacles
• This behavior can be enabled/disabled through a new dynamic parameter: allow_partial_planning

Description of documentation updates required from your changes

• The new parameter needs to be added to planner server configuration guide

• Partial plan indicator last_reached_index in the action result field needs to be added as an output port to ComputePathThroughPoses behavior tree plugin configuration guide

Description of how this change was tested

• I wrote unit test to test the new behavior while planning through poses with various obstacle positions and new parameter settings. I've outputted the paths from these tests to files and manually evaluated them to ensure correctness.

---

Future work that may be required in bullet points

• Parameter naming might be better
• Exception handling and logging changes done for this feature might need to be reviewed

For Maintainers:

• Check that any new parameters added are updated in docs.nav2.org
• Check that any significant change is added to the migration guide
• Check that any new features OR changes to existing behaviors are reflected in the tuning guide
• Check that any new functions have Doxygen added
• Check that any new features have test coverage
• Check that any new plugins is added to the plugins page
• If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists
• Should this be backported to current distributions? If so, tag with backport-*.

[SteveMacenski]

@armgits please take a look at the failing linting / precommit jobs. The copyright would be yours / your company's (if done for a company) if its code you wrote. If you copy+pasted it with minimum modifications from another file in Nav2, keep that original copyright.

[SteveMacenski]

Do you think we shuold add a new field to the action definition that indicates if the output path is partial or not? https://github.com/ros-navigation/navigation2/blob/main/nav2_msgs/action/ComputePathThroughPoses.action

I think that would be important for an application to be aware of.

Otherwise, mostly just small things for my preference for conciseness!

[SteveMacenski]

LGTM! I think the new Action API field is the only missing element (+ the BT node to parse it to put in an output port; 3 lines)

[armgits]

Do you think we shuold add a new field to the action definition that indicates if the output path is partial or not?

I was thinking if adding a new error code could do the same thing... Otherwise adding a new field to the result works too, I'm fine with either way to indicate a partial path in result.

(+ the BT node to parse it to put in an output port; 3 lines)

So is the job of this BT node just to evaluate whether the result from planner server is partial?
I'm guessing there's already a BT Node that just needs to be modified.

[SteveMacenski]

If the setting is 'true' that it is value to return a partial one, then I think that's not an error code-situation (i.e. a failure). I think we should add a field if a partial one was used and/or maybe even include the index of the goals that the final output represents?

I'm guessing there's already a BT Node that just needs to be modified.

Yeah, just use the outputPort method in the terminal conditions to parse the value (or reset in case of error) -- you'll see in the ComputePathThroughPoses BT node, its no biggie

[armgits]

I think we should add a field if a partial one was used and/or maybe even include the index of the goals that the final output represents?

So this would be a boolean field to indicate partial plan + a list of indices in the order of poses given in goal request? Would the final index just before obstacle work instead?

[mergify]

@armgits, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

[SteveMacenski]

So this would be a boolean field to indicate partial plan + a list of indices in the order of poses given in goal request? Would the final index just before obstacle work instead?

I think just the final index would work. Optionally: you could have this in a single field for the index of the goal -- whereas its set to -1 when it includes all goals. It could be an enum in the message definition like these https://github.com/ros-navigation/navigation2/blob/main/nav2_msgs/action/ComputePathThroughPoses.action#L11 (obviously needs to be signed) which can be set by default and overrided only when the failure condition happens.

Then any use case of this can just check if whatever-final-goal-variable != ComputePathThroughPoses::Result::ALL_GOALS to see if its not inclusive of all, rather than storing duplicate information in the Result message

[armgits]

you could have this in a single field for the index of the goal -- whereas its set to -1 when it includes all goals. It could be an enum in the message definition

This sounds perfect, I'll add this field to the action API.

[mergify]

@armgits, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

[mergify]

@armgits, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

[mergify]

@armgits, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

[SteveMacenski]

Otherwise, assuming the tests cover the code sufficiently, this looks good to me! Just some touch up details and this is good to ship 🚢

[SteveMacenski]

@armgits check out the docs PR, I asked for a small change there and otherwise once CI passes we can merge!

[codecov]

Codecov Report

❌ Patch coverage is 88.46154% with 3 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
...ugins/action/compute_path_through_poses_action.cpp	60.00%	2 Missing ⚠️
nav2_planner/src/planner_server.cpp	95.00%	1 Missing ⚠️

Files with missing lines	Coverage Δ
...ugins/action/compute_path_through_poses_action.hpp	100.00% <100.00%> (ø)
nav2_planner/src/planner_server.cpp	92.98% <95.00%> (+0.05%)	⬆️
...ugins/action/compute_path_through_poses_action.cpp	72.97% <60.00%> (-2.03%)	⬇️

... and 8 files with indirect coverage changes

🚀 New features to boost your workflow:

• ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

[SteveMacenski]

Just waiting on doc updates! CI's happy