def test_traversal():
root = Node(Pose(1, 1),
children=[
Node(Pose(2, 2)),
Node(Pose(3, 3),
children=[Node(Pose(5, 5)),
Node(Pose(4, 4))]),
Node(Pose(6, 6))
])
for node in root.traverse():
print(node)
def plot_tree(ax, rr_tree: Node, grid_size: int, start: Node, end: Node,
reached_target: bool = False, last_node: Node = None, obstacles: list[Obstacle] = None,
fast_plot: bool = False) -> None:
# Add obstacles to the Axes
if obstacles is not None:
for obs in obstacles:
rect = patches.Rectangle((obs.x, obs.y), obs.width, obs.height,
linewidth=1, edgecolor='none', facecolor='gray')
ax.add_patch(rect)
ax.set_xlim(-grid_size * 0.2, grid_size * 1.2)
ax.set_ylim(-grid_size * 0.2, grid_size * 1.2)
ax.set_aspect('equal')
ax.grid()
nodes = []
connections = []
for node in rr_tree.traverse():
nodes.append(node)
for child in node.children:
x1, y1 = node.pose.x, node.pose.y
x2, y2 = child.pose.x, child.pose.y
connections.append([[x1, x2], [y1, y2]])
# Draw connections to children
for connection in connections:
ax.plot(connection[0], connection[1], 'k--', linewidth=0.5, zorder=8)
# Draw the nodes themselves
x_nodes = [node.pose.x for node in nodes]
y_nodes = [node.pose.y for node in nodes]
if fast_plot: # use plt.plot instead of plt.scatter which is super slow
ax.plot(x_nodes, y_nodes, 'b.', zorder=10)
else:
colors = [end.distance_to(node) for node in nodes]
ax.scatter(x_nodes, y_nodes, c=colors, s=15, vmin=0, vmax=90, zorder=10)
ax.scatter([start.pose.x], [start.pose.y], c='red', s=60, zorder=11)
ax.scatter([end.pose.x], [end.pose.y], c='green', s=100, zorder=11)
if reached_target:
# Draw the actual path from goal to start
node = last_node
while node.parent is not None:
ax.plot([node.pose.x, node.parent.pose.x], [node.pose.y, node.parent.pose.y], 'r-')
plt.draw()
plt.pause(1e-17)
node = node.parent
time.sleep(0.05)
plt.draw()
plt.pause(1e-17)
ax.clear()
def find_closest_node(new_node: Node, rr_tree: Node,
obstacles: list[Obstacle]) -> Optional[Node]:
"""For a new node, find the nearest node in the tree. Return this nearest node if there is an obstacle-free
connection between the new and nearest node, else return None."""
closest_node = None
closest_distance = 1e6
for node in rr_tree.traverse():
distance = new_node.distance_to(node)
if distance < closest_distance:
closest_node = node
closest_distance = distance
all_object_lines = []
for obs in obstacles:
all_object_lines.extend(obs.segments)
seg_to_closest = [(new_node.pose.x, new_node.pose.y),
(closest_node.pose.x, closest_node.pose.y)]
if any([intersects(seg, seg_to_closest) for seg in all_object_lines]):
return None
else:
return closest_node