CBS is not properly working for 3 and more agents

[pycckuu]

Hello! Great collection of the algorithms.

I am trying to use CBS and I have some issues when I run it with 3 or more agents. The solution is never found although should be simple.

This is my input

agents:
-   start: [1, 5]
    goal: [3, 9]
    name: agent0
-   start: [1, 9]
    goal: [3, 7]
    name: agent1
-   start: [1, 7]
    goal: [3, 5]
    name: agent2
map:
    dimensions: [13, 12]
    obstacles:
    - !!python/tuple [0, 0]
    - !!python/tuple [0, 1]
    - !!python/tuple [0, 2]
    - !!python/tuple [0, 3]
    - !!python/tuple [0, 4]

    - !!python/tuple [0, 6]
    - !!python/tuple [0, 7]
    - !!python/tuple [0, 8]
    - !!python/tuple [0, 9]
    - !!python/tuple [0, 10]
    - !!python/tuple [0, 11]

    - !!python/tuple [1, 0]
    - !!python/tuple [2, 0]
    - !!python/tuple [3, 0]
    - !!python/tuple [4, 0]
    - !!python/tuple [5, 0]
    - !!python/tuple [6, 0]
    - !!python/tuple [7, 0]
    - !!python/tuple [7, 1]
    - !!python/tuple [7, 2]
    - !!python/tuple [7, 3]
    - !!python/tuple [7, 4]
    - !!python/tuple [8, 4]
    - !!python/tuple [9, 4]
    - !!python/tuple [10, 4]
    - !!python/tuple [11, 4]

    - !!python/tuple [1, 11]
    - !!python/tuple [2, 11]
    - !!python/tuple [3, 11]
    - !!python/tuple [4, 11]
    - !!python/tuple [5, 11]
    - !!python/tuple [6, 11]
    - !!python/tuple [7, 11]
    - !!python/tuple [8, 11]
    - !!python/tuple [9, 11]
    - !!python/tuple [10, 11]
    - !!python/tuple [11, 11]

    # right wall
    - !!python/tuple [12, 5]
    - !!python/tuple [12, 6]
    - !!python/tuple [12, 7]
    - !!python/tuple [12, 8]
    - !!python/tuple [12, 9]
    - !!python/tuple [12, 10]
    - !!python/tuple [12, 11]

    - !!python/tuple [2, 3]
    - !!python/tuple [2, 4]

    - !!python/tuple [2, 6]
    - !!python/tuple [2, 7]
    - !!python/tuple [2, 8]
    - !!python/tuple [2, 9]

    - !!python/tuple [4, 2]
    - !!python/tuple [4, 3]
    - !!python/tuple [4, 4]

    - !!python/tuple [4, 6]
    - !!python/tuple [4, 7]
    - !!python/tuple [4, 8]
    - !!python/tuple [4, 9]

    - !!python/tuple [6, 2]
    - !!python/tuple [6, 3]
    - !!python/tuple [6, 4]

    - !!python/tuple [6, 6]
    - !!python/tuple [6, 7]
    - !!python/tuple [6, 8]
    - !!python/tuple [6, 9]

    - !!python/tuple [8, 6]
    - !!python/tuple [8, 7]
    - !!python/tuple [8, 8]
    - !!python/tuple [8, 9]

    - !!python/tuple [10, 6]
    - !!python/tuple [10, 7]
    - !!python/tuple [10, 8]
    - !!python/tuple [10, 9]

[pycckuu]

Found a solution: See my PR

[pycckuu]

The solution: