search

atb033 / multi_agent_path_planning

Python implementation of a bunch of multi-robot path-planning algorithms.
https://atb033.github.io/multi_agent_path_planning/
MIT License
1.04k stars 256 forks source link

Fixing a repetition of the same HighLevelNode in the priority queue #3

Closed pycckuu closed 3 years ago

pycckuu commented 3 years ago

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 commented 3 years ago

A solution of the fixed algorithm 5 agents

atb033 commented 3 years ago

@pyccku

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

This PR solves issue #2