diff --git a/guided_mrmp/conflict_resolvers/__init__.py b/guided_mrmp/conflict_resolvers/__init__.py index e0fde9419737dbd9d30e3773204d49a30f58755a..3513a7b3d269e948f170269f4f0ebecdf13bd51c 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 c45ab87c4a93933806f0080635bd4fd7b020855a..c67f836b3614ee5c6213993b6e7d6249d6bfdfed 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