Skip to content
Snippets Groups Projects
Select Git revision
  • main default protected
  • updated-place-grid
2 results
Blame Permalink
discrete_resolver.py 1.55 KiB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47 






"""
DBResolver solves a discretized version of the conflict resolution problem.
"""

from guided_mrmp.conflict_resolvers.subproblems import SubproblemPlacer
from guided_mrmp.conflict_resolvers.subproblems.subproblem_og import Subproblem, find_subproblem, query_library

class DiscreteResolver():
    def __init__(self, conflict, all_robots, starts, goals, conflicts, grid, lib_2x3, lib_3x3, lib_2x5):
        """
        inputs:
            - conflicts (list): list of all conflicts
            - all_robots (list): list of all robots
            - dt (float): time step
        """
        self.conflict = conflict
        self.conflicts = conflicts
        self.all_robots = all_robots
        self.grid = grid # obstacle map

        self.starts = starts
        self.goals = goals

        self.lib_2x3 = lib_2x3
        self.lib_3x3 = lib_3x3
        self.lib_2x5 = lib_2x5

    def find_subproblem(self):
        conflicts, subproblem = find_subproblem(self.conflict, self.conflicts, [],self.all_robots, self.starts, self.goals, self.grid, True, True)
        return subproblem
    
    def solve_subproblem(self,s):
        sol = query_library(self.grid,s, self.lib_2x3, self.lib_3x3, self.lib_2x5)

        final_sol = []

        for r in range(len(self.starts)):
            this_sol = sol[r]
            final_this_sol = []
            for i in range(len(this_sol)):
                if this_sol[i] != [-1,-1]:
                    final_this_sol.append(s.get_world_coordinates(this_sol[i][0], this_sol[i][1]))
            final_sol.append(final_this_sol)

        return final_sol