Skip to content
Snippets Groups Projects
Commit 2214a772 authored by rachelmoan's avatar rachelmoan
Browse files

add back in plotting

parent ec657d7f
No related branches found
No related tags found
No related merge requests found
......@@ -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)
......
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