Introduce a sorted set for workcell tasks

[luca-della-vedova]

Fixes #102

A simple implementation of an ordered data structure for tasks stored at the workcell orchestrator level (in the end a set rather than a priority_queue as proposed, I think priority queues don't allow reordering items at runtime).
Changing from a fixed FIFO to a random access (but still ordered) data structure, allows the system orchestrator to request task execution for previously queued tasks at any time, without being forced to use the same order tasks were enqueued. This should allow higher efficiency as well as preventing deadlocks when, for example, integrating with AMR systems.

If tasks have the same status, we compare the time that status was set (so for example, if a task_0 was set to RUNNING at t=100s and a task_1 was set to RUNNING at t=200s, task_0 will always be earlier in the queue.
If tasks have a different status, they will be ordered in order of RUNNING - QUEUED - ASSIGNED - others (FAILED, FINISHED, undefined values).

Note that what really matters to make sure current behavior is preserved is that priority is respected when tasks are RUNNING, all the other states are not so important. Specifically, the current behavior is that tasks that are running do so in a "FIFO" basis, so the first item that is asked to run will do so and, if other tasks are requested to run but the workcell is not able to perform parallel tasks, they will not preempt the currently running task until it is finished.
This is ensured by the request time logic.

TODO:

• Refactor the custom set into a separate file, possibly in context.hpp.
• Add unit tests for the custom comparator.
• Come up with a test scenario where this helps

[luca-della-vedova]

Marking this as ready for review for high level design feedback.

The core idea and behavior of this new workcell storage can be understood by just looking at the unit tests.
Since we need to solve the problem of "in place editing a set, where its editing could result in its order changing" I had to come up with a quite interesting API and data structure, where now we can either get a pointer to a constant Context or, if we want to modify it, we need to do it in a lambda (i.e. here) to make sure that as soon as it is finished the set is reordered.
This kind of API will never leave pointers to mutable data hanging around which could, in theory, break the sorting of the set.