Planner skipps waypoints (cuts path)

[Thazz]

I'm using open-rmf and free_fleet_server (on Foxy) together with free_fleet_client on real robot (running Melodic).
Robot size is 1.6 m x 1 m, free_fllet_adapter settings for robot size are footprint_radiu: 0.8 and vicinity_radius: 2.0 and I'm using full_control adapter.

I've noticed that sometimes (mostly when collisions are detected, but sometimes also when using only one robot), path assigned to robot is missing some waypoints (e.g path includes only last waypoints) resulting in a path that goes through walls (doesn't follow lines drawn in traffic editor). Below are two pictures of such scenario:

Have you experienced something similar before? Can you give me some pointers (possible causes) on where to look when investigating this issue?

Attached arso also building file and generated navigational graph.

h4_building.zip
0.yaml.zip

[mxgrey]

Yeah, I've seen this once in a while when running demos on real robots, but it happened too rarely for me to effectively debug it (and with COVID restrictions in place, my access to robot hardware is limited at the moment). If you're able to reliably reproduce this issue, you could be a great help in debugging it.

My best guess is something is not being very smart in estimation.cpp. One of the calls to update_position is probably giving a bad value or being called when it shouldn't be getting called.

If it would help, I could create a branch of rmf_ros2 that adds some debug dumps to the terminal, and you could try running that version against your robots. Then whenever this issue happens, you could kill the fleet adapter and grab the most recent terminal output, then I could try looking it over and seeing if anything bad sticks out.

[Thazz]

Thank you for your quick answer. Yes, please add additional logging and I will try to reproduce the error.

Edit: I've deleted previous reply as I've been logged in with wrong account.

[Thazz]

Edit: The problem for behaviour described below was on my side, as I've done some changes in free_fleet_client and the reported path did not include all waypoints.

I've found a way to reproduce this issue. If I send a loop request to the robot and force the robot not to move (so it stays in place), every time that fleet adapter recalculates the plan (I get the Replanning for [fleet_name/robot_name] because of an interruption message), new path is missing couple of starting waypoints. I've recorder a video of such scenario:

[mxgrey]

Ah, if your free_fleet_client was modified to not report the whole remaining path, then that will definitely cause the skipping to occur, because the fleet adapter will believe that your robot has made more progress than it really has.

[mxgrey]

In case you do come across the skipping again, I've created a debug_waypoint_skips branch in rmf_ros2. The debug output in that branch might help us identify why unexpected skipping may occur.

[Thazz]

From what I can tell from further testing, the reason for this is that free_fleet_client did not send the remaining path properly.