feat(PlannerServer): BaseGlobalPlanner plugin accepts viapoints as part of API by sbadamikar-research · Pull Request #5995 · ros-navigation/navigation2

sbadamikar-research · 2026-03-03T09:57:37Z

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#5988
Primary OS tested on	Ubuntu 22.04
Robotic platform tested on	Gazebo simulation of Turtlebot3 Waffle
Does this PR contain AI generated software?	Noe
Was this PR description generated by AI software?	No

Description of contribution in a few bullet points

Added a new createPlan() interface to the BaseGlobalPlanner that accepts a vector of geometry_msgs/PoseStamped for intermediate viapoints (excluding goal)
The ComputePlanToPose action now accepts a list of stamped poses as intermediate viapoints that are forwarded to the planner plugin.
The ComputePlanToPose BT node accepts an input port for a std::vector of intermediate viapoints

Description of documentation updates required from your changes

Updated interface will have to be added to BaseGlobalPlanner documentation as optional
Implementation of updated interface will have to be added to Planner Plugin How-Tos as optional
ComputePathToPose documentation will have to be updated include description for viapoints

Description of how this change was tested

Performed validation through colcon test for all system and planner tests
Executed path planning with all permutations of following classes:
- Worlds:
  - TB3 Sandbox
  - TB4 Warehouse
- Planners:
  - NavFn
  - SmacPlanner2D
  - ThetaStartPlanner
- Replan frequency:
  - 0 Hz
  - 20 Hz

Tests included:

Single goal & planner throughout navigation sequence
Switching goals & planners mid navigation

Corresponding PRs:
ros-navigation/docs.nav2.org#904
ros-navigation/navigation2_tutorials#139

Future work that may be required in bullet points

Existing planner plugins may be upgraded to include viapoints for path computation
Tolerances for navigation goals may be included as part of the action goal for ComputePathToPose & ComputePathThroughPoses
Tolerances for viapoints may be included as part of action goal for ComputePathToPose & ComputePathThroughPoses

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

I think the behavior tree node is missing the new field using the action API update so that users with behavior trees can use this :-)

Otherwise, check out my comment on the ticket. I think this is good, but a contribution demonstrating use of this would be great and be a new capability that is missing (if you're open to it).

Sorry about the delay again.

SteveMacenski · 2026-03-12T21:32:31Z

+    const geometry_msgs::msg::PoseStamped & start,
+    const geometry_msgs::msg::PoseStamped & goal,
+    const nav_msgs::msg::Goals & viapoints,
+    std::function<bool()> cancel_checker)


I think we should keep 1x version of this function and require that viapoints is given (even if empty)

I would agree with that but that would necessitate any existing planners to update the function signature to be compatible. In this way existing planners don't need a change unless they intend to use waypoints.

I was thinking of adding it to a deprecation path of some sort and eventually moving to 1x version once people have had enough time to update the function signature. I am not sure what the protocol is for that with Nav2.

I think this is easy enough, its just updating to add a field const nav_msgs::msg::Goals & /*viapoints*/, and its done. We cannot backport this anyway to another distribution due to this change at all in the header + the server, so we're free to do what we need to do for code cleanliness.

That's perfect... I'll update it accordingly and check out the BT API

SteveMacenski · 2026-04-08T17:57:05Z

@sbadamikar-research any update? The Lyrical freeze is coming up here in a few weeks and I want to get this included!

sbadamikar-research · 2026-04-09T00:31:06Z

@SteveMacenski I've been moving cities for the past couple weeks and haven't been as active as I'd like to be. I should have something by EOW

SteveMacenski · 2026-04-13T18:56:46Z

Let me know when you want me to take a look! Also please rebase / pull in main when you have a chance, we did some things to rebalance our CI jobs

sbadamikar-research · 2026-04-13T23:27:24Z

@SteveMacenski The branch is rebased. Please have a look at your convenience. I have another branch where I am working on the planners that I will create a draft PR for you to have a look and provide feedback.

SteveMacenski · 2026-04-14T00:55:28Z

This is good timing, the Lyrical freeze will be in a couple of weeks so we want to get this in ASAP so its in Lyrical, else it would not be backportable after we run the first release due to the API breaking changes

codecov · 2026-04-14T01:14:09Z

Codecov Report

✅ All modified and coverable lines are covered by tests.

Files with missing lines	Coverage Δ
...ree/plugins/action/compute_path_to_pose_action.hpp	`100.00% <100.00%> (ø)`
...ree/plugins/action/compute_path_to_pose_action.cpp	`71.79% <100.00%> (-11.99%)`	⬇️
...anner/include/nav2_navfn_planner/navfn_planner.hpp	`100.00% <ø> (ø)`
nav2_navfn_planner/src/navfn_planner.cpp	`94.82% <100.00%> (+0.06%)`	⬆️
nav2_planner/src/planner_server.cpp	`91.81% <100.00%> (+0.05%)`	⬆️
nav2_smac_planner/src/smac_planner_2d.cpp	`94.09% <100.00%> (-0.87%)`	⬇️
nav2_smac_planner/src/smac_planner_hybrid.cpp	`90.47% <100.00%> (+0.04%)`	⬆️
nav2_smac_planner/src/smac_planner_lattice.cpp	`91.12% <100.00%> (+0.04%)`	⬆️
nav2_theta_star_planner/src/theta_star_planner.cpp	`91.95% <100.00%> (+0.18%)`	⬆️

... and 12 files with indirect coverage changes

🚀 New features to boost your workflow:

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

…rt of API The ComputePlanToPose action now incorporates a list of stamped poses as intermediate viapoints that are forwarded to the planner plugin. Signed-off-by: Sanchit B <researchbadamikar@gmail.com>

Signed-off-by: Sanchit B <researchbadamikar@gmail.com>

Pass non-empty viapoints vectors in planner unit tests to exercise the RCLCPP_WARN branch in createPlan() for planners that ignore viapoints (smac_2d, smac_hybrid, smac_lattice, theta_star, navfn). Signed-off-by: Sanchit B <researchbadamikar@gmail.com>

SteveMacenski · 2026-04-20T18:52:58Z

Docs PR is all that's left

…rt of API (ros-navigation#5995) * feat(PlannerServer): BaseGlobalPlanner plugin accepts viapoints as part of API The ComputePlanToPose action now incorporates a list of stamped poses as intermediate viapoints that are forwarded to the planner plugin. Signed-off-by: Sanchit B <researchbadamikar@gmail.com> * fix(planners): Improved logging of skipped viapoints and cleaned imports Signed-off-by: Sanchit B <researchbadamikar@gmail.com> * test(planners): Add test coverage for viapoint warning logs Pass non-empty viapoints vectors in planner unit tests to exercise the RCLCPP_WARN branch in createPlan() for planners that ignore viapoints (smac_2d, smac_hybrid, smac_lattice, theta_star, navfn). Signed-off-by: Sanchit B <researchbadamikar@gmail.com> --------- Signed-off-by: Sanchit B <researchbadamikar@gmail.com> Signed-off-by: Tony Najjar <tony.najjar@dexory.com>

The SmacPlannerHybrid and SmacPlannerLattice test files in this branch still called the 3-arg createPlan(start, goal, cancel_checker) signature. On main, BaseGlobalPlanner::createPlan gained a viapoints parameter (commit 51e3125, PR ros-navigation#5995), and main's test files were updated to the 4-arg form. Build Against Released Distributions CI fails on jazzy and kilted because this branch's test files diverged. Mirror main's pattern: declare a per-TEST std::vector<geometry_msgs::msg::PoseStamped> no_viapoints{} alongside dummy_cancel_checker, and pass it as the third argument to all 8 createPlan call sites in test_smac_hybrid.cpp and test_smac_lattice.cpp. No production-code changes. Smoother behaviour under test is unchanged. Signed-off-by: Akihiko Komada <aki1770@gmail.com>

sbadamikar-research force-pushed the feature/plugin_viapoints branch 2 times, most recently from 2e5afd9 to 7833cb2 Compare March 3, 2026 11:04

SteveMacenski reviewed Mar 12, 2026

View reviewed changes

sbadamikar-research force-pushed the feature/plugin_viapoints branch 6 times, most recently from fb0861c to 633be13 Compare April 11, 2026 13:37

sbadamikar-research force-pushed the feature/plugin_viapoints branch from 633be13 to d1217f7 Compare April 13, 2026 23:17

SteveMacenski reviewed Apr 14, 2026

View reviewed changes

Comment thread nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp

Comment thread nav2_smac_planner/src/smac_planner_lattice.cpp

sbadamikar-research and others added 3 commits April 20, 2026 12:38

feat(PlannerServer): BaseGlobalPlanner plugin accepts viapoints as pa…

a021e8f

…rt of API The ComputePlanToPose action now incorporates a list of stamped poses as intermediate viapoints that are forwarded to the planner plugin. Signed-off-by: Sanchit B <researchbadamikar@gmail.com>

fix(planners): Improved logging of skipped viapoints and cleaned imports

42ad2de

Signed-off-by: Sanchit B <researchbadamikar@gmail.com>

sbadamikar-research force-pushed the feature/plugin_viapoints branch from d1217f7 to aca3e5d Compare April 20, 2026 18:40

sbadamikar-research mentioned this pull request Apr 20, 2026

docs(planner): updating docs for global planner accepting viapoints. ros-navigation/docs.nav2.org#904

Merged

SteveMacenski approved these changes Apr 20, 2026

View reviewed changes

SteveMacenski merged commit 51e3125 into ros-navigation:main Apr 20, 2026
28 checks passed

Conversation

sbadamikar-research commented Mar 3, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Basic Info

Description of contribution in a few bullet points

Description of documentation updates required from your changes

Description of how this change was tested

Future work that may be required in bullet points

For Maintainers:

Uh oh!

SteveMacenski left a comment

Choose a reason for hiding this comment

Uh oh!

SteveMacenski Mar 12, 2026

Choose a reason for hiding this comment

Uh oh!

sbadamikar-research Mar 12, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

SteveMacenski Mar 12, 2026

Choose a reason for hiding this comment

Uh oh!

sbadamikar-research Mar 12, 2026

Choose a reason for hiding this comment

Uh oh!

SteveMacenski commented Apr 8, 2026

Uh oh!

sbadamikar-research commented Apr 9, 2026

Uh oh!

SteveMacenski commented Apr 13, 2026

Uh oh!

sbadamikar-research commented Apr 13, 2026

Uh oh!

Uh oh!

Uh oh!

SteveMacenski commented Apr 14, 2026

Uh oh!

codecov Bot commented Apr 14, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Codecov Report

Uh oh!

SteveMacenski commented Apr 20, 2026

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

sbadamikar-research commented Mar 3, 2026 •

edited

Loading

sbadamikar-research Mar 12, 2026 •

edited

Loading

codecov Bot commented Apr 14, 2026 •

edited

Loading