Backport GetCurrentPose, ArePosesNear, and ConcatenatePaths BT nodes for Route Server in Jazzy, and minimal BT for navigation.

[paureverte]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#5754
Primary OS tested on	Ubuntu
Robotic platform tested on	Gazebo simualtion of Turtlebot-3 and Gazebo simulation of Robotnik's RBVOGUI
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

• Backported three basic Behavior Tree (BT) nodes into the Jazzy distribution to enable full Route Graph navigation: GetCurrentPose, ArePosesNear, and ConcatenatePaths.

• Added a minimal example Behavior Tree (nav2_bt_navigator/behavior_trees/nav_to_pose_with_routes.xml) demonstrating the core sequencing required to navigate: From an arbitrary starting point to the route graph, along the route graph and from the route graph exit to an off-route final goal.

Description of documentation updates required from your changes

• The new minimal BT example should be referenced or included in the Nav2 documentation to show the required sequence for off-route start/goal navigation using the Route Server.

Description of how this change was tested

• Changes were tested by compiling successfully against the Jazzy distribution.
• Extensive validation in Gazebo simulation using the Route Server package.
• Tested with various robot configurations and different route definitions to ensure the minimal BT successfully executes the navigation sequence from off-route start/goal points.

Note: No dedicated unit tests or integration tests have been written yet, only simulation verification of the functionality.

---

Future work that may be required in bullet points

• Adding full recovery behaviors and replanning logic to the minimal BT example to match the robustness of the standard navigate_to_pose tree.

• Writing dedicated unit tests for the new GetCurrentPose, ArePosesNear, and ConcatenatePaths BT nodes.

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-*.

[mergify]

@preverte, all pull requests must be targeted towards the main development branch.
Once merged into main, it is possible to backport to @jazzy, but it must be in main
to have these changes reflected into new distributions.

[mini-1235]

@preverte A few requests to get this merged:

• Please sign with DCO
• Please fix the linting
• Please try to keep it as close to the main branch as possible, and only make changes where absolutely necessary. If you have any improvements to add, please open a PR targeting main first, then we can backport it to Jazzy

[SteveMacenski]

To add to what @mini-1235 said, where are the tests? Those should be backported too

[codecov]

Codecov Report

❌ Patch coverage is 90.47619% with 8 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
...ree/plugins/condition/are_poses_near_condition.cpp	83.33%	5 Missing ⚠️
...or_tree/plugins/action/get_current_pose_action.cpp	89.47%	2 Missing ⚠️
...r_tree/plugins/action/concatenate_paths_action.cpp	93.75%	1 Missing ⚠️

Files with missing lines	Coverage Δ
...r_tree/plugins/action/concatenate_paths_action.hpp	100.00% <100.00%> (ø)
...or_tree/plugins/action/get_current_pose_action.hpp	100.00% <100.00%> (ø)
...ree/plugins/condition/are_poses_near_condition.hpp	100.00% <100.00%> (ø)
...r_tree/plugins/action/concatenate_paths_action.cpp	93.75% <93.75%> (ø)
...or_tree/plugins/action/get_current_pose_action.cpp	89.47% <89.47%> (ø)
...ree/plugins/condition/are_poses_near_condition.cpp	83.33% <83.33%> (ø)

... and 18 files with indirect coverage changes

🚀 New features to boost your workflow:

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

[paureverte]

@SteveMacenski , @mini-1235 , thank you for the suggestions.

Please, find updated branch with DCO sign, linting fixed and corrections suggested above. Also, test files for new nodes have been included, as well as compute_route server handling for nav2_system_tests testing of BTs in nav2_bt_navigator

[mini-1235]

I will review tomorrow

[mini-1235]

I still see quite a few differences after the latest update. Please, please keep this 1:1 with the main branch so it's easier to review and maintain for future backports. We would really appreciate any improvements added to the main branch first, once merged, we can backport them to Jazzy

[paureverte]

@mini-1235 , I updated the code to be as close as possible with main branch.

Regarding the BT xml, I just deleted it as it is not part of the backport just like you mentioned, maybe I will add it as some new feature in the main branch.

[mini-1235]

Thanks for reiterating