Add support for docking through nav2 by aionescu-cpr · Pull Request #215 · open-rmf/free_fleet

aionescu-cpr · 2025-08-18T22:06:36Z

New feature implementation

Implemented feature

This change adds docking support using Nav2 as requested here: #187

Implementation description

The main logic change is done in the Nav2RobotAdapter's navigate function, where rather than only sending a robot to a destination, a check was added to determine whether a dock was specified in the destination. If a dock was specified then the robot is sent a DockRobot action goal, otherwise the robot is given the default action of NavigateToPose.

As part of the change, the ros2_types.py was modified to add translation for the related DockRobot_* action classes. The default zenoh config was also modified to allow the .*/dock_robot action topics. A nav2send_dock_robot.py example was also added to allow the zenoh bridge to be tested.

The large change I introduced is a refactoring of the Nav2RobotAdapter as the "navigate_to_pose" and "dock_robot" actions share a lot of the same logic. Rather than duplicate a lot of the existing code and make the adapter larger, I added a NavigationExecutionItem which encapsulates the request being executed, with two classes inheriting from this for each action: NavigateToPoseExecutionItem and DockExecutionItem. This class is responsible for executing the goal, providing updates and stopping it. The handling of these functions were previously all in the Nav2RobotAdapter class, which was simplified to now focus on mainly the control logic.

This is a large change and it may not be in the direction that the maintainers of the repo want the design to go into, so I understand if a simpler approach is requested.

GenAI Use

We follow OSRA's policy on GenAI tools

I used a GenAI tool in this PR.
I did not use GenAI

Generated-by:

aaronchongth · 2025-08-19T01:51:33Z

Ignore the change from draft to ready-to-review, I misclicked, I've reverted it.

aaronchongth

Thanks for putting this together @aionescu-cpr! Overall the changes to the abstractions make sense, I like that this opens the door to more configurable behaviors, allowing the use of other BTs than NavigateToPose.

We can probably give more thought to the naming, to prevent confusion with the existing ExecutionHandle. I've added some comments, but overall it looks good.

aionescu-cpr · 2025-08-25T12:41:38Z

Thanks for putting this together @aionescu-cpr! Overall the changes to the abstractions make sense, I like that this opens the door to more configurable behaviors, allowing the use of other BTs than NavigateToPose.

We can probably give more thought to the naming, to prevent confusion with the existing ExecutionHandle. I've added some comments, but overall it looks good.

Thank you for the review and the naming suggestions @aaronchongth. I struggled to find a name that's not confusing, but I'm glad at least it got the point across as my intention was also to be able to extend this to other behaviours as well. I'll use your naming suggestions, update the PR and will now look at the unit tests too and update as needed.

Signed-off-by: Andrei Ionescu <aionescu@clearpath.ai>

aionescu-cpr · 2026-03-31T16:11:35Z

@aaronchongth As this PR was open for quite a while I had to rebase it to pull in the latest changes. Since I refactored the code those changes would be in new files, so please take a look to make sure that all the changes are indeed included when reviewing. I went through each commit, but a second set of eyes would be appreciated.

aaronchongth

quick high level review based on the test failures, I'll do a more indepth review soon

aaronchongth · 2026-04-03T09:40:03Z

        rclpy.init()
        cls.node = rclpy.create_node('test_nav2_robot_adapter')
-        cls.zenoh_session = zenoh.open(zenoh.Config())
+        # Configure zenoh to connect to localhost router


this will not working within a github workflow. Is there a reason the endpoint needs to be manually defined?

Thanks for catching this. I used this only for my local testing but it snuck in with the rest of the commit. I have reverted it.

Signed-off-by: Andrei Ionescu <aionescu@clearpath.ai>

aaronchongth

Thanks for the huge refactor! I really appreciate the additional testing, mocks and reduction of repeated code throughout.

I left some minor review comments to fix, including linting, but after the linting is fixed, I believe the unit and integration tests should pass too, I've managed to run our usual simulation examples with your refactor.

The implementation of the docking action seem solid, however we currently don't have the setup to test it. Did you test it with any particular setup or sim? If so, could you update this PR's description with it?

I'll update the issue ticket with the scope that will be completed with this ticket, before we look further into RMF's support of the navgation2 docking interface

aaronchongth · 2026-04-08T05:14:50Z

+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import time


lint: unused import

aaronchongth · 2026-04-08T05:15:29Z

+        cls.node = rclpy.create_node('test_ros2_action_interface')
+        cls.zenoh_session = zenoh.open(zenoh.Config())
+
+


lint: too many blank lines

aaronchongth · 2026-04-08T05:16:36Z

@@ -0,0 +1,166 @@
+#!/usr/bin/env python3
+
+# Copyright 2024 Open Source Robotics Foundation, Inc.


Suggested change

# Copyright 2024 Open Source Robotics Foundation, Inc.

# Copyright 2026 Open Source Robotics Foundation, Inc.

aaronchongth · 2026-04-08T05:16:47Z

@@ -0,0 +1,127 @@
+#!/usr/bin/env python3
+
+# Copyright 2024 Open Source Robotics Foundation, Inc.


Suggested change

# Copyright 2024 Open Source Robotics Foundation, Inc.

# Copyright 2026 Open Source Robotics Foundation, Inc.

aaronchongth · 2026-04-08T05:17:06Z

@@ -0,0 +1,381 @@
+#!/usr/bin/env python3
+
+# Copyright 2024 Open Source Robotics Foundation, Inc.


Suggested change

# Copyright 2024 Open Source Robotics Foundation, Inc.

# Copyright 2026 Open Source Robotics Foundation, Inc.

aaronchongth · 2026-04-08T08:18:32Z

+    STATUS_CANCELING: pycdr2.types.int8 = 3
+    STATUS_SUCCEEDED: pycdr2.types.int8 = 4
+    STATUS_CANCELED: pycdr2.types.int8 = 5
+    STATUS_ABORTED: pycdr2.types.int8 = 6


is there a reason for this change? It looks like it was only moved. Was this due to linting errors?
I'd prefer to keep diffs small to make reviewing easier

aaronchongth · 2026-04-09T06:50:41Z

+    parser.add_argument('--namespace', '-n', type=str, default='')
+    parser.add_argument('--frame-id', '-f', type=str, default='map')
+    parser.add_argument('--dock', type=str, default='')
+    parser.add_argument('--stage', type=bool, default=True)


iiuc, bool in ArgumentParser has weird behaviors due to strings being evaluated as True in general. Should we use something similar to https://github.com/open-rmf/rmf_demos/blob/main/rmf_demos_tasks/rmf_demos_tasks/dispatch_patrol.py#L86-L90, but store_false instead? The parameter name will probably have to be no-staging or something

aaronchongth · 2026-04-09T06:58:50Z

+        ...
+
+    @abstractmethod
+    def get_action_string(self):


Suggested change

def get_action_string(self):

def get_action_name(self):

nit: this is a little bit more semantically correct, the child classes and function calls will need to be updated

mxgrey added this to PMC Board Aug 18, 2025

github-project-automation Bot moved this to Inbox in PMC Board Aug 18, 2025

aionescu-cpr marked this pull request as draft August 18, 2025 22:06

aaronchongth marked this pull request as ready for review August 19, 2025 01:50

aaronchongth marked this pull request as draft August 19, 2025 01:50

aaronchongth reviewed Aug 22, 2025

View reviewed changes

Comment thread free_fleet_adapter/free_fleet_adapter/nav2_execution_item.py Outdated

aaronchongth reviewed Aug 22, 2025

View reviewed changes

Comment thread free_fleet_adapter/free_fleet_adapter/nav2_execution_item.py Outdated

aaronchongth reviewed Aug 22, 2025

View reviewed changes

Comment thread free_fleet_adapter/free_fleet_adapter/nav2_execution_item.py Outdated

aaronchongth reviewed Aug 22, 2025

View reviewed changes

mxgrey moved this from Inbox to In Review in PMC Board Aug 26, 2025

aaronchongth moved this from In Review to In Progress in PMC Board Dec 16, 2025

Add support for docking through nav2

4a3f809

Signed-off-by: Andrei Ionescu <aionescu@clearpath.ai>

aionescu-cpr force-pushed the andrei/add_nav2_docking branch 2 times, most recently from 60f51d4 to 85ff6bb Compare February 16, 2026 15:46

aaronchongth moved this from In Progress to Spotlight in PMC Board Mar 16, 2026

aionescu-cpr force-pushed the andrei/add_nav2_docking branch from 85ff6bb to f28d085 Compare March 31, 2026 16:07

aionescu-cpr marked this pull request as ready for review March 31, 2026 16:08

aaronchongth requested changes Apr 3, 2026

View reviewed changes

Add unit tests and address code review comments

ab55a9b

Signed-off-by: Andrei Ionescu <aionescu@clearpath.ai>

aionescu-cpr force-pushed the andrei/add_nav2_docking branch from f28d085 to ab55a9b Compare April 6, 2026 11:59

aaronchongth removed this from PMC Board Apr 7, 2026

aaronchongth requested changes Apr 9, 2026

View reviewed changes

aaronchongth mentioned this pull request Apr 9, 2026

Docking/undocking implementation #187

Open

		cls.node = rclpy.create_node('test_ros2_action_interface')
		cls.zenoh_session = zenoh.open(zenoh.Config())

		@@ -0,0 +1,166 @@
		#!/usr/bin/env python3

		# Copyright 2024 Open Source Robotics Foundation, Inc.

	# Copyright 2024 Open Source Robotics Foundation, Inc.
	# Copyright 2026 Open Source Robotics Foundation, Inc.

		@@ -0,0 +1,127 @@
		#!/usr/bin/env python3

		# Copyright 2024 Open Source Robotics Foundation, Inc.

		@@ -0,0 +1,381 @@
		#!/usr/bin/env python3

		# Copyright 2024 Open Source Robotics Foundation, Inc.

Conversation

aionescu-cpr commented Aug 18, 2025

New feature implementation

Implemented feature

Implementation description

GenAI Use

Uh oh!

aaronchongth commented Aug 19, 2025

Uh oh!

Uh oh!

Uh oh!

Uh oh!

aaronchongth left a comment • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

aionescu-cpr commented Aug 25, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

aionescu-cpr commented Mar 31, 2026

Uh oh!

aaronchongth 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!

aaronchongth 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!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

aaronchongth left a comment •

edited

Loading

aionescu-cpr commented Aug 25, 2025 •

edited

Loading