milestones.append(np.array([90, 50])) milestones.append(np.array([30, 95])) milestones.append(np.array([5, 50])) milestones = np.array(milestones) # Generate kd-tree tree = KdTree(milestones) tree.init_build() # Create connection/adjacency matrix for nodes # Also remove invalid connections connections = np.zeros((tree.nodes.shape[0], tree.nodes.shape[0]), dtype=int) for milestone in milestones: best = tree.get_nn(milestone, 5) for b in best: line_l = Line(best[0].val - robot.b / 2, b.val - robot.b / 2) line_r = Line(best[0].val + robot.b / 2, b.val + robot.b / 2) if m.intersects_line(line_l) or m.intersects_line(line_r): continue connections[best[0].id, b.id] = 1 connections[b.id, best[0].id] = 1 fig, ax = plt.subplots() ax.imshow(pgm) ax.set_xlabel('Position (pixels)') ax.set_ylabel('Position (pixels)')