[Other issue]: Robot pose not updating on OpenRMF Server side when robot is actively navigating

[rtymk]

Description

Before proceeding, is there an existing issue or discussion for this?

I have done a search for similar issues and discussions.

OS and Version
Ubuntu 24.04

Installation Type
OpenRMF Binaries
Free Fleet Adapter built from source
Real robot, not simulation

Issue

Hello!

I just would first like to quickly thank everyone on this project for the great work you done, it has been incredibly helpful for my project!

I'm having a slight issue where the robot /fleet_states topic nearly stops updating (or updates at an extremely low frequency) when the robot is navigating to a command goal sent by OpenRMF. The robot pose updates in OpenRMF at the expected frequency when being manually controlled or commanded to a pose in nav2 on the robot itself. See the video below for an example of the issue occuring; the robot's pose updates at high frequency at the beginning of the video when being manually controlled, but when commanded to a pose from OpenRMF via Free Fleet with the OpenRMF planners running, the pose of the robot on the OpenRMF server side basically stops updating, as does the robots position on the /fleet_states topic (which does continue to publish though) until the robot physically arrives at the goal.

Screencast.from.2025-08-19.15-48-25.webm

I suspect the issue lies with the fact command handle seems to be unresponsive, however, I am just not quite sure why this could be occurring or how to address this.

Additional Information

Here is a sample of what the command line output appears as on the OpenRMF server side when this issue occurs

[fleet_adapter.py-5] [INFO] [1755709369.402146739] [turtlebot3_fleet_adapter]: Direct request [direct_d3cb44d0-6a3a-4260-8af2-bb9ad4e2997a] successfully queued for robot [nav2_tb3]
[fleet_adapter.py-5] [INFO] [1755709369.743026157] [turtlebot3_fleet_adapter]: Beginning next task [direct_d3cb44d0-6a3a-4260-8af2-bb9ad4e2997a] for robot [turtlebot3/nav2_tb3] 
[fleet_adapter.py-5] [INFO] [1755709369.743154187] [turtlebot3_fleet_adapter]: Selecting a new go_to_place location from [1] choices for robot [turtlebot3/nav2_tb3] 
[fleet_adapter.py-5] [INFO] [1755709369.743168577] [turtlebot3_fleet_adapter]: Got distance from [0] as 8.190637 
[fleet_adapter.py-5] [INFO] [1755709369.743207141] [turtlebot3_fleet_adapter]: Planning for [turtlebot3/nav2_tb3] to [tb3_charger] from one of these locations: [fleet_adapter.py-5] -- lane 0: { L0 < 6.79661 -8.54271> [tb3_charger] } -> { L0 < 12.523 -8.99332> [#1] } | location < 10.8918 -8.57599> | orientation 4.88081 [fleet_adapter.py-5] -- lane 1: { L0 < 12.523 -8.99332> [#1] } -> { L0 < 6.79661 -8.54271> [tb3_charger] } | location < 10.8918 -8.57599> | orientation 4.88081 
[fleet_adapter.py-5] [INFO] [1755709369.743303514] [turtlebot3_fleet_adapter]: Beginning new task [direct_d3cb44d0-6a3a-4260-8af2-bb9ad4e2997a] for [turtlebot3/nav2_tb3]. Remaining queue size: 0 
[fleet_adapter.py-5] [INFO] [1755709369.743879265] [turtlebot3_fleet_adapter]: Executing go_to_place [tb3_charger] for robot [turtlebot3/nav2_tb3] [fleet_adapter.py-5] [INFO] [1755709374.922997921] [turtlebot3_command_handle]: Return code: 0 [fleet_adapter.py-5] [INFO] [1755709374.923607612] [turtlebot3_command_handle]: Commanding [nav2_tb3] to navigate to [ 6.79660509 -9.04270778 3.13346625] on map [L0]
[fleet_adapter.py-5] [INFO] [1755709374.969769369] [turtlebot3_command_handle]: Navigation goal [87, 74, 201, 100, 92, 141, 89, 199, 153, 143, 195, 151, 119, 32, 153, 70] accepted 
[fleet_adapter.py-5] [WARN] [1755709384.971372451] [turtlebot3_fleet_adapter]: Requesting replan for [turtlebot3/nav2_tb3] because its command handle seems to be unresponsive 
[fleet_adapter.py-5] [INFO] [1755709384.971425233] [turtlebot3_fleet_adapter]: Replanning requested for [turtlebot3/nav2_tb3] 
[fleet_adapter.py-5] [INFO] [1755709385.113185567] [turtlebot3_command_handle]: Navigation goal [87, 74, 201, 100, 92, 141, 89, 199, 153, 143, 195, 151, 119, 32, 153, 70] was cancelled [fleet_adapter.py-5] [INFO] [1755709385.113886835] [turtlebot3_command_handle]: Return code: 0 
[fleet_adapter.py-5] [INFO] [1755709385.114081032] [turtlebot3_fleet_adapter]: Planning for [turtlebot3/nav2_tb3] to [tb3_charger] from one of these locations: [fleet_adapter.py-5] -- lane 1: { L0 < 12.523 -8.99332> [#1] } -> { L0 < 6.79661 -8.54271> [tb3_charger] } | location < 10.8918 -8.57599> | orientation 4.88215 
[fleet_adapter.py-5] -- lane 0: { L0 < 6.79661 -8.54271> [tb3_charger] } -> { L0 < 12.523 -8.99332> [#1] } | location < 10.8918 -8.57599> | orientation 4.88215 
[fleet_adapter.py-5] [INFO] [1755709385.114658589] [turtlebot3_fleet_adapter]: Executing go_to_place [tb3_charger] for robot [turtlebot3/nav2_tb3] 
[fleet_adapter.py-5] [INFO] [1755709385.116065962] [turtlebot3_command_handle]: Commanding [nav2_tb3] to navigate to [ 6.79660509 -9.04270778 3.13346569] on map [L0] [fleet_adapter.py-5] [INFO] [1755709385.207432013] [turtlebot3_command_handle]: Navigation goal [41, 247, 64, 236, 149, 190, 33, 68, 182, 206, 22, 40, 58, 216, 18, 183] accepted 
[fleet_adapter.py-5] [INFO] [1755709393.413258512] [turtlebot3_command_handle]: Navigation goal [41, 247, 64, 236, 149, 190, 33, 68, 182, 206, 22, 40, 58, 216, 18, 183] reached [schedule_visualizer_node-6] [INFO] [1755709413.423682687] [schedule_data_node]: Requesting new schedule update because update timed out [schedule_visualizer_node-6] [INFO] [1755709413.423714536] [schedule_data_node]: [rmf_traffic_ros2::MirrorManager::request_update] Requesting changes for query ID [1] since version [27] 
[fleet_adapter.py-5] [INFO] [1755709413.424128570] [turtlebot3_fleet_adapter]: Requesting new schedule update because update timed out 
[fleet_adapter.py-5] [INFO] [1755709413.424152428] [turtlebot3_fleet_adapter]: [rmf_traffic_ros2::MirrorManager::request_update] Requesting changes for query ID [1] since version [27]```

[aaronchongth]

Hi @rtymk! Thanks for flagging this out, and providing the information. We will try to reproduce it on our end and see how a fix can be applied, this looks like a race condition or a starved thread.

Sanity check, can you run ros2 topic hz ..... on the topic? This will help measure the frequency of the published messages, and may help inform us what is starving out the thread for publishing.

[rtymk]

Hi @aaronchongth , thanks for getting back to me! I really appreciate all the work that you've put in to this project!

ros2 topic echo /fleet_states shows the topic continues to publish at 10Hz even when the fleet state pose itself is not updating.

Regarding reproducibility, on my side I am able to reproduce this issue by running the Nav2 Single turtlebot3 world example from the free_fleet repo, following the process to launch the demo exactly as described in the readme.

If you watch the video below, the left side of the screen is the simulation domain ID, the right side is the RMF Server domain ID.

At first, when the robot is navigating to a goal pose set directly on it in nav2, it's pose updates in near real time on the RMF server. When the robot is navigating a path sent by RMF, however, it's pose does not update or change in the /fleet_states topic on the RMF server side. The fleet_states topic continues to publish at 10Hz still, but without showing the current pose. This is shown in the video below.

I will attempt to look into this issue on my own and let you know if I find anything, but I am relatively new to this project's codebase, and I haven't been able to diagnose much so far.

Screencast.from.2025-08-21.14-19-33.webm

Thanks again!

[rtymk]

Just an update, from some testing of mine, it appears what's hanging in nav2_robot_adapter.py is the line

if exec_handle.execution and exec_handle.goal_id and
self._is_navigation_done(exec_handle):

Based on the TODO comment in the code, it sounds like this is a known issue. I will try see if I can find a way to prevent this from holding up the update() function