Skip to content
Snippets Groups Projects
Commit 89a0dfd2 authored by Adam Sitabkhan's avatar Adam Sitabkhan
Browse files

Revised find_all_waiting_robots, queue iteration instead of recursion

parent 82ee7378
No related branches found
No related tags found
No related merge requests found
......@@ -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
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment