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?

…kage.xml

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 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 added 2 commits May 21, 2025 13:12

Relocation of BaseFootprintPublisher and updates to CmakeList and Pac…

0587aec

…kage.xml

Relocation of BaseFootprintPublisher and updates to CmakeList and Pac…

0bef745

…kage.xml

pranavk-2003 reopened this May 21, 2025

		</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

Are you sure you want to change the base?

Nav2 toolkit #5116

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