[Bug]: skip_rotation_commands: False Still Results in Consecutive Commands with Misaligned Angles

[osama-z-salah]

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.4 LTS

Open-RMF installation type

Source build

Other Open-RMF installation methods

No response

Open-RMF version or commit hash

Open-RMF on Rolling Ridley - Sync (2023-12-29)

ROS distribution

Humble

ROS installation type

Binaries

Other ROS installation methods

No response

Package or library, if applicable

No response

Description of the bug

Hello, I encountered an issue while running my fleet adapter, where I noticed that the skip_rotation_commands: False setting didn't seem to behave as expected. I have set the parameter skip_rotation_commands to False, which should prevent the fleet adapter from sending consecutive navigation commands to different locations unless their angles are already aligned. However, I have noticed that after planning a new go_to_place location or after waiting for traffic, the fleet adapter consistently fails to issue the necessary rotation command to align the robot to the correct angle before issuing the next movement command.

In the logs below, the robot was commanded to navigate to temp2 with an angle of 0 (command ID 103). After reaching temp2, the next command was sent to move to wp14, where the target angle was -1.57512112 (command ID 104). Ideally, the fleet adapter should have issued a rotation command at temp2 to match the target angle (-1.57512112) before moving to wp14, but this did not happen.

This issue always occurs in these specific scenarios:

1. After the fleet adapter plans a new go_to_place location.
2. After the robot finishes waiting for traffic.

[fleet_adapter-1] [INFO] [1730857147.817778145] [Nano500XW_fleet_adapter_node]: Commanding [nano500xw02] to navigate to temp2 at [ 7.64497743 -4.40710463  0.        ] on map [L1]: cmd_id 103
[fleet_adapter-1] [INFO] [1730857152.441238083] [Robot500xw_fleet_adapter]: Selecting a new go_to_place location from [1] choices for robot [Robot500xw/nano500xw02]
[fleet_adapter-1] [INFO] [1730857152.441292465] [Robot500xw_fleet_adapter]: Got distance from [15] as 13.740406
[fleet_adapter-1] [INFO] [1730857152.441378237] [Robot500xw_fleet_adapter]: Planning for [Robot500xw/nano500xw02] to [unloading_station] from one of these locations:
[fleet_adapter-1]  -- L1 <7.64498 -4.4071> [temp2] | location < 7.63887 -4.38971> | orientation -0.741929
[fleet_adapter-1]  -- lane 51: { L1 <7.64498 -4.4071> [temp2] } -> { L1 < 7.64498 -2.97803> [wp14] } | location < 7.63887 -4.38971> | orientation -0.741929
[fleet_adapter-1]  -- lane 52: { L1 <7.64498 -4.4071> [temp2] } -> { L1 <11.3026 -4.4071> [station_500] } | location < 7.63887 -4.38971> | orientation -0.741929
[fleet_adapter-1] [INFO] [1730857152.441690383] [Nano500XW_fleet_adapter_node]: Robot [nano500xw02] has completed cmd_id 103
[fleet_adapter-1] [INFO] [1730857152.447206762] [Robot500xw_fleet_adapter]: Executing go_to_place [unloading_station] for robot [Robot500xw/nano500xw02]
[fleet_adapter-1] [INFO] [1730857152.449000934] [Nano500XW_fleet_adapter_node]: Commanding [nano500xw02] to navigate to wp14 at [ 7.64497743 -2.9780335  -1.57512112] on map [L1]: cmd_id 104
[fleet_adapter-1] [INFO] [1730857152.453842024] [Nano500XW_fleet_adapter_node]: Robot [nano500xw02] is not facing the goal position. Need to rotate to the optimal angle where forward_angle_to_goal: 1.5614651723387714 and backward_angle_to_goal: -1.5801274812510218 and current_position[2]: 0.023339053794265353

Steps to reproduce the bug

1. Run a fleet adapter with skip_rotation_commands: False.
2. Command the robot to navigate between two locations with different target angles.
3. Observe that the fleet adapter always skips the necessary rotation command after planning a new go_to_place or after waiting for traffic, resulting in the robot not being properly aligned for the next movement.

Expected behavior

With skip_rotation_commands: False, the expected sequence should have been:

1. Rotate at temp2 to the target angle (-1.57512112).
2. Then, proceed with the movement command to wp14.

Instead, the robot was given a movement command to wp14 directly without aligning its angle first, resulting in misalignment and additional unnecessary rotations.

Actual behavior

No response

Additional information or screenshots

No response