def _init_graph(self): self._grid_graph = GridGraph(NodeFactory(), self._config.grid_node_count, self._config.grid_rect.size, self._config.destination_point) self._grid_rect = self._config.grid_rect self._init_obstacles(self._config.destination_point) self._calculate_node_safety_levels(self._config.actor_size, self.INITIAL_SAFETY_AGGRESSIVENESS) self._set_starting_node(self._config.starting_point) self._set_destination_node(self._config.destination_point) self._create_edges() self._log_pathfinding_step("Path Finding Graph")
class Pathfinder(object): INITIAL_SAFETY_AGGRESSIVENESS = 1 SIMPLIFICATION_SAFETY_AGGRESSIVENESS = 1 def __init__(self, path_simplifier): self._path_simplifier = path_simplifier self._init_attributes() def _init_attributes(self): self._grid_graph = None self._obstacles = [] self._node_path = [] self._starting_node = None self._destination_node = None self._drawable_path = None def reset(self): self._init_attributes() def find_path(self, pathfinder_config): self.reset() self._config = pathfinder_config self._init_graph() self._node_path = self._apply_dijkstra() if self._config.simplify_path: self._simplify_path(self._node_path) self._drawable_path = self._node_path.drawable_path return self._node_path def draw(self, image, draw_pathfinding_nodes = True): if self._grid_graph is None: return image self._draw_pathfinding_nodes(image, draw_pathfinding_nodes) if self._drawable_path is not None: OpenCV.draw_path(image, self._drawable_path.clone().offset(self._grid_rect.location), path_color = Color.BLUE, path_thickness = 2, vertex_color = Color.DIM_GRAY, vertex_radius = 3) def _draw_pathfinding_nodes(self, image, draw_pathfinding_nodes): for node in self._grid_graph.nodes: if node is self._starting_node or node is self._destination_node: OpenCV.fill_circle(image, node.position.clone().offset(self._grid_rect.location), radius = 5, color = Color.RED) elif node.is_safe and draw_pathfinding_nodes: OpenCV.fill_circle(image, node.position.clone().offset(self._grid_rect.location), radius = 1, color = node.safety_level.node_color) def _init_graph(self): self._grid_graph = GridGraph(NodeFactory(), self._config.grid_node_count, self._config.grid_rect.size, self._config.destination_point) self._grid_rect = self._config.grid_rect self._init_obstacles(self._config.destination_point) self._calculate_node_safety_levels(self._config.actor_size, self.INITIAL_SAFETY_AGGRESSIVENESS) self._set_starting_node(self._config.starting_point) self._set_destination_node(self._config.destination_point) self._create_edges() self._log_pathfinding_step("Path Finding Graph") def _init_obstacles(self, destination_point): for obstacle in self._config.obstacles: if obstacle.contains(destination_point): obstacle.disabled = True def _calculate_node_safety_levels(self, actor_size, aggressiveness): for node in self._grid_graph.nodes: node.obstacle_safety_level = self._calculate_obstacle_safety_level(node, actor_size, aggressiveness) node.wall_safety_level = NodeSafetyLevel.from_distance(self._grid_graph.rect.distance(node.position), actor_size / 2, aggressiveness) def _calculate_obstacle_safety_level(self, node, actor_size, aggressiveness): active_obstacles = self._get_active_obstacles() if len(active_obstacles) > 0: obstacle_distance = min([obstacle.distance(node.position) for obstacle in active_obstacles]) return NodeSafetyLevel.from_distance(obstacle_distance, actor_size / 2, aggressiveness) else: return NodeSafetyLevel.NORMAL def _get_active_obstacles(self): return list(filter(lambda obstacle: not obstacle.disabled, self._config.obstacles)) def _set_starting_node(self, starting_point): self._starting_node = self._get_closest_safe_node_from_point(starting_point) def _set_destination_node(self, destination_point): self._destination_node = self._get_closest_safe_node_from_point(destination_point) def _get_closest_safe_node_from_point(self, point): return min(self._get_safe_nodes(), key=lambda node: Segment(point, node.position).length) def _get_safe_nodes(self): return filter(lambda node: node.is_safe, self._grid_graph.nodes) def _create_edges(self): self._grid_graph.create_edges(self._edge_creation_callback) def _edge_creation_callback(self, source_node, target_node): if not target_node.is_safe or not source_node.is_safe: return GridGraph.NULL_EDGE_WEIGHT else: return self._calculate_edge_weight(source_node, target_node) def _calculate_edge_weight(self, source_node, target_node): return Segment(source_node.position, target_node.position).length * target_node.safety_level.weight_multiplier def _apply_dijkstra(self): nodes = self._grid_graph.dijkstra(self._starting_node, self._destination_node) self._log_pathfinding_step("Path Finding Raw Path") return NodePath(nodes) def _simplify_path(self, node_path): self._calculate_node_safety_levels(self._config.actor_size, self.SIMPLIFICATION_SAFETY_AGGRESSIVENESS) self._path_simplifier.simplify_path(node_path, self._grid_graph, self._config.max_segment_length) self._log_pathfinding_step("Path Finding Simplified Path") # Logging def _log_pathfinding_step(self, description): if Logger.get_instance().save_blob_data: self._config.reference_image.save_state() self.draw(self._reference_image) Logger.get_instance().log(self, "Path Finding Step", details = description, blob_data = self._config.reference_image) self._config.reference_image.restore_state()