[Bug]: Robot Stops at Incorrect Heading Despite Correct Initial Waypoint Command in RMF Demo Office Launch

[Samaleke]

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 22.04.3 LTS

Open-RMF installation type

Binaries

Other Open-RMF installation methods

No response

Open-RMF version or commit hash

0.0.1

ROS distribution

Humble

ROS installation type

Binaries

Other ROS installation methods

No response

Package or library, if applicable

No response

Description of the bug

I encountered an issue with the rmf_demo, specifically when running the Office World. After disabling the finishing_request by setting it to none, I issued a command to send a robot to the patrol_A2 waypoint with a heading of 90 degrees using dispatch_go_to_place. The robot successfully received the command with the intended waypoint and heading. The command handler received the following set of waypoints:

[fleet_adapter-16] [INFO] [1707923626.611421301] [tinyRobot_command_handle]: Received new path for tinyRobot1
[fleet_adapter-16] [INFO] [1707923626.611764118] [tinyRobot_command_handle]: nanoamr debug: Waypoint 0 is at [10.43, -5.58, 1.33]
[fleet_adapter-16] [INFO] [1707923626.611909057] [tinyRobot_command_handle]: nanoamr debug: Waypoint 1 is at [10.09, -6.98, 1.33]
[fleet_adapter-16] [INFO] [1707923626.612038607] [tinyRobot_command_handle]: nanoamr debug: Waypoint 2 is at [10.09, -6.98, -0.01]
[fleet_adapter-16] [INFO] [1707923626.612164350] [tinyRobot_command_handle]: nanoamr debug: Waypoint 3 is at [11.57, -7.00, -0.01]
[fleet_adapter-16] [INFO] [1707923626.612288889] [tinyRobot_command_handle]: nanoamr debug: Waypoint 4 is at [11.57, -7.00, 0.03]
[fleet_adapter-16] [INFO] [1707923626.612411493] [tinyRobot_command_handle]: nanoamr debug: Waypoint 5 is at [16.86, -6.88, 0.03]
[fleet_adapter-16] [INFO] [1707923626.612535130] [tinyRobot_command_handle]: nanoamr debug: Waypoint 6 is at [16.86, -6.88, 0.00]
[fleet_adapter-16] [INFO] [1707923626.612696016] [tinyRobot_command_handle]: nanoamr debug: Waypoint 7 is at [18.74, -6.87, 0.00]
[fleet_adapter-16] [INFO] [1707923626.612821408] [tinyRobot_command_handle]: nanoamr debug: Waypoint 8 is at [18.74, -6.87, 1.57]
[fleet_adapter-16] [INFO] [1707923626.612945035] [tinyRobot_command_handle]: nanoamr debug: Waypoint 9 is at [18.73, -3.90, 1.57]

The final waypoint had the intended heading. However, when the robot reached this waypoint with the correct heading, the command handler received a new path with two waypoints. These new waypoints had the same x and y coordinates as my requested one but with a different yaw:

[fleet_adapter-16] [INFO] [1707923671.131147056] [tinyRobot_command_handle]: Received new path for tinyRobot1
[fleet_adapter-16] [INFO] [1707923671.131422531] [tinyRobot_command_handle]: nanoamr debug: Waypoint 1 is at [18.73, -3.90, -1.57]
[fleet_adapter-16] [INFO] [1707923671.131561035] [tinyRobot_command_handle]: nanoamr debug: Waypoint 2 is at [18.73, -3.90, -1.57]

This resulted in the robot stopping at a different heading from the one I had requested. I believe these last two waypoints are coming from the ResponsiveWait feature. However, I am unclear why the yaw component is different from my requested one.

Could this be a bug, or is there a specific reason why the ResponsiveWait feature overrides the requested yaw with a different value? How can I ensure the robot maintains the requested heading upon reaching the final waypoint?

Thank you for your assistance.

Steps to reproduce the bug

1. Disabling the finishing_request by setting it to 'none' in the configuration file
2. Launch the RMF Office Demo using ros2 launch rmf_demos_gz_classic office.launch.xml
3. Dispatch the Robot to a Specific Waypoint using ros2 run rmf_demos_tasks dispatch_go_to_place -p patrol_A2 -o 90 --use_sim_time

Expected behavior

Upon dispatching the robot to the patrol_A2 waypoint with a heading of 90 degrees:

1. The robot should accurately navigate to patrol_A2 and align itself with the 90-degree heading upon arrival.
2. Once at the waypoint, the robot's orientation should remain at the specified 90 degrees without any uncommanded changes.

Actual behavior

Upon dispatching the robot to the patrol_A2 waypoint with a heading of 90 degrees, the actual outcome deviated from expectations in the following ways:

1. Initial Navigation Success: The robot successfully navigated to the patrol_A2 waypoint and initially aligned with the specified 90-degree heading.
2. Unexpected Heading Adjustment: After reaching the destination with the correct heading, the robot received new path instructions that included waypoints with the same x and y coordinates but a different yaw value of -1.57 radians. This caused the robot to adjust its heading away from the intended 90 degrees.

Additional information or screenshots

No response

[mxgrey]

As the name suggests, go_to_place is only concerned with getting a robot to a place, optionally with a certain heading. Once the robot arrives, it will move onto the next action that it needs to perform in its task. If there are no more actions in the task, then the task is considered complete, and the robot will move onto its next task. If there are no more tasks in its queue, then the robot will begin its idle behavior. The idle behavior is either

1. Perform the "finishing" task
2. Begin responsive waiting
3. Do absolutely nothing

Whether the robot's idle behavior is (1), (2), or (3) depends on your configuration. If you assign a finish_request then it will do (1). If you do not assign a finish_request and responsive_wait is set to True then it will be (2). If you do not assign a finished_request and responsive_wait is False, then it will be (3).

I generally don't recommend using Responsive Wait. It's almost always better to have a sensible finish request set. Responsive wait was put in because some users felt it was necessary, but it can be disruptive if you have complicated multi-robot operations. For the rmf_demos in particular, Responsive Wait is messy because the simulated robots report very very small errors in their positions resulting in them trying to correct those errors with every Responsive Wait cycle. We could improve this behavior but it's mostly a superficial problem and we have much more pressing concerns in our backlog.

In your particular case, I assume you're sending the robot to a certain location with a certain heading because you want to do some action at that location with that heading. We have some builtin actions like clean and payload_transfer, but if you need something custom then there's perform_action. To get the effect that you want, you'll need to add one of these actions to your task definition as a new phase after the go_to_place.