Easy Traffic Light

[mxgrey]

This PR introduces the EasyTrafficLight API. This new API is much simpler than the original TrafficLight API (which will be getting some breaking changes from this PR; the breaking changes were necessary in order to interoperate with the full control fleet adapters).

This also massively overhauls the implementation of the traffic light system in general. There is now a moderator whose job is to guarantee that traffic light robots never get into a permanent deadlock (as long as some very basic conditions are met by the traffic light fleet adapter user).

This PR depends on PRs the following two repos:

• Traffic Light Scenarios rmf_demos#168
• Add pausing feature open-rmf/rmf_traffic_editor#267

There are a few known issues remaining, but I don't consider these issues to be critical enough to block these PRs from moving forward. They can be address in later iterations:

1. Some kind of issue with how we're using rxcpp is causing the traffic light fleet adapter to get temporarily hung when difficult negotiations are going on. When this happens, it will look as though the fleet adapter has died (e.g. all of the traffic light robots will come to a stop and the schedule will stop updating for them), but really it just needs 15-20 seconds to resume normal operation. This should be totally fixable by auditing the way we're using rxcpp, but I'm going to leave that for a later fix.

2. There's a timing estimation issue when traffic light robots are turning corners which results in the message Failed to compute timing estimate for [tinyRobot2] owned by [tinyRobot2] moving away from checkpoint index [4]. This should be a relatively simple math issue to fix, but it hasn't been a showstopper so far, so I'm going to leave it for a later PR.

3. In the office test (described below), the full control robot tends to get stuck on a chair because it cuts a corner too deeply (picture provided below). This is because of known (semi-intentional) behavior in the full control fleet adapter. In the long term this can be fixed by exposing a parameter for the amount of corner-cutting that we permit, but I'll save that for a future PR. In the short term, when this happens in a simulation, it can be fixed by manually dragging the robot away from the desk using Gazebo's translation tool.
   

4. When mixing traffic light fleet adapter and full control fleet adapter in the same environment, it seems that a rare situation can arise where the traffic light robot's schedule vanishes and does not get updated, which can lead to a permanent deadlock. I'll be investigating this further in a follow-up PR.

There are three scenarios in the rmf_demos PR that are helpful for testing the traffic light fleet adapter:

Triple-H scenario:

$ ros2 launch demos triple_H.launch.xml
(new terminal) $ ros2 launch demos the_pedigree.launch.xml

Battle Royale Scenario:

$ ros2 launch demos battle_royale.launch.xml
(new terminal) $ ros2 launch demos battle_go.launch.xml

Office Scenario:

$ ros2 launch demos office_mock_traffic_light.launch.xml
(new terminal) $ ros2 launch demos office_traffic_light_test.launch.xml

The key thing to watch when running these tests is that they never permanently deadlock. If it seems like the robots are stuck, make sure it's not issue (1) by simply giving them time to unfreeze (if it lasts longer than a minute, then that's concerning), and make sure it's not issue (2) by seeing if the full control robot has gotten snagged on some office furniture. If it's issue (4), then you can manually intervene by moving the full control robot out of the way of the traffic light robot.

[Yadunund]

Did not run into any problems during my testing ✨