Issue with ResponsiveWait Feature in Open RMF Fleet Management

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

• I have done a search for similar issues and discussions.

Description

I am currently utilizing full_control RMF fleet adapter for my robotics project. My workflow involves sending the robot to a specific waypoint using the following command:

ros2 run rmf_demos_tasks dispatch_go_to_place -p my_parking -o -90

This command is intended to direct the robot to my predefined parking waypoint with an orientation of -90 degrees. The robot successfully reaches the destination with the correct heading. However, upon arrival, I encounter an issue where it receives two additional waypoints. I suspect these are generated by the ResponsiveWait feature. These waypoints have the same x and y coordinates as my intended destination but come with different yaw values. Consequently, this discrepancy results in the robot assuming a different heading from the one I specified, thus deviating from the desired orientation.

Could you please assist me in resolving this issue? I am looking for a way to either adjust the ResponsiveWait feature's behavior to prevent the generation of these additional waypoints or to ensure that any waypoints generated maintain the robot's orientation as specified in the dispatch command.

Thank you for your assistance.

Start by reproducing the dispatch_go_to_place command with the full_control RMF fleet adapter and trace where ResponsiveWait adds the two post-arrival waypoints. Compare their yaw values with the requested -90-degree orientation; done means the extra waypoints are avoided or preserve the dispatched heading, with a regression test for this scenario.