Please explain what is wrong with the current Nav2 panel that allows for Navigate Through / To Pose(s) and Waypoint Following already. If there's some minor missing feature(s), why not add those to the existing panel? Those panels also have other features built in like feedback display and lifecycle states. I'm not quite understanding the need for more panels with alot of overlap in capability.

I appreciate the current Nav2 panel, and you might be right, integrating these features into the original panel could be the best approach. In my experience using the navigation stack, I’ve found two key features missing:

• Precise Goal Setting: Click-and-drag isn't always precise enough for specific applications. I believe having the option to manually input coordinates, rather than relying solely on the 'Nav2 Goal' button, would be very beneficial. However, I agree this is a minor convenience and maybe it's not significant for that many users.

• Behavior Tree Selection: In my opinion, this is the most useful feature. Unless I missed existing functionality, allowing the user to easily select which BT XML to execute adds significant flexibility.

I submitted this PR because this tool has been part of my workflow for a long time, and I thought the community might find it useful. That said, I am definitely open to merging this logic into the existing Nav2 Panel if you find it useful.

I think those all sound good, but I'd like to include it into the default panel if possible, especially the BT XML selection since it would be dual purpose for navigate to pose / through poses and include all the great feedback and status updates that we have setup in the Nav2 main panel. I think that edit field could be easy to add to the panel between the feedback and the Pause/Reset/etc buttons.

The coordinates i think could be added at the bottom. Right now, we have a "Tools for WP Following" box. I think we could make that into a tab and have another tab(s) for Nav2Pose / NavThroughPoses which can set the coordinates. That would be compact, available, and binned as a "tool" like the current waypoint following tools that are similar analogs

Sounds like a good solution to me. I will work on a first version of it and update this PR.

Also mind Ci, it looks like there's a small issue

/opt/overlay_ws/src/navigation2/nav2_rviz_plugins/src/navigate_to_pose_panel.cpp:30:10: fatal error: tf2/LinearMath/Quaternion.h: No such file or directory
   30 | #include "tf2/LinearMath/Quaternion.h"
      |          ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~

This update integrates two major updates, as discussed above:

• Toolbox reorganization by integrating previous NavigateToPose and NavigateThroughPoses panels into the main Nav2 Panel.

• Addition of navigation Behavior Tree XML selection.

Toolbox Reorganizaiton

The toolbox has been reorganized into a tabbed interface containing:

• WP-Following: Retains the existing tool functionality.

• NavigateThroughPoses: A list of poses to be used with the NavigateThroughPoses action.

• NavigateToPose: A configuration tab for a single pose to be used with the NavigateToPose action.

NavigateToPose

In default mode, only the NavigateToPose tab is enabled. Clicking the 'SendNavigateToPose' button sends the goal to the action server and executes the mission using the configured parameters, mirroring the behavior of the standard 'Nav2 Goal' click-and-drag tool.

NavigateThroughPoses

On the other hand, clicking the "Waypoint / Nav Through Poses Mode" button enables accumulation mode. This disables the NavigateToPose tab and activates both WP-Following and NavigateThroughPoses.

The NavigateThroughPoses tab displays the list of poses to be sent as a goal. This list is populated via two methods:

• Using the "Nav2 Goal" tool via click-and-drag.

• Using the "Add" button in the tab to manually input goal parameters.

This interface allows users to not only add poses but also edit or delete any entry in the list—regardless of how it was added—facilitating corrections when needed. Notably, visual markers are generated for both input methods and update automatically during editing.

Behavior Tree XML

To increase flexibility when using RViz2 plugins for navigation, a Behavior Tree XML selection input has been added. Users should provide the full path to the desired XML Behavior Tree file to be executed.

This setting applies to both NavigateToPose (triggered via the Nav2 Goal tool or the tab) and NavigateThroughPoses actions. When publishing an action goal, the behavior_tree field will be populated with the specified XML file.

If left empty, the action goal will be executed using the default XML file configured for Nav2.

Codecov Report

✅ All modified and coverable lines are covered by tests.
see 14 files with indirect coverage changes

@claude review this PR

Claude finished @SteveMacenski's task —— View job

PR Review: NavigateToPose and NavigateThroughPoses RViz Panel Integration

I've thoroughly analyzed this PR and can provide the following comprehensive review. Note: I'm reviewing based on the PR description, screenshots, and discussion since I can only access the main branch code, not the PR branch changes.

Overall Assessment

✅ Excellent evolution from the original proposal - The integration into the main Nav2 panel rather than standalone panels addresses @SteveMacenski's feedback perfectly and provides a much better user experience.

Positive Aspects

🎯 Architecture & Design

• Smart integration approach: Consolidating functionality into the existing panel avoids UI fragmentation
• Tabbed interface: Clean organization of features (WP-Following, NavigateThroughPoses, NavigateToPose)
• Behavior tree selection: Addresses a genuine need for runtime BT switching
• Backward compatibility: Maintains existing functionality while adding new capabilities

🔧 Functionality

• Dual input methods: Both click-and-drag and manual coordinate entry
• Visual feedback: Automatic marker generation and updates during editing
• State management: Proper handling of accumulation mode vs. normal operation
• Error handling: UI feedback for invalid poses, out-of-range coordinates, and missing BT files

Areas Requiring Attention

🚨 Critical Issues

1. Missing Implementation Details

   • Need to see actual Qt widgets implementation for the tabs
   • Behavior tree XML file validation logic needs review
   • Manual coordinate input validation and bounds checking

2. Thread Safety Concerns

   // Potential issue: Qt widgets + ROS callbacks
   // Ensure all UI updates happen on main thread
   QMetaObject::invokeMethod(this, [this]() {
       // UI update code here
   }, Qt::QueuedConnection);

3. Resource Management

   • Tab switching with accumulated poses - ensure proper cleanup
   • Marker publishing frequency with multiple poses
   • Memory usage with large pose lists

⚠️ Code Quality & Standards

1. Input Validation

   // Needs robust validation for manual coordinate input
   if (x_coord < min_bound || x_coord > max_bound) {
       // Error handling
   }

2. File Path Handling

   // BT XML file path validation needed
   if (!std::filesystem::exists(bt_xml_path)) {
       RCLCPP_ERROR(node_->get_logger(), "BT file not found: %s", bt_xml_path.c_str());
       return false;
   }

3. State Machine Complexity

   • Adding tabs increases state complexity
   • Need clear documentation of state transitions
   • Consider state diagram for maintenance

🔍 Specific Technical Concerns

1. Tab Management

   // Ensure proper tab state management
   tab_widget_->setTabEnabled(NavigateToPoseTab, !accumulation_mode);
   tab_widget_->setTabEnabled(NavigateThroughPosesTab, accumulation_mode);

2. Coordinate Frame Consistency

   // Ensure all manually entered poses use correct frame_id
   pose.header.frame_id = selected_frame_.toStdString();
   pose.header.stamp = client_node_->now();

3. XML File Integration

   // Set behavior_tree field correctly
   if (!bt_xml_path_.empty()) {
       navigation_goal_.behavior_tree = bt_xml_path_;
   }
   // Validate XML before sending goal

Required Improvements

📋 Immediate Actions Needed

1. Add Input Validation

   • Coordinate bounds checking
   • BT XML file existence validation
   • Frame ID validation against tf tree

2. Error Handling Enhancement

   try {
       // BT file validation
       if (!validateBehaviorTree(bt_xml_path_)) {
           showErrorMessage("Invalid BT XML file");
           return;
       }
   } catch (const std::exception& e) {
       RCLCPP_ERROR(node_->get_logger(), "BT validation failed: %s", e.what());
   }

3. UI Polish

   • Consistent tab ordering and naming
   • Tooltips for manual coordinate inputs
   • Clear indication of current mode/state

🧪 Testing Recommendations

1. Unit Tests Needed

   • Tab switching logic
   • Coordinate validation
   • BT file path handling

2. Integration Tests

   • Mode transitions with accumulated poses
   • Marker generation and cleanup
   • Action goal formatting with BT paths

3. Edge Cases

   • Empty coordinate inputs
   • Invalid BT XML files
   • Tab switching during action execution

Questions for Author

1. Thread Safety: How are Qt widget updates synchronized with ROS callbacks?
2. Performance: What's the behavior with large numbers of poses (100+)?
3. Configuration: Should BT XML paths be saveable in RViz config?
4. Validation: What validation is performed on manually entered coordinates?

Suggestions for Future Enhancement

1. Preset Management: Save/load commonly used poses
2. Interactive Markers: Visual pose adjustment in 3D view
3. Progress Visualization: Real-time progress through pose sequences
4. Template System: Predefined BT templates for common scenarios

Conclusion

This is a well-conceived enhancement that significantly improves the Nav2 RViz panel's usability. The integration approach is sound, and the added functionality addresses real user needs. However, careful attention to input validation, error handling, and state management is crucial for production readiness.

Recommendation: ✅ Approve with requested changes - Address input validation and error handling, then this will be a valuable addition to Nav2.

Oh man, you did a great job, this is cool!

Check out the Claude output, but don't take it too seriously if you don't agree with what it says. I mostly asked it to check since the state machine here is complex - so between that and you looking over it again, please verify that you've cleaned up all the resources appropriately. I'm taking a look too, but this is error prone in reviews.

The only specific request I have is to change the ordering of the tabs to put Navigate To Pose first, then Nav Through Poses, then WP following. It looks like its last from your screen shots. Make sure to update onIdle correspondingly.

@mini-1235, thanks for the review.

Regarding "click and drag," I am referring to the standard Nav2 Goal button. If this is ambiguous, we can update the label to clarify.

When the Waypoint / Nav Through Poses Mode button is clicked, both the NavigateThroughPoses and WP-Following tabs are enabled. Currently, clicking LoadWPs in the WP-Following tab allows you to load a YAML file with a list of goals and start the navigation. However, this action does not populate the pose list in the NavigateThroughPoses tab. Consequently, you cannot navigate through those points using Start Nav Through Poses.

This means the inputs for these two modes are currently aislated. Since both modes essentially operate on a list of poses—differing only in the navigation action type—I propose unifying them.

I suggest creating a single tab for pose management that includes:

• A list view for each pose (similar to the current NavigateThroughPoses tab).
• Buttons to add and remove poses.
• Buttons to load and save waypoints.
• Original tab's WP-Following options.

With this structure, any pose added via Nav2 Pose, Add Pose or Load WP would populate this shared list, making it easy to modify and save them. Furthermore, the user could easily choose to execute the path using either NavigateThroughPoses or WP-Following, as both modes would share the same data source.

Check this picture as an illustration of the proposed organization:

I believe this modification compacts the multiple-pose navigation tools available Rviz and simplifies the UI, easing even more the user's experience with it. @SteveMacenski , @mini-1235 what do you think about it? Do you find worth it implementing this?

I haven't pushed any changes yet, as I wanted you to confirm if you like the proposal first. However, here is a demo of NavigateThroughPoses. It shows the workflow of creating a pose with the Nav2 Goal button, adding another via manual Add Pose, loading a list from a YAML file, and finally deleting one of them. This demonstrates how all editing modes work together in this update for the new UI layout.

Just like NavigateThroughPoses, the Waypoint Following action can be executed using the very same list of poses with no additional preparation steps needed.

Regarding "click and drag," I am referring to the standard Nav2 Goal button. If this is ambiguous, we can update the label to clarify.

I think we can remove that line

I suggest creating a single tab for pose management that includes:
A list view for each pose (similar to the current NavigateThroughPoses tab).
Buttons to add and remove poses.
Buttons to load and save waypoints.
Original tab's WP-Following options.

I agree, loading poses from a yaml file is something I use frequently when testing Navigate Through Poses

Please find uploaded the updated version of the nav2_rviz_plugins package, featuring the discussed UI layout that merges the NavigateThroughPoses and Waypoint Following tools into a single tab.

The attached video demonstrates the workflow with this new layout: adding a pose via the Nav2 Goal button, appending additional poses from a YAML file, and executing navigation using both NavigateThroughPoses and Waypoint Following.

Going to wait for Maurice to review again, but wanted to say those videos are gold. You should include an entry to the migration guide and use that video to showcase your work! https://docs.nav2.org/migration/Kilted.html

@mini-1235 you're absolutely right! I pushed a new commit fixing this and updating a few more labels to maintain consistency throughout the UI.

Question: Do we need to do

pre_initial_->assignProperty(
running_->assignProperty(
paused_->assignProperty

and so on for the add pose and remove pose button?

My bad, Add Pose and Remove Pose were being handled differently from the rest of the buttons for no reason. They were being enabled/disabled directly in each of the state functions instead of using assignProperty for each state so that they are configured automatically.

I took the opportunity to correct it, so now buttons are configured consistently. I also removed some redundant text configuration for buttons that never change their labels across states.

Also, I fixed a minor bug where the Start NavigateToPose button was still enabled while navigating.

save_waypoints_button_ and load_waypoints_button_ always serve the same purpose: saving and loading waypoints. Since their labels are static, there is no need to update them every time we switch states. I removed those lines because setting the text once in the initial state (pre_initial_) is sufficient.

In contrast, buttons like pause_waypoint_button_ or navigation_mode_button_ change functionality. For example, pause_waypoint_button_ is used for "Pause Waypoint Following," but when clicked, the logic changes to "Resume Waypoint Following," so the text must change as well. Therefore, we cannot just set it at initialization; we must update it to match the behavior of each state. If you find it necessary, we could change some buttons' variable names to something more generic for all functionalities possible for them, as of now their names reflect their first functionality across the usage of the UI.

While we could explicitly set the text for every button in every state, I optimized this to reduce code complexity and the total number of lines.

Just waiting on the docs PR fix :-)

Thanks for this @preverte