[Bug]: Incorrect Final Orientation in Waypoint Command Causes Premature Waiting Release

[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

Source build

Other Open-RMF installation methods

No response

Open-RMF version or commit hash

0.0.1

ROS distribution

Rolling

ROS installation type

Source build

Other ROS installation methods

No response

Package or library, if applicable

No response

Description of the bug

I’m encountering an issue where RMF sends a robot to a waypoint with an incorrect final orientation. Here’s the specific problem:

When RMF sends the robot to a waypoint, the final orientation in the command is not properly aligned with the next waypoint.
As a result, when the robot reaches the waypoint, it needs to rotate at that point to face the next waypoint.
However, RMF assumes the robot is ready to move to the next waypoint immediately, which causes RMF to end the waiting time for other robots prematurely.
This results in a collision risk, as the robot still needs to rotate before it can proceed to the next waypoint, while RMF assumes it can start moving without rotation.

Steps to reproduce the bug

Set up a fleet where one robot has to navigate through multiple waypoints.
Ensure there is a waypoint where the final orientation is not aligned with the next movement direction.
Observe that RMF releases other robots, assuming the first robot has completed its movement, while the robot is still performing a rotation to align with the next waypoint.

Expected behavior

No response

Actual behavior

No response

Additional information or screenshots

No response

[Yadunund]

However, RMF assumes the robot is ready to move to the next waypoint immediately, which causes RMF to end the waiting time for other robots prematurely.

Open-RMF does not assume this. Your fleet adapter is the one that lets Open-RMF know that it has finished navigating to a specific waypoint requested.

This results in a collision risk, as the robot still needs to rotate before it can proceed to the next waypoint, while RMF assumes it can start moving without rotation.

The responsibility for collision checking with the surroundings of the robot lies with the navigation stack and not Open-RMF.

[Samaleke]

@Yadunund , thank you for your response, and I apologize for the misunderstanding. I understand that the fleet adapter informs RMF when the robot has completed its navigation to a specified waypoint. However, sometimes the final orientation at the waypoint is incorrect, preventing the robot from proceeding to the next waypoint in plan without the needs to rotate at the previous waypoint to face the new one, due to nonholonomic nature of the robot. I have attached a video demonstrating the scenario.

WhatsApp.Video.2024-09-13.at.9.15.20.AM.mp4

[Samaleke]

@mxgrey, I was able to reproduce the same issue in simulation using two TurtleBots. Below is the navigation graph for reference
building_name: turtle doors: {} levels: L1: lanes: - - 0 - 1 - {orientation_constraint: backward, undock_name: pgv_dock2} - - 1 - 0 - {dock_name: pgv_dock2, orientation_constraint: forward} - - 1 - 2 - {} - - 2 - 1 - {} - - 2 - 3 - {} - - 3 - 2 - {} - - 3 - 4 - {} - - 4 - 3 - {} - - 4 - 5 - {dock_name: pgv_dock1, orientation_constraint: forward} - - 5 - 4 - {orientation_constraint: backward, undock_name: pgv_dock1} - - 3 - 6 - {} - - 6 - 3 - {} - - 3 - 7 - {} - - 7 - 3 - {} - - 7 - 8 - {} - - 8 - 7 - {} vertices: - - 11.998353167212343 - -8.648812908032232 - {dock_name: pgv_dock2, name: pgv_dock2} - - 11.270853020173702 - -8.648812908032232 - {name: wp2} - - 10.47451232183818 - -8.648812908032232 - {name: wp1} - - 10.47451232183818 - -9.798655086556748 - {name: wp4} - - 11.270853020173702 - -9.798655086556748 - {name: wp5} - - 11.998353167212343 - -9.798655086556748 - {dock_name: pgv_dock1, name: pgv_dock1} - - 10.47451232183818 - -11.112124806399121 - {is_charger: true, is_parking_spot: true, name: robot1_parking} - - 9.49439684749152 - -9.798655086556748 - {name: wp3} - - 9.49439684749152 - -11.098226713980434 - {is_charger: true, is_parking_spot: true, name: robot2_parking} lifts: {}
Here is the building file:

coordinate_system: reference_image crowd_sim: agent_groups: - {agents_name: [], agents_number: 0, group_id: 0, profile_selector: external_agent, state_selector: external_static, x: 0, y: 0} agent_profiles: - {ORCA_tau: 1, ORCA_tauObst: 0.40000000000000002, class: 1, max_accel: 0, max_angle_vel: 0, max_neighbors: 10, max_speed: 0, name: external_agent, neighbor_dist: 5, obstacle_set: 1, pref_speed: 0, r: 0.25} enable: 0 goal_sets: [] model_types: [] obstacle_set: {class: 1, file_name: L1_navmesh.nav, type: nav_mesh} states: - {final: 1, goal_set: -1, name: external_static, navmesh_file_name: ""} transitions: [] update_time_step: 0.10000000000000001 graphs: {} levels: L1: drawing: filename: turtlebot3_world.png elevation: 0 lanes: - [2, 3, {bidirectional: [4, true], demo_mock_floor_name: [1, ""], demo_mock_lift_name: [1, ""], graph_idx: [2, 0], mutex: [1, ""], orientation: [1, backward], speed_limit: [3, 0]}] - [3, 4, {bidirectional: [4, true], demo_mock_floor_name: [1, ""], demo_mock_lift_name: [1, ""], graph_idx: [2, 0], mutex: [1, ""], orientation: [1, ""], speed_limit: [3, 0]}] - [4, 5, {bidirectional: [4, true], demo_mock_floor_name: [1, ""], demo_mock_lift_name: [1, ""], graph_idx: [2, 0], mutex: [1, ""], orientation: [1, ""], speed_limit: [3, 0]}] - [5, 6, {bidirectional: [4, true], demo_mock_floor_name: [1, ""], demo_mock_lift_name: [1, ""], graph_idx: [2, 0], mutex: [1, ""], orientation: [1, ""], speed_limit: [3, 0]}] - [6, 7, {bidirectional: [4, true], demo_mock_floor_name: [1, ""], demo_mock_lift_name: [1, ""], graph_idx: [2, 0], mutex: [1, ""], orientation: [1, forward], speed_limit: [3, 0]}] - [5, 8, {bidirectional: [4, true], demo_mock_floor_name: [1, ""], demo_mock_lift_name: [1, ""], graph_idx: [2, 0], mutex: [1, ""], orientation: [1, ""], speed_limit: [3, 0]}] - [5, 9, {bidirectional: [4, true], demo_mock_floor_name: [1, ""], demo_mock_lift_name: [1, ""], graph_idx: [2, 0], mutex: [1, ""], orientation: [1, ""], speed_limit: [3, 0]}] - [9, 10, {bidirectional: [4, true], demo_mock_floor_name: [1, ""], demo_mock_lift_name: [1, ""], graph_idx: [2, 0], mutex: [1, ""], orientation: [1, ""], speed_limit: [3, 0]}] layers: {} measurements: - [0, 1, {distance: [3, 5.0999999999999996]}] vertices: - [200.69, 132.85900000000001, 0, ""] - [200.69, 234.87299999999999, 0, ""] - [240, 173, 0, pgv_dock2, {dock_name: [1, pgv_dock2]}] - [225.44800000000001, 173, 0, wp2] - [209.51900000000001, 173, 0, wp1] - [209.51900000000001, 196, 0, wp4] - [225.44800000000001, 196, 0, wp5] - [240, 196, 0, pgv_dock1, {dock_name: [1, pgv_dock1]}] - [209.51900000000001, 222.273, 0, robot1_parking, {is_charger: [4, true], is_parking_spot: [4, true]}] - [189.91399999999999, 196, 0, wp3] - [189.91399999999999, 221.995, 0, robot2_parking, {is_charger: [4, true], is_parking_spot: [4, true]}] lifts: {} name: turtle

The issue can be consistently reproduced by sending a patrol request between pgv_dock1 and robot1_parking 100 times for the first robot, and between pgv_dock2 and robot2_parking 100 times for the second robot.

WhatsApp.Video.2024-09-13.at.4.24.47.PM.mp4

[anandharaj-dotworld]

@Samaleke The same thing happened to me as well. Do you have any solutions for this issue?

[mxgrey]

@Samaleke When sharing raw file content it's helpful to put it into https://gist.github.com/ and then share a link to the gist. I would appreciate if you can edit your comment by removing the raw file data and replacing each file with a link to a gist, otherwise it's very difficult for us to copy/paste the content.

[Samaleke]

@mxgrey sorry for the late reply here is the link https://gist.github.com/Samaleke/6d7dc75f88371fc913c99f52c1686baa
The issue can be consistently reproduced by sending a patrol request between s_1 and robot1_parking 100 times for the first robot, and between s_2 and robot2_parking 100 times for the second robot.

[osama-z-salah]

@Samaleke have face the same issue did u manage to solve it