Ensure that the robots subproblem goals are contained within the subproblem in the discrete world

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
 

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]])