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
