Move nav_2d_utils to nav2_util

[mini-1235]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	Related to #5390
Primary OS tested on	Ubuntu
Robotic platform tested on	(Steve's Robot, gazebo simulation of Tally, hardware turtlebot)
Does this PR contain AI generated software?	No
Was this PR description generated by AI software?	Out of respect for maintainers, AI for human-to-human communications are banned

---

Description of contribution in a few bullet points

This PR moves common functions/classes from nav_2d_util to nav2_utils, the nav_2d_msgs and nav_2d_utils package currently are only used in dwb related packages

cc @adivardi

Description of documentation updates required from your changes

Description of how this change was tested

---

Future work that may be required in bullet points

Should we update all nav_2d_msgs/twist2d to geometry_msgs/twist at this point? If yes, I think nav_2d_utils can be safely removed

There are also a couple of packages still implementing its own transformPose like

navigation2/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp

Lines 131 to 148 in c5531cb

	bool PathHandler::transformPose(
	const std::string frame,
	const geometry_msgs::msg::PoseStamped & in_pose,
	geometry_msgs::msg::PoseStamped & out_pose) const
	{
	if (in_pose.header.frame_id == frame) {
	out_pose = in_pose;
	return true;
	}
	try {
	tf_->transform(in_pose, out_pose, frame, transform_tolerance_);
	out_pose.header.frame_id = frame;
	return true;
	} catch (tf2::TransformException & ex) {
	RCLCPP_ERROR(logger_, "Exception in transformPose: %s", ex.what());
	}
	return false;

I think this can be updated using nav2_utils::TransformPoseInTargetFrame, I can update in this PR or in a separate PR
edit: done in the latest commit

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

[mini-1235]

Not sure why codecov is failing, maybe I should update the key?

[adivardi]

I had a quick look at the changes, and it looks good

[SteveMacenski]

@mini-1235 try pushing again when you make the changes I mention above. Sometimes the codecov stage fails for fluke reasons (i.e. codecov's APIs aren't that stable, the internet connection isn't great on the CircleCI builder, etc). I wouldn't read too much into it unless it happens again.

[mergify]

@mini-1235, 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]

@mini-1235, 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]

Otherwise LGTM!

[SteveMacenski]

If these 2x aren't in the configuration guide for the controller server, we need to add them

[SteveMacenski]

Ditto

[SteveMacenski]

There's a build failure here:

Starting >>> nav2_util
--- stderr: nav2_util                              
In file included from /opt/overlay_ws/src/navigation2/nav2_util/include/nav2_util/robot_utils.hpp:29,
                 from /opt/overlay_ws/src/navigation2/nav2_util/src/robot_utils.cpp:25:
/opt/ros/rolling/include/tf2_ros/tf2_ros/buffer.h:40:4: error: #warning BUFFER_HEADER_DEPRECATION [-Werror=cpp]
   40 |   #warning BUFFER_HEADER_DEPRECATION
      |    ^~~~~~~
cc1plus: all warnings being treated as errors
gmake[2]: *** [src/CMakeFiles/nav2_util_core.dir/build.make:118: src/CMakeFiles/nav2_util_core.dir/robot_utils.cpp.o] Error 1

[mini-1235]

There's a build failure here:

Starting >>> nav2_util
--- stderr: nav2_util                              
In file included from /opt/overlay_ws/src/navigation2/nav2_util/include/nav2_util/robot_utils.hpp:29,
                 from /opt/overlay_ws/src/navigation2/nav2_util/src/robot_utils.cpp:25:
/opt/ros/rolling/include/tf2_ros/tf2_ros/buffer.h:40:4: error: #warning BUFFER_HEADER_DEPRECATION [-Werror=cpp]
   40 |   #warning BUFFER_HEADER_DEPRECATION
      |    ^~~~~~~
cc1plus: all warnings being treated as errors
gmake[2]: *** [src/CMakeFiles/nav2_util_core.dir/build.make:118: src/CMakeFiles/nav2_util_core.dir/robot_utils.cpp.o] Error 1

#5381 handles it, but we are still unable to merge that

[SteveMacenski]

That kind of puts us in an unideal spot blocking this PR as well. What do you suggest?

[mini-1235]

That kind of puts us in an unideal spot blocking this PR as well. What do you suggest?

What about temporarily building geometry2 and related packages with sources first in Kilted & Jazzy? We can remove once the binaries are out

[SteveMacenski]

I don't think that's the intent of these jobs - they're to show that you can work with Nav2 on the binaries provided in those distributions. If we break binary support, we break users of Nav2 using that compatibility as well. Asking people to rebuild geometry2 for rolling on occasion is fine, but on a released distro seems more problematic and breaks the trust that that compatibility is prioritized.

I think we need to wait for these to pass from binary syncs and/or add preprocessor conditions in some abstracted include files for use in the servers to get around this in the interim. Did you check that a new Kilted/Jazzy release was cut of TF2 for these backported changes so that the next sync should include them?

Kilted had a sync recently, was this not included in it? https://discourse.openrobotics.org/t/preparing-for-kilted-sync-and-patch-release-2025-07-25/49245

Jazzy hasn't had one in about 4-5 weeks, so one should be coming soon

[mini-1235]

Did you check that a new Kilted/Jazzy release was cut of TF2 for these backported changes so that the next sync should include them?

Yes, I believe the PRs in rosdistro have also been merged 1/2 weeks ago

Kilted had a sync recently, was this not included in it? https://discourse.openrobotics.org/t/preparing-for-kilted-sync-and-patch-release-2025-07-25/49245

Unfortunately the announcement is made before the backport is done, the released version is 0.41.1, while 0.41.2 is what we want. https://discourse.openrobotics.org/t/new-packages-and-patch-release-for-kilted-kaiju-2025-07-28/49312

If you prefer to work with preprocessor conditions, I can probably do it tomorrow

[SteveMacenski]

If these aren't blocking anything else, we could also just wait it out. I don't want you to spend time on things that aren't value generating - you do good work!

[SteveMacenski]

Upon further thought and a night's speed I realized that my argument is totally missing the most important point that the main branch is first and foremost for rolling and we need our CI to turn over to be able to merge anyone elses' PRs.

I merged in your TF2 updates - please pull in / rebase the others!

Thanks :-)

[mini-1235]

Upon further thought and a night's speed I realized that my argument is totally missing the most important point that the main branch is first and foremost for rolling and we need our CI to turn over to be able to merge anyone elses' PRs.

I merged in your TF2 updates - please pull in / rebase the others!

Thanks :-)

Ok!

[codecov]

Codecov Report

❌ Patch coverage is 98.03922% with 1 line in your changes missing coverage. Please review.

Files with missing lines	Patch %	Lines
nav2_controller/src/controller_server.cpp	91.66%	1 Missing ⚠️

Files with missing lines	Coverage Δ
...ller/include/nav2_controller/controller_server.hpp	100.00% <100.00%> (ø)
nav2_controller/plugins/pose_progress_checker.cpp	96.66% <ø> (ø)
...av2_controller/plugins/simple_progress_checker.cpp	100.00% <ø> (ø)
...docking/include/opennav_docking/docking_server.hpp	100.00% <ø> (ø)
nav2_docking/opennav_docking/src/controller.cpp	100.00% <ø> (ø)
...av2_docking/opennav_docking/src/docking_server.cpp	90.04% <100.00%> (+0.04%)	⬆️
...er/dwb_core/include/dwb_core/dwb_local_planner.hpp	100.00% <ø> (ø)
..._dwb_controller/dwb_core/src/dwb_local_planner.cpp	81.40% <100.00%> (-0.23%)	⬇️
nav2_dwb_controller/dwb_critics/src/goal_align.cpp	94.11% <100.00%> (ø)
...av2_dwb_controller/dwb_critics/src/oscillation.cpp	73.97% <100.00%> (ø)
... and 18 more

... and 3 files with indirect coverage changes

🚀 New features to boost your workflow:

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

[SteveMacenski]

@mini-1235 this has caused a regression but mostly by uncovering a previous bug that made this work.

nav_2d_utils::transformPose had a fallback if the transform could not be found due to an extrapolation error. Because the in_pose - transform would be negative, it would always be greater than the transform_tolerance. Without this behavior, if the transform cannot be found because the timestamp of the end_pose_ in the controller server is older than the TF buffer length (imagine a 10 minute navigation task, TF's buffer is ~30 seconds, with no replanning). The behavior now would use the current transform always so it 'worked'.

When the frame is stable like odom or map, this is probably fine behavior since the stamp is not important (which to be fair is how this is almost always used).

I looked over this PR and I see this new method used in the Controller Server, Path Handlers for RPP, Graceful, MPPI, and DWB.

• Controller Server: Used to transform current pose into path frame, I think that's OK. Used to transform the end_pose_ into global frame known problem
• DWB Path Handler: First use appears to not populate a timestamp, so should work fine. Second use appears to use getRobotPose from the computeVelocityCommands API in the controller server, so that should be current. Third use appears to do it on the path, so if the path points have timestamps this looks to be a problem
• Graceful Path Handler: First use appears to use getRobotPose from the computeVelocityCommands API in the controller server, so that should be current. Second use appears to do it on the path, but unlike DWB restamps to the current pose timestamp, so that seems to work OK? But inconsistent with DWB.
• MPPI Path Handler: First use appears to use getRobotPose from the computeVelocityCommands API in the controller server, so that should be current. Second use appears to use getRobotPose from the computeVelocityCommands API in the controller server, so that should be current. Third use appears to use getRobotPose from the computeVelocityCommands API in the controller server, so that should be current.
• Graceful Path Handler: First use appears to use getRobotPose from the computeVelocityCommands API in the controller server, so that should be current. Second use I think appears to restamp , so that seems to work OK? But inconsistent with DWB.

Can you review these as well and make sure you concur?

So that leads to possible solutions:

(1) We remove the timestamps so it always uses the most current transformation. This is in effect how it works now once the TF buffer duration expires (seems like an OK answer).

(2) We set the timestamps so it always aligned with the current pose from the input robot_pose / pose from computeVelocityCommands so that it is still synchronized with something (seems like the right answer).

(3) We introduce that fallback to transformPoseInTargetFrame that was previously in nav_2d_utils, which is probably a bug but would at least keep behavior consistent (likely not the right answer)

I audited other uses of transformPoseInTargetFrame and while not always as synchronized as I would like (some just set to now() or leave as zero), I don't see any immediate issues. Though, perhaps not a bad idea to fix all of those which we're at it to be synchronized with a real timestamp.

What do you think?

[mini-1235]

@mini-1235 this has caused a regression but mostly by uncovering a previous bug that made this work.

nav_2d_utils::transformPose had a fallback if the transform could not be found due to an extrapolation error. Because the in_pose - transform would be negative, it would always be greater than the transform_tolerance. Without this behavior, if the transform cannot be found because the timestamp of the end_pose_ in the controller server is older than the TF buffer length (imagine a 10 minute navigation task, TF's buffer is ~30 seconds, with no replanning). The behavior now would use the current transform always so it 'worked'.

When the frame is stable like odom or map, this is probably fine behavior since the stamp is not important (which to be fair is how this is almost always used).

I looked over this PR and I see this new method used in the Controller Server, Path Handlers for RPP, Graceful, MPPI, and DWB.

• Controller Server: Used to transform current pose into path frame, I think that's OK. Used to transform the end_pose_ into global frame known problem
• DWB Path Handler: First use appears to not populate a timestamp, so should work fine. Second use appears to use getRobotPose from the computeVelocityCommands API in the controller server, so that should be current. Third use appears to do it on the path, so if the path points have timestamps this looks to be a problem
• Graceful Path Handler: First use appears to use getRobotPose from the computeVelocityCommands API in the controller server, so that should be current. Second use appears to do it on the path, but unlike DWB restamps to the current pose timestamp, so that seems to work OK? But inconsistent with DWB.
• MPPI Path Handler: First use appears to use getRobotPose from the computeVelocityCommands API in the controller server, so that should be current. Second use appears to use getRobotPose from the computeVelocityCommands API in the controller server, so that should be current. Third use appears to use getRobotPose from the computeVelocityCommands API in the controller server, so that should be current.
• Graceful Path Handler: First use appears to use getRobotPose from the computeVelocityCommands API in the controller server, so that should be current. Second use I think appears to restamp , so that seems to work OK? But inconsistent with DWB.

Can you review these as well and make sure you concur?

So that leads to possible solutions:

(1) We remove the timestamps so it always uses the most current transformation. This is in effect how it works now once the TF buffer duration expires (seems like an OK answer).

(2) We set the timestamps so it always aligned with the current pose from the input robot_pose / pose from computeVelocityCommands so that it is still synchronized with something (seems like the right answer).

(3) We introduce that fallback to transformPoseInTargetFrame that was previously in nav_2d_utils, which is probably a bug but would at least keep behavior consistent (likely not the right answer)

I audited other uses of transformPoseInTargetFrame and while not always as synchronized as I would like (some just set to now() or leave as zero), I don't see any immediate issues. Though, perhaps not a bad idea to fix all of those which we're at it to be synchronized with a real timestamp.

What do you think?

Sorry for overlooking that detail, I will go through those cases and double check it

edit: Yes, I also think the second solution is the best, I will raise a PR once I go over them