Reactive easy full control

[mxgrey]

This PR updates #158 to be compatible with open-rmf/rmf_ros2#286

[luca-della-vedova]

I noticed a quite reproducible (most times, but not 100% of the times) jump in the schedule after a negotiation. I'm not sure what implications it might have but it seems a bit odd.

I can reproduce it best at startup in the battle royale scenario:

ros2 launch rmf_demos_gz_classic battle_royale.launch.xml
ros2 launch rmf_demos battle_go.launch.xml

An example video is below, robot with the schedule jump is in the top left. In all my tests it's either TinyRobotA or TinyRobotB in the top left of the map.

Screencast.from.2023-07-13.10-59-19.webm

[xiyuoh]

After some brief testing, I noticed that when a cleanerbot's cleaning path gets in the way of a patrol robot's route, the robots get stuck in a deadlock without negotiation. Not 100% sure but it might be because trajectory scheduling is not implemented for the cleaning perform action (example) .

hotel-efc-5x-cut.mp4

[mxgrey]

it might be because trajectory scheduling is not implemented for the cleaning perform action

Strange, it is implemented. I might suggest trying a clean build with the reactive_easy_full_control branches for both rmf_ros2 and rmf_demos. Here's a video of what it should look like when the robot is cleaning:

activity_schedule.mp4

Maybe try a test where you just send the cleaning bot with nothing else running and see if the cleaning trajectory shows up on the schedule.

[xiyuoh]

Ah yes you're right, a clean build and the trajectories are back up! I can still reproduce the same situation though. Haven't figured out a potential cause, leaving a screen recording with terminal logs here first.

hotel_2.mp4

[mxgrey]

@luca-della-vedova Good catch, I've found a subtle bug in the schedule update code that wasn't correctly accounting for planned waits. If you run the latest version the problem should be completely gone now.

@xiyuoh I've added traffic checkpoints for the activity (in this case cleaning) traffic schedule, so now the traffic negotiations should work better.

[Yadunund]

It's really awesome seeing how simple it is now to write a new fleet adapter!

[Yadunund]

Nit: Should we update the response body to indicate the existence of msg["data"]["path"]?

[mxgrey]

Great idea, added in 39ba81a