Insert in-place rotation waypoint if missing #111

xiyuoh · 2024-11-22T06:01:47Z

This PR targets open-rmf/rmf#556 where in-place rotation commands for certain paths are missing. After tracing the fleet adapter and rmf_traffic planner, it seems the planner sometimes fails to provide these waypoints. These in-place rotation waypoints may be essential for AGVs that rely entirely on RMF to align towards their next destination. The fix implemented is to check for such gaps inside reconstruct_waypoints and insert the rotation command as needed.

Test the fix

The issue can be consistently reproduced in the Office demos world by sending a patrol command for tinyRobot1 to hardware_2. Do set the skip_rotation_commands param to False when testing.

When the robot is returning from hardware_2 to its charger, it would provide the following path:

[tinyRobot_fleet_adapter]: Executing final go_to_place [tinyRobot1_charger] for robot [tinyRobot/tinyRobot1]
[tinyRobot_command_handle]: Commanding [tinyRobot1] to navigate to [ 20.89365413 -10.3083739   -1.59019108] on map [L1]: cmd_id 14
[tinyRobot_command_handle]: Commanding [tinyRobot1] to navigate to [ 20.89365413 -10.3083739   -3.11109903] on map [L1]: cmd_id 15
[tinyRobot_fleet_adapter]: Opening door [hardware_door] for [tinyRobot/tinyRobot1]
[tinyRobot_command_handle]: Commanding [tinyRobot1] to navigate to [ 18.79442303 -10.3724069   -3.11109903] on map [L1]: cmd_id 16
[tinyRobot_fleet_adapter]: Releasing door [hardware_door] for [tinyRobot/tinyRobot1]
[tinyRobot_command_handle]: Commanding [tinyRobot1] to navigate to [18.73952429 -6.87398187 -1.5551052 ] on map [L1]: cmd_id 17
[tinyRobot_command_handle]: Commanding [tinyRobot1] to navigate to [ 1.87395243e+01 -6.87398187e+00  3.91881687e-03] on map [L1]: cmd_id 18
[tinyRobot_command_handle]: Commanding [tinyRobot1] to navigate to [ 1.68558247e+01 -6.88136378e+00  3.91881687e-03] on map [L1]: cmd_id 19
[tinyRobot_command_handle]: Commanding [tinyRobot1] to navigate to [11.5658893  -6.99680773  0.0182071 ] on map [L1]: cmd_id 20
[tinyRobot_command_handle]: Commanding [tinyRobot1] to navigate to [11.5658893  -6.99680773 -0.01173879] on map [L1]: cmd_id 21
[tinyRobot_command_handle]: Commanding [tinyRobot1] to navigate to [10.08614619 -6.97943654 -0.01173879] on map [L1]: cmd_id 22
[tinyRobot_command_handle]: Commanding [tinyRobot1] to navigate to [10.08614619 -6.97943654  1.32861949] on map [L1]: cmd_id 23
[tinyRobot_command_handle]: Commanding [tinyRobot1] to navigate to [10.4330537  -5.57509559  1.32861949] on map [L1]: cmd_id 24
[tinyRobot_fleet_adapter]: Charging [tinyRobot/tinyRobot1] to [100.000000]

Notice the consecutive commands provided to the fleet adapter with cmd_id 19 and 20 have neither the same location nor yaw. path provided to the fleet adapter goes from

With this PR, an intermediate rotation waypoint (cmd_id 20) will be inserted and provided to the fleet adapter:

[tinyRobot_fleet_adapter]: Executing final go_to_place [tinyRobot1_charger] for robot [tinyRobot/tinyRobot1]
[tinyRobot_command_handle]: Commanding [tinyRobot1] to navigate to [ 20.89365413 -10.3083739   -1.59019246] on map [L1]: cmd_id 14
[tinyRobot_command_handle]: Commanding [tinyRobot1] to navigate to [ 20.89365413 -10.3083739   -3.11109903] on map [L1]: cmd_id 15
[tinyRobot_fleet_adapter]: Opening door [hardware_door] for [tinyRobot/tinyRobot1]
[tinyRobot_command_handle]: Commanding [tinyRobot1] to navigate to [ 18.79442303 -10.3724069   -3.11109903] on map [L1]: cmd_id 16
[tinyRobot_fleet_adapter]: Releasing door [hardware_door] for [tinyRobot/tinyRobot1]
[tinyRobot_command_handle]: Commanding [tinyRobot1] to navigate to [18.73952429 -6.87398187 -1.5551052 ] on map [L1]: cmd_id 17
[tinyRobot_command_handle]: Commanding [tinyRobot1] to navigate to [ 1.87395243e+01 -6.87398187e+00  3.91881687e-03] on map [L1]: cmd_id 18
[tinyRobot_command_handle]: Commanding [tinyRobot1] to navigate to [ 1.68558247e+01 -6.88136378e+00  3.91881687e-03] on map [L1]: cmd_id 19
[tinyRobot_command_handle]: Commanding [tinyRobot1] to navigate to [16.85582469 -6.88136378  0.0182071 ] on map [L1]: cmd_id 20
[tinyRobot_command_handle]: Commanding [tinyRobot1] to navigate to [11.5658893  -6.99680773  0.0182071 ] on map [L1]: cmd_id 21
[tinyRobot_command_handle]: Commanding [tinyRobot1] to navigate to [11.5658893  -6.99680773 -0.01173879] on map [L1]: cmd_id 22
[tinyRobot_command_handle]: Commanding [tinyRobot1] to navigate to [10.08614619 -6.97943654 -0.01173879] on map [L1]: cmd_id 23
[tinyRobot_command_handle]: Commanding [tinyRobot1] to navigate to [10.08614619 -6.97943654  1.32861949] on map [L1]: cmd_id 24
[tinyRobot_command_handle]: Commanding [tinyRobot1] to navigate to [10.4330537  -5.57509559  1.32861949] on map [L1]: cmd_id 25
[tinyRobot_fleet_adapter]: Charging [tinyRobot/tinyRobot1] to [100.000000]

Signed-off-by: Xiyu Oh <[email protected]>

osama-z-salah · 2024-12-03T03:41:56Z

Hi @xiyuoh , as mentioned in open-rmf/rmf#556, this problem consistently occurs in the following scenarios:

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

Your PR has resolved the issue for the first scenario, but the problem still persists after the robot done waiting for traffic where in-place rotation commands for certain paths are missing.

[fleet_adapter-1] [INFO] [1733197797.500725207] [tinyRobot_fleet_adapter]: Executing final go_to_place [robot1_parking] for robot [tinyRobot/robot1]
[fleet_adapter-1] [INFO] [1733197797.501530134] [Turtles_fleet_adapter_node]: Commanding [robot1] to navigate to s_1 at [11.99835317 -9.79865509  0.        ] on map [L1]: cmd_id 13
[fleet_adapter-1] [INFO] [1733197797.681876014] [tinyRobot_fleet_adapter]: [tinyRobot/robot1] waiting for traffic
[fleet_adapter-1] [INFO] [1733197800.697415744] [tinyRobot_fleet_adapter]: Planning for [2] robot(s) and [0] request(s)
[fleet_adapter-1] [INFO] [1733197813.547751842] [tinyRobot_fleet_adapter]: [tinyRobot/robot1] done waiting for traffic
[fleet_adapter-1] [INFO] [1733197813.548394587] [Turtles_fleet_adapter_node]: Commanding [robot1] to navigate to wp4 at [10.47451232 -9.79865509  0.        ] on map [L1]: cmd_id 14
[fleet_adapter-1] [INFO] [1733197818.563972326] [tinyRobot_fleet_adapter]: [tinyRobot/robot1] waiting for traffic
[fleet_adapter-1] [INFO] [1733197818.564083857] [tinyRobot_fleet_adapter]: [tinyRobot/robot1] done waiting for traffic
[fleet_adapter-1] [INFO] [1733197818.564885627] [Turtles_fleet_adapter_node]: Commanding [robot1] to navigate to robot1_parking at [ 10.47451232 -11.11212481  -1.57079633] on map [L1]: cmd_id 15
[fleet_adapter-1] [WARN] [1733197818.567197281] [Turtles_fleet_adapter_node]: =====================================
[fleet_adapter-1] [WARN] [1733197818.567408575] [Turtles_fleet_adapter_node]: Robot: [robot1] had angle mismatch between current position 0.001204171850951135 and destination angle -1.5707963267948966. the difference is 1.5720004986458478
[fleet_adapter-1] [WARN] [1733197818.567594303] [Turtles_fleet_adapter_node]: =====================================

xiyuoh · 2024-12-09T07:01:33Z

Hi @osama-z-salah , thanks for testing this PR! I've tried out what you're describing in sim and it doesn't seem like its related to traffic, but I do see in-place rotations being skipped from within the EasyFullControl fleet adapter. I've opened another PR for that since the code lives in another repo, feel free to try that out and let me know if the issue persists.

Signed-off-by: Xiyu Oh <[email protected]>

luca-della-vedova · 2024-12-10T05:22:32Z

rmf_traffic/src/rmf_traffic/agv/planning/DifferentialDrivePlanner.cpp

+    const bool same_stack =
+      prev_wp.graph_index.has_value() && current_wp.has_value()
+      && *prev_wp.graph_index == *current_wp;


Isn't this condition a subset of the position check? Would two waypoints that are exactly the same always return true for:

const bool same_pos = (prev_pos - node->position).norm() < 1e-3;

I think same graph index always mean same position, but same (or sufficiently close) positions don't always mean same graph index. Having !(same_stack || same_pos) was to include any locations that either have the same graph index, or a different graph index but sufficiently close within the threshold of 0.001m. I can see cases where two separate waypoints are added close to one another to manage the lane direction and traffic.

Yap that's what I meant, if two waypoints have the same index they will always have the exact same position, so would checking only for the position be sufficient?

Ah I see what you mean, gotcha removed in 67ae677

luca-della-vedova · 2024-12-10T05:23:28Z

rmf_traffic/src/rmf_traffic/agv/planning/DifferentialDrivePlanner.cpp

+    const auto prev_candidate = candidates.back();
+    const auto prev_wp = prev_candidate.waypoint;
+    const auto current_wp = node->waypoint;


Nit, we should use references for more complex types to avoid copying const auto&

Signed-off-by: Xiyu Oh <[email protected]>

luca-della-vedova

Thanks for iterating! Someone with admin access needs to go through the required workflows for this repo though, or this will be blocked on CI settings

…-place insertions Signed-off-by: Michael X. Grey <[email protected]>

Signed-off-by: Michael X. Grey <[email protected]>

mxgrey

After fixing the CI with #112 it turns out that there were some test failures in the PR. There were two root causes:

The "same orientation" calculation wasn't taking into account overflow at 2*pi
We weren't matching the plan waypoint with a checkpoint in the trajectory

Both issues are fixed by 344559c. (2) probably doesn't matter much to be honest, but I thought of a semi-reasonable way to make it work, and that spares us from needing to put fudge into the assumptions of the unit tests.

I'd appreciate if someone can take some time to test these changes on a demo world before merging. I don't think (2) should have any detrimental effects, but it's better to do a smoke test on that than to assume.

xiyuoh · 2024-12-20T04:21:58Z

Thanks for fixing the remaining issues! Ran this PR a couple times in office world and things are looking good, will merge it in.

Insert in-place rotation if missing

3120bb6

Signed-off-by: Xiyu Oh <[email protected]>

xiyuoh requested review from mxgrey and luca-della-vedova November 22, 2024 06:01

xiyuoh mentioned this pull request Dec 9, 2024

Do not skip waypoint if orientation is not aligned open-rmf/rmf_ros2#398

Merged

Fix typo

20ac53a

Signed-off-by: Xiyu Oh <[email protected]>

luca-della-vedova reviewed Dec 10, 2024

View reviewed changes

xiyuoh added 2 commits December 11, 2024 03:22

Use references

53908a2

Signed-off-by: Xiyu Oh <[email protected]>

Remove same stack requirement

67ae677

Signed-off-by: Xiyu Oh <[email protected]>

luca-della-vedova previously approved these changes Dec 11, 2024

View reviewed changes

mxgrey added 3 commits December 19, 2024 17:44

Merge branch 'main' into xiyu/insert_rotation

349d583

Fix orientation test and checkpoint identification for manual turn-in…

344559c

…-place insertions Signed-off-by: Michael X. Grey <[email protected]>

Tweak test debugging functions

a756cfe

Signed-off-by: Michael X. Grey <[email protected]>

mxgrey dismissed luca-della-vedova’s stale review via a756cfe December 19, 2024 13:18

mxgrey approved these changes Dec 19, 2024

View reviewed changes

Merge branch 'main' into xiyu/insert_rotation

bd86f2d

xiyuoh merged commit 982bf13 into main Dec 20, 2024
9 checks passed

xiyuoh deleted the xiyu/insert_rotation branch December 20, 2024 04:22

luca-della-vedova mentioned this pull request Jan 2, 2025

[Bug]: skip_rotation_commands: False Still Results in Consecutive Commands with Misaligned Angles open-rmf/rmf#556

Closed

1 task

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Insert in-place rotation waypoint if missing #111

Insert in-place rotation waypoint if missing #111

xiyuoh commented Nov 22, 2024

osama-z-salah commented Dec 3, 2024 •

edited

Loading

xiyuoh commented Dec 9, 2024

luca-della-vedova Dec 10, 2024

xiyuoh Dec 11, 2024

luca-della-vedova Dec 11, 2024

xiyuoh Dec 11, 2024

luca-della-vedova Dec 10, 2024 •

edited

Loading

xiyuoh Dec 11, 2024

luca-della-vedova left a comment

mxgrey left a comment •

edited

Loading

xiyuoh commented Dec 20, 2024

Insert in-place rotation waypoint if missing #111

Insert in-place rotation waypoint if missing #111

Conversation

xiyuoh commented Nov 22, 2024

osama-z-salah commented Dec 3, 2024 • edited Loading

xiyuoh commented Dec 9, 2024

luca-della-vedova Dec 10, 2024

Choose a reason for hiding this comment

xiyuoh Dec 11, 2024

Choose a reason for hiding this comment

luca-della-vedova Dec 11, 2024

Choose a reason for hiding this comment

xiyuoh Dec 11, 2024

Choose a reason for hiding this comment

luca-della-vedova Dec 10, 2024 • edited Loading

Choose a reason for hiding this comment

xiyuoh Dec 11, 2024

Choose a reason for hiding this comment

luca-della-vedova left a comment

Choose a reason for hiding this comment

mxgrey left a comment • edited Loading

Choose a reason for hiding this comment

xiyuoh commented Dec 20, 2024

osama-z-salah commented Dec 3, 2024 •

edited

Loading

luca-della-vedova Dec 10, 2024 •

edited

Loading

mxgrey left a comment •

edited

Loading