244 lines
12 KiB
Python
244 lines
12 KiB
Python
import DRDViz
|
|
import math
|
|
|
|
def hSLD(node, search):
|
|
# take the node value in
|
|
# find the node value coordinates
|
|
coords = search.nodeLocs[node]
|
|
goalCoords = search.nodeLocs[search.endNode]
|
|
# calculate the distance between the two points
|
|
distance = math.sqrt((abs(int(coords[0]) - int(goalCoords[0])))**2 + abs(int(coords[1]) - int(goalCoords[1]))**2)
|
|
return distance
|
|
|
|
class SearchNode:
|
|
def __init__(self, label, pathcost):
|
|
self.label = label
|
|
self.pathcost = pathcost
|
|
self.node = tuple((label, pathcost))
|
|
def showBasic(self):
|
|
# print(self.node) # debugging purposes
|
|
return self.node
|
|
def __eq__(self, object):
|
|
return ((self.label) == (object.label))
|
|
def __lt__(self, object):
|
|
return ((self.label) < (object.label))
|
|
def __repr__(self):
|
|
return repr(self.node)
|
|
|
|
class Searcher:
|
|
def __init__(self, mapfile, searchType, verbose=False):
|
|
self.mapfile = mapfile
|
|
self.searchType = searchType
|
|
self.verbose = verbose
|
|
# let the user know what searchtype is used and the filename
|
|
if self.verbose: print(f"Loaded search type {self.searchType} with map in file: {self.mapfile}")
|
|
# load the mapfile's structure
|
|
with open(self.mapfile) as mapper:
|
|
mapContents = mapper.read()
|
|
mapContents = mapContents.split("\n")
|
|
for mapIter in range(len(mapContents)):
|
|
mapContents[mapIter] = mapContents[mapIter].split(", ")
|
|
for firstDimMapIter in range(len(mapContents) - 1):
|
|
for secondDimMapIter in range(len(mapContents[firstDimMapIter])):
|
|
mapContents[firstDimMapIter][secondDimMapIter] = mapContents[firstDimMapIter][secondDimMapIter].strip("')(][")
|
|
mapContents.pop(len(mapContents) - 1)
|
|
for tupleIter in range(len(mapContents)):
|
|
mapContents[tupleIter] = tuple(mapContents[tupleIter])
|
|
# print(mapContents) # list of tuples
|
|
# print(mapContents[0]) # tuple of strings (data)
|
|
# [first node, second node, path cost, first node x, first node y, second node x, second node y]
|
|
# 0 1 2 3 4 5 6
|
|
# print(mapContents[0][0]) # data, string format
|
|
# make two dictionaries of nodes, iterating through list of tuples to make note of every node's paths and locations
|
|
nodeLoc = dict()
|
|
nodePaths = dict()
|
|
for nodeIter in range(len(mapContents)):
|
|
# these node locations are fine, because they should technically be in only one place.
|
|
nodeLoc[mapContents[nodeIter][0]] = tuple((mapContents[nodeIter][3], mapContents[nodeIter][4]))
|
|
nodeLoc[mapContents[nodeIter][1]] = tuple((mapContents[nodeIter][5], mapContents[nodeIter][6]))
|
|
# however, this is only taking the first node path it gets to...
|
|
# I probably need a list instead, that gets updated if the node is not in the node's child list.
|
|
# check if the key exists, if it does, then add the value to the internal dictionary,
|
|
# if the internal dictionary doesn't exist, then create it and add the node.
|
|
if mapContents[nodeIter][0] not in nodePaths.keys():
|
|
nodePaths[mapContents[nodeIter][0]] = dict()
|
|
nodePaths[mapContents[nodeIter][0]][mapContents[nodeIter][1]] = mapContents[nodeIter][2]
|
|
else:
|
|
if nodePaths[mapContents[nodeIter][0]].values() not in nodePaths[mapContents[nodeIter][0]].keys():
|
|
nodePaths[mapContents[nodeIter][0]][mapContents[nodeIter][1]] = dict()
|
|
nodePaths[mapContents[nodeIter][0]][mapContents[nodeIter][1]] = mapContents[nodeIter][2]
|
|
else:
|
|
nodePaths[mapContents[nodeIter][0]][mapContents[nodeIter][1]] = mapContents[nodeIter][2]
|
|
if mapContents[nodeIter][1] not in nodePaths.keys():
|
|
nodePaths[mapContents[nodeIter][1]] = dict()
|
|
nodePaths[mapContents[nodeIter][1]][mapContents[nodeIter][0]] = mapContents[nodeIter][2]
|
|
else:
|
|
if nodePaths[mapContents[nodeIter][1]].values() not in nodePaths[mapContents[nodeIter][1]].keys():
|
|
nodePaths[mapContents[nodeIter][1]][mapContents[nodeIter][0]] = dict()
|
|
nodePaths[mapContents[nodeIter][1]][mapContents[nodeIter][0]] = mapContents[nodeIter][2]
|
|
else:
|
|
nodePaths[mapContents[nodeIter][1]][mapContents[nodeIter][0]] = mapContents[nodeIter][2]
|
|
self.nodePaths = nodePaths
|
|
self.nodeLocs = nodeLoc
|
|
# need to get the myViz attribute by importing the DRDViz stuff.
|
|
self.myViz = DRDViz.DRDViz()
|
|
# now, start using the myViz thingy to 'display' a map.
|
|
self.myViz.loadGraphFromFile(mapfile)
|
|
self.myViz.plot()
|
|
# create an 'open' list
|
|
self.open = list()
|
|
def setStartGoal(self, startNode, endNode):
|
|
self.startNode = startNode.upper()
|
|
self.endNode = endNode.upper()
|
|
self.myViz.markStart(self.startNode)
|
|
self.myViz.markGoal(self.endNode)
|
|
# add the starting node to the 'open' list.
|
|
starting = SearchNode(startNode, 0)
|
|
self.open.append(starting)
|
|
def successors(self, parentNode):
|
|
childList = list()
|
|
# grab the nodes from the nested dictionary given that the parentNode is a tuple.
|
|
nodeChildren = self.nodePaths.get(parentNode.label)
|
|
# grab each value from the dictionary, turn it into a SearchNode.
|
|
for node in nodeChildren.keys():
|
|
childList.append(SearchNode(node, int(nodeChildren[node])))
|
|
# sort the child node list
|
|
childList.sort()
|
|
return (childList)
|
|
def reset(self):
|
|
# clear all the items from the open list
|
|
self.open.clear()
|
|
# add the start node back into the list.
|
|
self.open.append(SearchNode(self.startNode, 0))
|
|
# do I need to reset the visualization too?
|
|
def insert_front(self, nodeList):
|
|
# merge the existing open list with the new nodes
|
|
# had to minorly change the algorithm from the previous version.
|
|
combinedList = sorted(self.open + [item for item in nodeList if item not in self.open])
|
|
self.open = combinedList
|
|
def insert_end(self, nodeList):
|
|
# merge the existing open list with the new nodes
|
|
combinedList = sorted(self.open + [item for item in nodeList if item not in self.open])
|
|
self.open = combinedList
|
|
def insert_ordered(self, nodeList):
|
|
# merge the existing open list with the new nodes based on path cost
|
|
combinedList = sorted(self.open + [item for item in nodeList if item not in self.open], key=lambda item: item.pathcost)
|
|
self.open = combinedList
|
|
def search(self):
|
|
# reset the open list if another search is done at a different time
|
|
self.reset()
|
|
# keep track of the visited nodes as a set
|
|
visited = set()
|
|
# parent dictionary
|
|
parentNode = {self.startNode: None}
|
|
# check to see if the A* algorithm is being used, if it is, use
|
|
# the heuristic functions
|
|
if self.searchType.lower() == "a*":
|
|
goalCosts = {self.startNode: 0}
|
|
totalCosts = {self.startNode: hSLD(self.startNode, self)}
|
|
# while loop through the open list, inserting successor nodes to the open list
|
|
# depending on the algorithm
|
|
while self.open:
|
|
# grab the current node, if not A*
|
|
if self.searchType.lower() != "a*":
|
|
currentNode = self.open.pop(0)
|
|
else:
|
|
# if A* search, then grab the node with the lowest totalCost
|
|
currentNode = min(self.open, key=lambda node: totalCosts.get(node.label, float('inf')))
|
|
# remove it from the open list
|
|
self.open.remove(currentNode)
|
|
# print out the node explored if verbosity is set
|
|
if self.verbose: print(f"Exploring node: {currentNode.label}")
|
|
# use the DRDViz tool to plot the explored node.
|
|
# self.myViz.exploreNode(currentNode.label, visited)
|
|
# check if the current node is the goal
|
|
if currentNode.label == self.endNode:
|
|
tempNode = currentNode
|
|
# backtrack to find the path taken
|
|
pathTaken = []
|
|
# loop through the node list
|
|
while currentNode:
|
|
# append the node to the path list
|
|
pathTaken.append(currentNode.label)
|
|
# grab the node from the parentNode dictionary
|
|
currentNode = parentNode[currentNode.label]
|
|
# reverse the pathTaken list, and display it.
|
|
pathTaken.reverse()
|
|
print(f"Success! Reached goal node {self.endNode} with path: {pathTaken}")
|
|
self.myViz.exploreNode(tempNode.label, pathTaken)
|
|
# save the DRDViz plot.
|
|
if self.searchType.lower() != "a*":
|
|
self.myViz.save(f"{self.mapfile[:-4]}-{self.searchType}.png")
|
|
else:
|
|
self.myViz.save(f"{self.mapfile[:-4]}-ASTAR.png")
|
|
# return 'early' to prevent further traversal
|
|
return
|
|
# check to see if the current node isn't in the visited set
|
|
if currentNode.label not in visited:
|
|
# add the node to the visited set
|
|
visited.add(currentNode.label)
|
|
# find the successors to the current node
|
|
successors = self.successors(currentNode)
|
|
# print the successor list if verbosity is set
|
|
if self.verbose: print(f"Inserting new children: {successors}")
|
|
# insert the successors in the open list, depending on the algorithm
|
|
for successor in successors:
|
|
# if successor.label not in visited:
|
|
if successor.label not in visited and successor not in self.open:
|
|
parentNode[successor.label] = currentNode
|
|
if self.searchType.lower() == "depth":
|
|
self.insert_front([successor])
|
|
elif self.searchType.lower() == "breadth":
|
|
self.insert_end([successor])
|
|
elif self.searchType.lower() == "best":
|
|
self.insert_ordered([successor])
|
|
elif self.searchType.lower() == "a*":
|
|
# since this is A*, now we need to compute all the heuristic values for the nodes
|
|
currentGoalCost = goalCosts[currentNode.label] + successor.pathcost
|
|
# check if the successor is not in the goalCosts or in the currentGoalCost and less than the goalCosts list
|
|
if successor.label not in goalCosts or currentGoalCost < goalCosts[successor.label]:
|
|
# add the successor to the goal and totalCosts dictionaries
|
|
goalCosts[successor.label] = currentGoalCost
|
|
totalCosts[successor.label] = currentGoalCost + hSLD(successor.label, self)
|
|
# then, append itself to the open list.
|
|
if successor not in self.open:
|
|
self.open.append(successor)
|
|
if self.verbose: print(f"Open List: {self.open}")
|
|
# in the event that the while loop completes and never actually reaches a goal
|
|
# print that if verbosity is set.
|
|
if self.verbose: print("Unable to reach goal.")
|
|
|
|
# 10test.txt map
|
|
x=Searcher("10test.txt", searchType="DEPTH", verbose=True)
|
|
x.setStartGoal('h','k')
|
|
x.search()
|
|
|
|
x=Searcher("10test.txt", searchType="BREADTH", verbose=True)
|
|
x.setStartGoal('h','k')
|
|
x.search()
|
|
|
|
x=Searcher("10test.txt", searchType="BEST", verbose=True)
|
|
x.setStartGoal('h','k')
|
|
x.search()
|
|
|
|
x=Searcher("10test.txt", searchType="A*", verbose=True)
|
|
x.setStartGoal('h','k')
|
|
x.search()
|
|
|
|
# 50test.txt map
|
|
x=Searcher("50test.txt", searchType="DEPTH", verbose=False)
|
|
x.setStartGoal('s','c')
|
|
x.search()
|
|
|
|
x=Searcher("50test.txt", searchType="BREADTH", verbose=False)
|
|
x.setStartGoal('s','c')
|
|
x.search()
|
|
|
|
x=Searcher("50test.txt", searchType="BEST", verbose=False)
|
|
x.setStartGoal('s','c')
|
|
x.search()
|
|
|
|
x=Searcher("50test.txt", searchType="A*", verbose=False)
|
|
x.setStartGoal('s','c')
|
|
x.search()
|
|
|