diff --git a/guided_mrmp/conflict_resolvers/discrete_resolver.py b/guided_mrmp/conflict_resolvers/discrete_resolver.py
index fd1ecc8e3539cee5b351c6f2f5c8a342161cfa08..522863f347cc97532d33bf914d306e0faaa42d8c 100644
--- a/guided_mrmp/conflict_resolvers/discrete_resolver.py
+++ b/guided_mrmp/conflict_resolvers/discrete_resolver.py
@@ -42,16 +42,5 @@ class DiscreteResolver():
final_this_sol.append(s.get_world_coordinates(this_sol[i][0], this_sol[i][1]))
final_sol.append(final_this_sol)
-
-
- # sol is in subproblem local coordinates
- # convert to global grid coordinates
- # for i in range(len(sol)):
- # for j in range(len(sol[i])):
- # if sol[i][j] != [-1,-1]:
- # sol[i][j] = s.get_world_coordinates(sol[i][j][0], sol[i][j][1])
-
-
-
return final_sol
diff --git a/guided_mrmp/conflict_resolvers/subproblems/subproblem_og.py b/guided_mrmp/conflict_resolvers/subproblems/subproblem_og.py
index cbb059e9ccd18cde355b90ead410c0780598b390..4ddd8bf23df457aa475882f86ac516d48f3aba79 100644
--- a/guided_mrmp/conflict_resolvers/subproblems/subproblem_og.py
+++ b/guided_mrmp/conflict_resolvers/subproblems/subproblem_og.py
@@ -149,6 +149,10 @@ class Subproblem:
x_next <= max_x and x_next >= min_x and y_next <= max_y and y_next >= min_y:
all_robots.append(r)
+ for r in self.conflict:
+ if r not in all_robots:
+ all_robots.append(r)
+
return all_robots
def subproblem_contains_conflict(self, min_x, max_x, min_y, max_y, conflict):
@@ -328,27 +332,27 @@ class Subproblem:
# we want the node that comes directly before this
if x > max_x or x < min_x or y > max_y or y < min_y:
# print(f"triggered node = {node}")
- # print(f"want to assign node {path[j-1]}")
+ print(f"want to assign node {path[j-1]}")
# print(f"temp goals = {temp_goals}")
if [path[j-1][0], path[j-1][1]] not in temp_goals:
- # print(f"Assigned it")
+ print(f"Assigned it")
temp_goals[i] = [path[j-1][0], path[j-1][1]]
assigned = True
break
else:
# print("cant assign desired goal")
- x_rand = random.randint(min_x,max_x)
- y_rand = random.randint(min_y,max_y)
+ x_rand = random.randint(min_x+1,max_x)
+ y_rand = random.randint(min_y+1,max_y)
# print(f"x rand = {x_rand}")
# print(f"y rand = {y_rand}")
# print(f"obs map = {self.map[x_rand][y_rand]}")
while(([x_rand, y_rand] in temp_goals) or (self.map[x_rand][y_rand])):
- x_rand = random.randint(min_x,max_x)
- y_rand = random.randint(min_y,max_y)
+ x_rand = random.randint(min_x+1,max_x)
+ y_rand = random.randint(min_y+1,max_y)
# print(f"x rand = {x_rand}")
# print(f"y rand = {y_rand}")
# print(f"obs map = {self.map[x_rand][y_rand]}")
- # print(f"assigning {(x_rand, y_rand)}")
+ print(f"assigning {(x_rand, y_rand)}")
temp_goals[i] = [x_rand, y_rand]
# print(f"temp goals = {temp_goals}")
@@ -359,17 +363,18 @@ class Subproblem:
break
if not assigned:
# assign a random unassigned node
- x_rand = random.randint(min_x,max_x)
- y_rand = random.randint(min_y,max_y)
+ x_rand = random.randint(min_x+1,max_x)
+ y_rand = random.randint(min_y+1,max_y)
while([x_rand, y_rand] in temp_goals or (self.map[x_rand][y_rand])):
- x_rand = random.randint(min_x,max_x)
- y_rand = random.randint(min_y,max_y)
+ x_rand = random.randint(min_x+1,max_x)
+ y_rand = random.randint(min_y+1,max_y)
+ print(f"assigning {(x_rand, y_rand)}")
temp_goals[i] = [x_rand, y_rand]
assigned = True
# temp_goals.append(path[0])
- # print(f"temp goals assigned = {temp_goals}")
+ print(f"temp goals assigned = {temp_goals}")
return temp_goals
def get_starts(self):
@@ -576,6 +581,9 @@ class Subproblem:
x = bottom_right[0]-x_count
y = bottom_right[1]-y_count
+ print(f"x = {x}")
+ print(f"y = {y}")
+
starts = []
goals = []
@@ -583,6 +591,9 @@ class Subproblem:
start = self.temp_starts[k]
goal = self.temp_goals[k]
+ print(f"goal = {goal}")
+
+
start_x = start[0]
start_y = start[1]
goal_x = goal[0]
@@ -679,6 +690,16 @@ class Subproblem:
# print(f"robot {robot_idx} is left out at position {pos}")
return False
+ for r in self.conflict:
+ pos = r.goal
+ x = pos[0]
+ y = pos[1]
+
+ if x > max_x or x < min_x or y > max_y or y < min_y:
+ # print(f"robot {robot_idx} is left out at position {pos}")
+ return False
+
+
# check that there is no overlap with any other subproblem in S
x = set(range(min_x,max_x+1))
y = set(range(min_y,max_y+1))
@@ -767,6 +788,7 @@ def find_subproblem(c, conflicts, S, robots, starts, goals, obstacle_map, find_b
# print(f"conflicts = {conflicts}")
# type = best[3]
+ print(f"best subproblem = {best_subproblem.transformed_goals}")
return conflicts, best_subproblem
@@ -788,6 +810,8 @@ def order_query(starts, goals):
ordered_goals - The goals in the correct order
new_to_old - The mapping of indices, so that we can recover the original order.
"""
+ print(f"starts = {starts}")
+ print(f"goals = {goals}")
fake_starts = []
for start in starts:
fake_starts.append([3*(start[0]), start[1]])