outputting tracker feedback on ComputeAndTrack BT node by alexanderjyuen · Pull Request #5327 · ros-navigation/navigation2

alexanderjyuen · 2025-07-02T19:42:15Z

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	N/A
Primary OS tested on	Ubuntu
Robotic platform tested on	Gazebo simulation
Does this PR contain AI generated software?	No

Description of contribution in a few bullet points

*Modified the ComputeAndTrackRoute BT Action node to also receive feedback and output the feedback on the black board during on_wait_for_result

Description of documentation updates required from your changes

Add routing actions to https://docs.nav2.org/behavior_trees/overview/nav2_specific_nodes.html
Add new outputs to https://docs.nav2.org/configuration/packages/bt-plugins/actions/ComputeAndTrackRoute.html

Description of how this change was tested

This change was tested with the change in Nonblocking sequence bt control node #5325 in simulation. A route was generated and FollowPath was used to navigate the path

Future work that may be required in bullet points

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

Signed-off-by: Alexander Yuen <alex@polymathrobotics.com>

Copilot

Pull Request Overview

Adds the ability for ComputeAndTrackRouteAction to receive intermediate feedback during execution and expose it on the Behavior Tree blackboard.

Extended on_wait_for_result to propagate tracker feedback fields (last_node_id, next_node_id, current_edge_id, route, path, rerouted) to the BT blackboard.
Declared corresponding BT output ports in the node’s header.
Registered the new output ports in the XML plugin definition.

Reviewed Changes

Copilot reviewed 3 out of 3 changed files in this pull request and generated no comments.

File	Description
nav2_behavior_tree/plugins/action/compute_and_track_route_action.cpp	Handle and set new feedback outputs in `on_wait_for_result`.
nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_and_track_route_action.hpp	Added BT `OutputPort` declarations for all new feedback fields.
nav2_behavior_tree/nav2_tree_nodes.xml	Added XML `<output_port>` entries for each new feedback field.

Comments suppressed due to low confidence (2)

nav2_behavior_tree/nav2_tree_nodes.xml:140

The description for current_edge_id is just "e" and may be unclear to users. Consider changing it to something more descriptive like "ID of the current edge in the route" to match the header doc.

      <output_port name="current_edge_id">"e"</output_port>

nav2_behavior_tree/plugins/action/compute_and_track_route_action.cpp:134

New output ports are added for feedback, but there aren’t any tests covering these fields. Consider adding unit or integration tests to verify that each feedback value is correctly written to the blackboard.

    setOutput("last_node_id", feedback->last_node_id);

nav2_behavior_tree/plugins/action/compute_and_track_route_action.cpp

nav2_behavior_tree/nav2_tree_nodes.xml

Signed-off-by: Alexander Yuen <alex@polymathrobotics.com>

codecov · 2025-07-07T20:42:14Z

Codecov Report

Attention: Patch coverage is 74.28571% with 9 lines in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
.../plugins/action/compute_and_track_route_action.cpp	67.85%	9 Missing ⚠️

Files with missing lines	Coverage Δ
.../plugins/action/compute_and_track_route_action.hpp	`100.00% <100.00%> (ø)`
.../plugins/action/compute_and_track_route_action.cpp	`67.74% <67.85%> (+0.04%)`	⬆️

... and 10 files with indirect coverage changes

🚀 New features to boost your workflow:

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

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_and_track_route_action.hpp

nav2_behavior_tree/plugins/action/compute_and_track_route_action.cpp

…ompute_and_track_route_action.hpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: alexanderjyuen <103065090+alexanderjyuen@users.noreply.github.com>

… feedback Signed-off-by: Alexander Yuen <alex@polymathrobotics.com>

Signed-off-by: Alexander Yuen <alex@polymathrobotics.com>

* outputting tracker feedback on BT node Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * initializing outputs Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * outputting last state on success Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * linting Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * fixed nav2_tree_nodes.xml Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * Update nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_and_track_route_action.hpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: alexanderjyuen <103065090+alexanderjyuen@users.noreply.github.com> * consolidated function to set outputs null, only setOutput with active feedback Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * add class to method Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * linting Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> --------- Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> Signed-off-by: alexanderjyuen <103065090+alexanderjyuen@users.noreply.github.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* enable_groot_monitoring_ false (#5246) Signed-off-by: Guillaume Doisy <guillaume@dexory.com> Co-authored-by: Guillaume Doisy <guillaume@dexory.com> * Add min_distance_to_obstacle parameter to RPP (#4543) * min_distance_to_obstacle Signed-off-by: Guillaume Doisy <guillaume@dexory.com> * suggestion to time base and combine Signed-off-by: Guillaume Doisy <guillaume@dexory.com> * typo Signed-off-by: Guillaume Doisy <guillaume@dexory.com> * use min_approach_linear_velocity Signed-off-by: Guillaume Doisy <guillaume@dexory.com> --------- Signed-off-by: Guillaume Doisy <guillaume@dexory.com> Co-authored-by: Guillaume Doisy <guillaume@dexory.com> * Parametrizing obstacle layer tf filter tolerance (#5261) Signed-off-by: Marco Bassa <marco.bassa@idealworks.com> * Fix backport compiler warning (#5277) Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Add missing include of algorithm in differential_motion_model.cpp (#5293) Signed-off-by: Silvio Traversaro <silvio@traversaro.it> * Remove unused unistd.h header from route_tool.cpp (#5292) Signed-off-by: Silvio Traversaro <silvio@traversaro.it> * Adding epsilon for voxel_layer precision loss (#5314) * Adding epsilon for voxel_layer precision loss Signed-off-by: bhx <1273698633@qq.com> * Update nav2_costmap_2d/plugins/voxel_layer.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_costmap_2d/plugins/voxel_layer.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_costmap_2d/plugins/voxel_layer.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: bhx <1273698633@qq.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * fix: correct ThroughActionResult type alias in would_a_planner_recovery_help_condition (#5326) The ThroughActionResult type alias was incorrectly referencing Action::Result instead of ThroughAction::Result, causing the condition to not work properly for ComputePathThroughPoses actions. Fixes #5324 Co-authored-by: claude[bot] <209825114+claude[bot]@users.noreply.github.com> * outputting tracker feedback on ComputeAndTrack BT node (#5327) * outputting tracker feedback on BT node Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * initializing outputs Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * outputting last state on success Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * linting Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * fixed nav2_tree_nodes.xml Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * Update nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_and_track_route_action.hpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: alexanderjyuen <103065090+alexanderjyuen@users.noreply.github.com> * consolidated function to set outputs null, only setOutput with active feedback Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * add class to method Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> * linting Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> --------- Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> Signed-off-by: alexanderjyuen <103065090+alexanderjyuen@users.noreply.github.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Adding slow down at target heading to RPP Controller (#5361) * Adding slow down at target heading to RPP Signed-off-by: SteveMacenski <stevenmacenski@gmail.com> * Update test_regulated_pp.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: SteveMacenski <stevenmacenski@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Include <stdexcept> in docking_exceptions.hpp for exception handling (#5363) Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Eexception rethrow in dockRobot method (#5364) Signed-off-by: Alberto Tudela <ajtudela@gmail.com> * Add global min obstacle height in voxel layer (#5389) * Add min obstacle height in voxel layer Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> * Fix linting Signed-off-by: Maurice <mauricepurnawan@gmail.com> --------- Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> Signed-off-by: Maurice <mauricepurnawan@gmail.com> * [DEX] Enforce 3 digits precision (#5398) Signed-off-by: Guillaume Doisy <guillaume@dexory.com> Co-authored-by: Guillaume Doisy <guillaume@dexory.com> * [static_layer] limit comparison precision (#5405) * [DEX] limit comparison precision Signed-off-by: Guillaume Doisy <guillaume@dexory.com> * EPSILON 1e-5 Signed-off-by: Guillaume Doisy <guillaume@dexory.com> --------- Signed-off-by: Guillaume Doisy <guillaume@dexory.com> Co-authored-by: Guillaume Doisy <guillaume@dexory.com> * corner case bin check (#5413) Signed-off-by: Guillaume Doisy <guillaume@dexory.com> Co-authored-by: Guillaume Doisy <guillaume@dexory.com> * Add IndexType definition for Nanoflann KDTree in `node_spatial_tree`. (#5420) * fix: Add KDTree type definition to include unsigned int for IndexType Signed-off-by: Ericsii <ericfengx@foxmail.com> * code format Signed-off-by: Ericsii <ericfengx@foxmail.com> --------- Signed-off-by: Ericsii <ericfengx@foxmail.com> * Smooth path even if goal pose is so much near to the robot (#5423) * Smooth path even if goal pose is so much near to the robot Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com> * Apply suggestions Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com> * Remove unnecessary diff Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com> --------- Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com> * Fix KeepoutFilter on the ARM architecture (#5436) Signed-off-by: Sushant Chavan <gitecsvc@gmail.com> * Fix missing dependency (#5460) * Support loading multiple behavior tree files as subtrees (#5426) * Support loading multiple behavior tree files as subtrees Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai> * Fix code style Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai> * Added default param value Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai> * Added recursive check to loadBehaviorTree and adapted unit test accordingly Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai> * Removed nested loadBehaviorTree check in navigators Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai> * Removed whitespace cpplint Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai> * Fixed goalReceived Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai> * Let loadbehaviorTree use its own search_directories var Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai> * PR fixes-format-lint and test Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai> * fix pointer Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai> * Added unit test for BT xml validity Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai> * CPPLint Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai> * Check non existent search directory for bt Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai> * CPPLint Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai> * Fixed BT tests Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai> * Fixed BT tests Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai> --------- Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai> * bump 1.4.0 to 1.4.1 for aug 19 sync Signed-off-by: SteveMacenski <stevenmacenski@gmail.com> * Fixing backport error Signed-off-by: SteveMacenski <stevenmacenski@gmail.com> * load balance CI Signed-off-by: SteveMacenski <stevenmacenski@gmail.com> * Fixing BT Navigator backport merge conflict issue Signed-off-by: SteveMacenski <stevenmacenski@gmail.com> --------- Signed-off-by: Guillaume Doisy <guillaume@dexory.com> Signed-off-by: Marco Bassa <marco.bassa@idealworks.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Silvio Traversaro <silvio@traversaro.it> Signed-off-by: bhx <1273698633@qq.com> Signed-off-by: Alexander Yuen <alex@polymathrobotics.com> Signed-off-by: alexanderjyuen <103065090+alexanderjyuen@users.noreply.github.com> Signed-off-by: SteveMacenski <stevenmacenski@gmail.com> Signed-off-by: Alberto Tudela <ajtudela@gmail.com> Signed-off-by: mini-1235 <mauricepurnawan@gmail.com> Signed-off-by: Maurice <mauricepurnawan@gmail.com> Signed-off-by: Ericsii <ericfengx@foxmail.com> Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com> Signed-off-by: Sushant Chavan <gitecsvc@gmail.com> Signed-off-by: Jad El Hajj <jad.elhajj@inmind.ai> Co-authored-by: Guillaume Doisy <doisyg@users.noreply.github.com> Co-authored-by: Guillaume Doisy <guillaume@dexory.com> Co-authored-by: Marco Bassa <101661130+MarcoMatteoBassa@users.noreply.github.com> Co-authored-by: Silvio Traversaro <silvio@traversaro.it> Co-authored-by: hutao <108395177+coderbaibai@users.noreply.github.com> Co-authored-by: claude[bot] <209825114+claude[bot]@users.noreply.github.com> Co-authored-by: alexanderjyuen <103065090+alexanderjyuen@users.noreply.github.com> Co-authored-by: Alberto Tudela <ajtudela@gmail.com> Co-authored-by: mini-1235 <mauricepurnawan@gmail.com> Co-authored-by: YLFeng <ericfengx@foxmail.com> Co-authored-by: Cihat Kurtuluş Altıparmak <cihataltiparmak1@gmail.com> Co-authored-by: Sushant Chavan <gitecsvc@gmail.com> Co-authored-by: Tim Clephas <tim.clephas@nobleo.nl> Co-authored-by: Jad EL HAJJ <jad.elhajj@inmind.ai>

outputting tracker feedback on BT node

d0c3103

Signed-off-by: Alexander Yuen <alex@polymathrobotics.com>

SteveMacenski requested a review from Copilot July 2, 2025 20:38

Copilot AI reviewed Jul 2, 2025

View reviewed changes

SteveMacenski requested changes Jul 2, 2025

View reviewed changes

nav2_behavior_tree/plugins/action/compute_and_track_route_action.cpp Outdated Show resolved Hide resolved

SteveMacenski reviewed Jul 2, 2025

View reviewed changes

nav2_behavior_tree/nav2_tree_nodes.xml Outdated Show resolved Hide resolved

alexanderjyuen mentioned this pull request Jul 2, 2025

Added documentation for non-blocking node ros-navigation/docs.nav2.org#719

Merged

alexanderjyuen added 4 commits July 3, 2025 18:42

initializing outputs

d9770d2

Signed-off-by: Alexander Yuen <alex@polymathrobotics.com>

outputting last state on success

16e0a8f

Signed-off-by: Alexander Yuen <alex@polymathrobotics.com>

linting

7f1efa5

Signed-off-by: Alexander Yuen <alex@polymathrobotics.com>

fixed nav2_tree_nodes.xml

25aa368

Signed-off-by: Alexander Yuen <alex@polymathrobotics.com>

SteveMacenski requested changes Jul 7, 2025

View reviewed changes

alexanderjyuen and others added 4 commits July 7, 2025 14:18

Update nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/c…

a98382b

…ompute_and_track_route_action.hpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: alexanderjyuen <103065090+alexanderjyuen@users.noreply.github.com>

consolidated function to set outputs null, only setOutput with active…

c2d2da2

… feedback Signed-off-by: Alexander Yuen <alex@polymathrobotics.com>

add class to method

a8e5dfd

Signed-off-by: Alexander Yuen <alex@polymathrobotics.com>

linting

067c252

Signed-off-by: Alexander Yuen <alex@polymathrobotics.com>

SteveMacenski approved these changes Jul 7, 2025

View reviewed changes

SteveMacenski merged commit dc92138 into ros-navigation:main Jul 7, 2025
15 of 16 checks passed

mini-1235 mentioned this pull request Jan 13, 2026

Add tracking feedback in FollowPath output port #5854

Merged

8 tasks

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

outputting tracker feedback on ComputeAndTrack BT node#5327

outputting tracker feedback on ComputeAndTrack BT node#5327
SteveMacenski merged 9 commits intoros-navigation:mainfrom
alexanderjyuen:add_feedback_for_compute_and_track

alexanderjyuen commented Jul 2, 2025

Uh oh!

Copilot AI left a comment

Uh oh!

Uh oh!

Uh oh!

codecov bot commented Jul 7, 2025 •

edited

Loading

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

Conversation

alexanderjyuen commented Jul 2, 2025

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!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull Request Overview

Reviewed Changes

Uh oh!

Uh oh!

Uh oh!

codecov bot commented Jul 7, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Codecov Report

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

codecov bot commented Jul 7, 2025 •

edited

Loading