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()
