milestones.append(np.array([5, 5])) for coord in coordinates: circle = Circle(coord, robot.b / 2) if len(m.get_intersecting_rects(circle)) == 0: milestones.append(coord) milestones.append(np.array([70, 15])) 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