Removing resolve_conflicts function from main policy. Just call a conflict resolver instead.

guided_mrmp/conflict_resolvers/__init__.py

 from .traj_opt import TrajOptMultiRobot
+from .traj_opt_resolver import TrajOptResolver
+from .traj_opt_db_resolver import TrajOptDBResolver
 from .local_resolver import LocalResolver
 from .query import QueryDatabase
\ No newline at end of file

guided_mrmp/planners/multirobot/db_guided_mrmp.py

@@ -8,11 +8,11 @@ from shapely.geometry import Polygon, Point
 
 
 
-from guided_mrmp.utils import Conflict, Robot, Env, Plotting
+from guided_mrmp.utils import Env
 from guided_mrmp.utils.helpers import *
 
-from guided_mrmp.conflict_resolvers import LocalResolver
-from guided_mrmp.controllers.path_tracker import plot_roomba
+from guided_mrmp.conflict_resolvers import TrajOptDBResolver
+
 from guided_mrmp.utils import Roomba
 
 
@@ -62,21 +62,6 @@ class GuidedMRMP:
 
         return conflicts
 
-    def resolve_conflict(self, c, conflicts, screen):
-        r1, r2 = c
-
-        # place a grid around the robots in conflict
-        # cr = ConflictResolver(r1.radius*3, 6, c, conflicts, self.robots)
-        # grid = cr.place_grid([r1.current_position, r2.current_position])
-        # self.add_vis_grid(screen, cr.grid_size, cr.top_left_grid, cr.cell_size)
-
-        # create a subproblem on that grid
-        # soln = cr.get_discrete_solution(grid)
-        # print(f"soln = {soln}")
-
-        # solve the subproblem using database soln as a seed to the solver
-
-
     def all_robots_at_goal(self):
         for r in self.robots:
             if (np.sqrt((r.tracker.state[0] - r.goal[0]) ** 2 + (r.tracker.state[1] - r.goal[1]) ** 2) > 0.1):
@@ -101,8 +86,8 @@ class GuidedMRMP:
         conflicts = self.find_all_conflicts(dt)
 
         # resolve the conflicts using the database
-        lr = LocalResolver()
-        lr.resolve_conflict(conflicts, screen)
+        resolver = TrajOptDBResolver()
+        resolver.resolve_conflict(conflicts, screen)
 
 
         # update the state of each robot