[Bug]: Incorrect Final Orientation in Waypoint Command Causes Premature Waiting Release (#527)

Open-RMF Open-RMF General
1

Posted by @Samaleke:

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

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

2

Posted by @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.

3

Posted by @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.

4

Posted by @mxgrey:

Is this capture of rviz coming from a simulation or a live robot? And are you using an open source fleet adapter that you can point us to or your own?

If you’re using your own fleet adapter, then see you using the RobotCommandHandle API? If so, can you share a code snippet for how your follow_new_path decides when to move on to the next waypoint? I notice that the yellow dot jumps to the next waypoint which means RMF believes it was told that your robot reached the next waypoint in the path. That usually means you’ve input the wrong path index into the arrival estimator.

For what its worth, this kind of error is usually eliminated by using the Easy Full Control API.

5

Posted by @Samaleke:

@mxgrey , this is a real robot, and I am indeed using the fleet adapter from the demo. I’ve adapted the client API to integrate with my fleet manager, so it’s essentially utilizing the Easy Full Control API, correct? and this issue is happed rarely

Edited by @Samaleke at 2024-09-13T01:49:01Z

6

Posted by @mxgrey:

If you’re using the Easy Full Control API and still having this occur then we may be able to take steps to debug it on our end.

Are you able to reproduce the issue in a simulation? Perhaps by setting up a map with the relevant lanes of the nav graph, and making a launch file or bash script to issue the task requests that lead to this happening? If you’re able to provide those, then I should be able to track down the cause of the schedule jump and fix it.

7

Posted by @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.

8

Posted by @anandharaj-dotworld:

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

9

Posted by @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.

Edited by @mxgrey at 2024-09-13T14:57:15Z

10

Posted by @Samaleke:

@mxgrey sorry for the late reply here is the link Incorrect Final Orientation in Waypoint Command Causes Premature Waiting Release · GitHub
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.

Edited by @Samaleke at 2024-09-15T04:48:33Z

11

Posted by @osama-z-salah:

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

Edited by @osama-z-salah at 2024-09-16T04:41:44Z