add back in plotting

guided_mrmp/controllers/multi_path_tracking_db.py

@@ -435,10 +435,8 @@ class MultiPathTrackerDB(MultiPathTracker):
 
         for c in all_conflicts:
             # 3. If they do collide, then reroute the reference trajectories of these robots
-            # Get the robots involved in the conflict
-            robot_positions = [ [state[i][:2]] for i in c ]
-            # for i in (c):
-            #     robot_positions.append(state[i][:2])
+            # Get the positions of robots involved in the conflict
+            robot_positions = [ state[i][:2] for i in c ]
 
             # get the largest x and y difference between the robots in conflict
             x_diff = max([abs(robot_positions[i][0] - robot_positions[j][0]) for i in range(len(c)) for j in range(len(c))])
@@ -478,6 +476,9 @@ class MultiPathTrackerDB(MultiPathTracker):
                                                                         self.settings["database"]["smoothing_sigma"]
                                                                         )
                     
+                    if show_plots:
+                        draw_grid_solution(self.num_robots, self.radius, self.grid_size, self.cell_size, self.env, self.trajs, continuous_soln, state, grid_origin, grid_obstacle_map, c, time)
+
                     # for each robot in conflict, reroute its reference trajectory to match the grid solution
                     import copy
                     old_paths = copy.deepcopy(self.paths)