Revised find_all_waiting_robots, queue iteration instead of recursion

guided_mrmp/controllers/multi_path_tracking_db.py

@@ -613,7 +613,7 @@ class MultiPathTrackerDB(MultiPathTracker):
             else: # Discrete solver failed, resolve the conflict another way
                 if waiting: 
                     # Choose all but one robot to "wait" (i.e. controls are 0)
-                    c_wait = c[1:]
+                    c_wait = np.random.choice(c, len(c)-1, replace=False)
                     all_waiting_robots.extend(c_wait)
 
                 else:
@@ -665,24 +665,27 @@ class MultiPathTrackerDB(MultiPathTracker):
         """
         Given a list of robots that have been marked as waiting, find all other robots
         whose next state intersects with the waiting robots' current state. Those robots
-        should also be marked as waiting. This will need to be resursive since the robots
-        that are marked as waiting will also need to be checked for intersections with others
+        should also be marked as waiting.
 
         params:
             - waiting (list): list of robots that have already been marked as waiting
             - x_mpcs (list): list of the next states of all robots
         """
 
-        # check if the next state of any of the robots intersects with the current state of the waiting robots
-        for i in range(self.num_robots):
-            if i in waiting: continue
-            for j in waiting:
-                # print(f"x_mpc[i] = {x_mpc[i]}")
-                i_state = [x_mpc[i][0][1], x_mpc[i][1][1]]
-                if np.linalg.norm(i_state - state[j][:2]) < 2*self.radius:
-                    waiting.append(i)
-                    self.find_all_waiting_robots(waiting, x_mpc)
-                    break
+        # Init queue with all pairs of (i, j) where i is non-waiting and j is waiting
+        queue = [(i, j) for j in waiting for i in range(self.num_robots) if i not in waiting]
+        while queue:
+            i, j = queue.pop(0)
+            if i in waiting: continue # Case where i was originally non-waiting but has since been marked as waiting
+            
+            i_next_state = x_mpc[i][:2][1]
+            j_curr_state = state[j][:2]
+            if np.linalg.norm(i_next_state - j_curr_state) < 2*self.radius:
+                waiting.append(i)
+                # Set i to waiting and add all pairs of (i, k) to the queue for non-waiting k
+                for k in range(self.num_robots):
+                    if k not in waiting:
+                        queue.append((k, i))
 
         return waiting