def generate_graph_node(self, start_x, start_y, goal_x, goal_y, obstacles):
# add start and goal as nodes
nodes = [DijkstraSearch.Node(start_x, start_y),
DijkstraSearch.Node(goal_x, goal_y, 0, None)]
# add vertexes in configuration space as nodes
for obstacle in obstacles:
cvx_list, cvy_list = self.calc_vertexes_in_configuration_space(
obstacle.x_list, obstacle.y_list)
for (vx, vy) in zip(cvx_list, cvy_list):
nodes.append(DijkstraSearch.Node(vx, vy))
for node in nodes:
plt.plot(node.x, node.y, "xr")
return nodes
def planning(self, sx, sy, gx, gy, ox, oy, robot_radius):
obstacle_tree = KDTree(np.vstack((ox, oy)).T)
sample_x, sample_y = self.voronoi_sampling(sx, sy, gx, gy, ox, oy)
if show_animation: # pragma: no cover
plt.plot(sample_x, sample_y, ".b")
road_map_info = self.generate_road_map_info(
sample_x, sample_y, robot_radius, obstacle_tree)
rx, ry = DijkstraSearch(show_animation).search(sx, sy, gx, gy,
sample_x, sample_y,
road_map_info)
return rx, ry
def planning(self, start_x, start_y, goal_x, goal_y, obstacles):
nodes = self.generate_graph_node(start_x, start_y, goal_x, goal_y,
obstacles)
road_map_info = self.generate_road_map_info(nodes, obstacles)
if self.do_plot:
self.plot_road_map(nodes, road_map_info)
plt.pause(1.0)
rx, ry = DijkstraSearch(show_animation).search(
start_x, start_y, goal_x, goal_y, [node.x for node in nodes],
[node.y for node in nodes], road_map_info)
return rx, ry
