Nav2 toolkit #5116

pranavk-2003 · 2025-04-30T13:46:02Z

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#5070
Primary OS tested on	Ubuntu
Robotic platform tested on	Turtlebot gazebo simulation.
Does this PR contain AI generated software?	Somewhat

Description of testing performed

Functional tests were conducted on turtlebot3 and are working as expected

Description of contribution in a few bullet points

This Contribution adds a new package named nav2_toolkit which has QOL features, which can be handy while working with Nav2 .

Description of documentation updates required from your changes

Addition of 3 new parameterauto_start_saving, auto_restore_pose, save_interval_sec, so need to add that to nav2_params and documentation page

Description of how this change was tested

All the changes were tested multiple times in simulation over a course of 3 weeks.

Future work that may be required in bullet points

In total , this package will contain 3 tools i.e

pose_saver & pose_restore with added launch file in nav2_bringup.
Relocation of BaseFootprintPublisher.
A node which can pause and resume nav2 goals.

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

SteveMacenski

Good first stab!

Maybe lets wait until some more of this is fixed, but eventually we'll also need to add a configuration guide entry for this package in the docs.nav2.org and a migration guide entry to let folks know about the new features!

Anywhere that "AMCL" in mentioned in logs or naming, we should remove that and generally just talk about the pose publisher (which as an implementation detail is just AMCL for now). That way its not confusing when folks are using something that isn't AMCL, like SLAM Toolbox or another solution like VSLAM.

nav2_toolkit/package.xml

nav2_toolkit/CMakeLists.txt

nav2_toolkit/src/pose_saver_node.cpp

SteveMacenski · 2025-05-03T00:40:08Z

nav2_toolkit/src/pose_saver_node.cpp

+    std::bind(&PoseSaverNode::reset_pose_file_cb, this, std::placeholders::_1, std::placeholders::_2));
+
+  if (auto_start) {
+    saving_active_ = true;


Is saving_active_ needed? If the timer is running shouldn't it always be true? Every time its toggled it should be stopped.

True,but i feel its better having a separate flag which keeps track of it.

It doesn't, please remove. Its another piece of state that could be mishandled and cause bugs

nav2_toolkit/src/pose_saver_node.cpp

SteveMacenski · 2025-05-03T00:43:49Z

nav2_toolkit/src/pose_saver_node.cpp

+      }
+    }
+
+    // Wait for AMCL to subscribe to /initialpose


I think if we abstract out the functionality of restore_service_cb into a new method, that method could just be called here and called in the service callback to share this capability.

Yes good suggestion! Made it.

I dont see it being used in the startup

mergify · 2025-05-06T07:18:47Z

@pranavk-2003, 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 · 2025-05-07T18:24:24Z

@pranavk-2003 let me know when you want me to take a look again! I see you pushed some changes but a number of items are not yet addressed 😄

pranavk-2003 · 2025-05-11T14:28:05Z

@pranavk-2003 let me know when you want me to take a look again! I see you pushed some changes but a number of items are not yet addressed 😄

Its somewhat figured out,I'll let you know soon , got busy with university stuff ☹ .the few pushed stuff i still have to test.I am right now finding it difficult to get the nav2_package() macro right for nav2_toolkit.

pranavk-2003 · 2025-05-12T15:02:16Z

Hey @SteveMacenski ,I think i am done with the code updates , you can now have a look again throughout.The doxygen docs is in progress. : ) Also the checks are failing , let me know what has to be done for that too.

SteveMacenski · 2025-05-12T19:56:05Z

@pranavk-2003 open the failing jobs and fix the issues they lay out :-) It looks like alot of static code quality issues in linting

SteveMacenski

Needs test coverage as well!

SteveMacenski · 2025-05-12T19:54:42Z

nav2_toolkit/src/pose_saver_main.cpp

@@ -0,0 +1,11 @@
+#include "rclcpp/rclcpp.hpp"


Missing copyright header

SteveMacenski · 2025-05-12T19:55:00Z

nav2_toolkit/package.xml

+  </description>
+
+
+  <maintainer email="pranavkolekar13@gmail.com">Pranav</maintainer>


Add me too so I get emails on build issues

SteveMacenski · 2025-05-12T19:55:27Z

nav2_bringup/params/nav2_params.yaml

@@ -79,7 +79,6 @@ bt_navigator:
      - nav_to_pose


Please resolve by adding back in

SteveMacenski · 2025-05-12T19:56:29Z

nav2_bringup/launch/nav2_toolkit_launch.py

+def generate_launch_description():
+    bringup_dir = get_package_share_directory('nav2_bringup')
+    default_params_file = os.path.join(bringup_dir, 'params', 'nav2_params.yaml')
+    default_pose_file = os.path.join(os.environ['HOME'], 'last_known_pose.yaml')


Remove, this should be considered in the node.

SteveMacenski · 2025-05-12T19:56:58Z

nav2_toolkit/CMakeLists.txt

+project(nav2_toolkit)
+
+find_package(nav2_common REQUIRED)
+nav2_package()


After all find_packages

SteveMacenski · 2025-05-12T20:03:12Z

nav2_toolkit/README.md

@@ -0,0 +1,12 @@
+# nav2_toolkit
+
+A modular ROS 2 node for persisting and restoring the robot's last known pose using the `/amcl_pose` topic. This is useful in applications where the robot is never manually moved (e.g. warehouse robots), and it enables automatic re-localization.


@pranavk-2003 this wasnt completed

SteveMacenski · 2025-05-12T20:04:26Z

nav2_toolkit/src/pose_saver_node.cpp

+    "/amcl_pose", 10,
+    std::bind(&PoseSaverNode::pose_callback, this, std::placeholders::_1));
+
+  initial_pose_pub_ = this->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("/initialpose", 10);


Rather than publishing the initial pose whereas you do not know if it was received, using a service means you know it was received properly due to getting a response. If none is given, then you can error handle

SteveMacenski · 2025-05-12T20:04:59Z

nav2_toolkit/src/pose_saver_node.cpp

+
+void PoseSaverNode::pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
+{
+  last_pose_ = std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>(*msg);


No, just store the *msg object itself

SteveMacenski · 2025-05-12T20:05:56Z

nav2_toolkit/src/pose_saver_node.cpp

+      }
+    }
+
+    // Wait for AMCL to subscribe to /initialpose


I dont see it being used in the startup

SteveMacenski · 2025-05-12T20:06:19Z

nav2_toolkit/src/pose_saver_node.cpp

+    std::bind(&PoseSaverNode::reset_pose_file_cb, this, std::placeholders::_1, std::placeholders::_2));
+
+  if (auto_start) {
+    saving_active_ = true;


It doesn't, please remove. Its another piece of state that could be mishandled and cause bugs

SteveMacenski · 2025-05-12T20:08:11Z

nav2_toolkit/src/pose_saver_node.cpp

+  if (!saving_active_ || !last_pose_) return;
+
+  try {
+    write_pose_to_file(pose_file_path_);


Why not write to file when AMCL publishes to the pose topic? We could always throttle that to store every N seconds in case it is a fast publishing topic like from VSLAM, but then we could remove this timer entirely.

SteveMacenski · 2025-05-12T20:08:37Z

nav2_toolkit/src/pose_saver_node.cpp

+
+void PoseSaverNode::pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
+{
+  last_pose_ = std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>(*msg);


Ex check if we should write by the write frequency when we get a pose here and then write it in this callback if saving_active_

So what i understood from this is, when everytime amcl publishes a pose, it should be written to the file , instead of every N th second.For fast publishing it shall write every Nth second.But the question i have is , how would we decide when to write using timer and when to write with amcl publish update? A condition?

So what i understood from this is, when everytime amcl publishes a pose, it should be written to the file , instead of every N th second.

Each time this callback is triggered, check against the last write time (variable to store) and write if greater than the maximum write frequency. Else, do nothing. That way if this topic publishes very fast, like in VSLAM, we're not doing a ton of IO at 100hz and instead writing to files every 1hz or less to get the general pose of the robot.

rclcpp::Time time_now = now(); if (time_now - last_write_time_ >= write_duration_) { last_write_time_ = time_now; // do writing operation }

pranavk-2003 · 2025-05-14T14:31:57Z

Needs test coverage as well!

Hey okay, so what shall it be?test as in demos for using the tool?

pranavk-2003 · 2025-05-14T14:45:21Z

@pranavk-2003 open the failing jobs and fix the issues they lay out :-) It looks like alot of static code quality issues in linting

Okay i understood how to solve those errors, i would solve these after all the code adjustments are done , as the code adjustments would introduce more of them

SteveMacenski · 2025-05-14T23:34:00Z

Hey okay, so what shall it be?test as in demos for using the tool?

You should create a unit test that runs the object and then publishes some data to the topic its listening to. Check after awhile that there's a file at the requested location and the data is correct.

Then run another tests with autostart on and test that it correctly reads from that file and publishes / calls the service to set the pose correctly (the test object should expose the service, so nothing else should be running).

Then another test should be with autostart / auto-store off and call the services to start / stop / reset and check that it does what it should do.

Finally, a series of unit tests that checks read_pose_from_file write_pose_to_file individual function's correctness individually

CI should then tell you the coverage metrics in the codecov job - we need it to be at least 90% to be merged

SteveMacenski · 2025-05-14T23:36:01Z

Also as a matter of practice, do not resolve the PR topic discussions. I'll hit "resolve" when I verify that it was completed as intended. It makes my life easier as a maintainer to know that I validated each of my previous comments in your updated fixes without having to go back through all of the collapsed tabs each time :-)

mergify · 2025-05-20T16:46:50Z

This pull request is in conflict. Could you fix it @pranavk-2003?

pranavk-2003 · 2025-05-21T07:43:48Z

Resolved the conflicts, @SteveMacenski i'll let you know when i want you to check this PR again.

mergify · 2025-05-21T07:46:37Z

@pranavk-2003, 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).

pranavk-2003 · 2025-06-23T17:53:58Z

Hey @SteveMacenski its been a while.I have added tests, adjusted the code according to your recommendations and enhancement suggestions.I have tried to solve and fix most of what we discussed last time.Please review the changes and let me know any modifications.Also dont worry about the checks as i will fix them after all the code changes are done.I have also relocated the BaseFootprintPublisher to nav2_toolkit.:)

mergify · 2025-06-23T18:02:09Z

@pranavk-2003, 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).

pranavk-2003 · 2025-06-27T16:49:24Z

Sincere apologies @SteveMacenski for the delay between last and this update and this dual mention .I understand its alot of work to review this PR from the start again , but i'll make sure to wrapup this PR to the earliest from now without any delays.

SteveMacenski · 2025-07-02T18:31:31Z

OK! Let me know when you're ready. Please also heed the linters + DCO sign offs. It will make it much easier for me to review if the styling is consistent and I'm not distracted with pointing out pedantic errors. It also doesn't seem to build - which would be a minimum requirement for me to review.

Try rebasing / pulling in main

mergify · 2025-07-03T04:33:44Z

@pranavk-2003, 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 · 2025-07-03T16:44:09Z

@pranavk-2003, 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 · 2025-07-03T17:13:06Z

@pranavk-2003, 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).

pranavk-2003 · 2025-07-03T17:17:35Z

Sorry for the email spams of builds, ill remove you as a maintainer for now so it would not spam you.

pranavk-2003 · 2025-07-05T15:21:57Z

Hi @SteveMacenski i think i might have done something wrong while trying to sign off my old commits :( . What should i do? maybe make a new draft PR.Although i Fixed most of the Build check fails.i would guess the failing ones are because of DCO.Please guide me through this.Apologies for the extra steps — really appreciate your patience.

SteveMacenski · 2025-07-14T20:55:19Z

There are alot of merge conflicts. You could either resolve them or possibly try applying your work on an up-to-date branch and opening a new PR. I'm still seeing alot of build and linter errors if you check the failing red jobs above. They're mostly from linting that if you check on the linting jobs will tell you exactly how to fix them

mergify · 2025-07-18T15:54:44Z

This pull request is in conflict. Could you fix it @pranavk-2003?

mergify · 2025-07-18T15:56:31Z

@pranavk-2003, 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).

pranavk-2003 mentioned this pull request Apr 30, 2025

Store last known possition using AMCL directly! #5070

Open

SteveMacenski reviewed May 3, 2025

View reviewed changes

SteveMacenski requested changes May 12, 2025

View reviewed changes

SteveMacenski reviewed May 12, 2025

View reviewed changes

pranavk-2003 closed this May 21, 2025

pranavk-2003 force-pushed the nav2_toolkit branch from 596bc7a to cb39462 Compare May 21, 2025 07:30

pranavk-2003 reopened this May 21, 2025

pranavk-2003 requested a review from SteveMacenski June 29, 2025 05:31

pranavk-2003 force-pushed the nav2_toolkit branch from a4ae040 to 418a50b Compare July 5, 2025 15:04

pranavk-2003 force-pushed the nav2_toolkit branch from 418a50b to b3804cf Compare July 18, 2025 15:54

pranavk-2003 closed this Jul 18, 2025

pranavk-2003 force-pushed the nav2_toolkit branch from b3804cf to b66ef49 Compare July 18, 2025 15:59

pranavk-2003 deleted the nav2_toolkit branch July 18, 2025 16:01

pranavk-2003 mentioned this pull request Jul 18, 2025

Nav2 toolkit #5369

Draft

		</description>


		<maintainer email="pranavkolekar13@gmail.com">Pranav</maintainer>

		@@ -0,0 +1,12 @@
		# nav2_toolkit

		A modular ROS 2 node for persisting and restoring the robot's last known pose using the `/amcl_pose` topic. This is useful in applications where the robot is never manually moved (e.g. warehouse robots), and it enables automatic re-localization.

Nav2 toolkit #5116

Nav2 toolkit #5116

Uh oh!

Conversation

pranavk-2003 commented Apr 30, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Basic Info

Description of testing performed

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!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

mergify bot commented May 6, 2025

Uh oh!

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

Uh oh!

Uh oh!

pranavk-2003 commented May 11, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

pranavk-2003 commented May 12, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

SteveMacenski commented May 12, 2025

Uh oh!

SteveMacenski left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

pranavk-2003 May 18, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

pranavk-2003 commented Apr 30, 2025 •

edited

Loading

SteveMacenski commented May 7, 2025 •

edited

Loading

pranavk-2003 commented May 11, 2025 •

edited

Loading

pranavk-2003 commented May 12, 2025 •

edited

Loading

pranavk-2003 May 18, 2025 •

edited

Loading

SteveMacenski May 20, 2025 •

edited

Loading

pranavk-2003 commented Jun 23, 2025 •

edited

Loading

SteveMacenski commented Jul 2, 2025 •

edited

Loading

pranavk-2003 commented Jul 5, 2025 •

edited

Loading