From e89d1b67b930379ded0d2c05f7196d845fb2f42d Mon Sep 17 00:00:00 2001
From: rachelmoan <moanrachel516@gmail.com>
Date: Mon, 19 Aug 2024 17:23:50 -0500
Subject: [PATCH] Removing resolve_conflicts function from main policy. Just
call a conflict resolver instead.
---
guided_mrmp/conflict_resolvers/__init__.py | 2 ++
.../planners/multirobot/db_guided_mrmp.py | 25 ++++---------------
2 files changed, 7 insertions(+), 20 deletions(-)
diff --git a/guided_mrmp/conflict_resolvers/__init__.py b/guided_mrmp/conflict_resolvers/__init__.py
index e0fde94..3513a7b 100644
--- a/guided_mrmp/conflict_resolvers/__init__.py
+++ b/guided_mrmp/conflict_resolvers/__init__.py
@@ -1,3 +1,5 @@
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
diff --git a/guided_mrmp/planners/multirobot/db_guided_mrmp.py b/guided_mrmp/planners/multirobot/db_guided_mrmp.py
index c45ab87..c67f836 100644
--- a/guided_mrmp/planners/multirobot/db_guided_mrmp.py
+++ b/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
--
GitLab