Fixing a repetition of the same HighLevelNode in the priority queue

[pycckuu]

Here is the fix for the CBS algorithm. The problem is that the same HighLevelNode P was recalculated during the searching for the solution.

Testing 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]

A solution of the fixed algorithm

[atb033]

@pyccku

Thanks a lot for the PR. Nice catch. I hadn't considered this case previously.

This PR solves issue #2