def Astar(self):
search = []
# Set current node to start and add start node to the node list and node search dictionary
CurrentNode = Node(self.start, self.start, self.goal, self.stepSize)
NodeList = [CurrentNode]
NodeDict = {tuple(CurrentNode.env)}
search.append(CurrentNode)
# Check if the current node is the goal node
while sqrt((CurrentNode.env[0] - self.goal[0]) ** 2 + (CurrentNode.env[1] - self.goal[1]) ** 2) > 1.5:
# Keep checking if there are nodes in list
if len(NodeList) > 0:
# Set current node to the first node in the list and then delete from list
CurrentNode = NodeList.pop()
Course = Environment(CurrentNode.env, self.clearance)
# Check all of the possible actions
for action in Course.possibleMoves(self.start, CurrentNode, self.stepSize):
# Search dictonary and add node to list and dictionary if it hasn't been explored yet
if tuple((int(action.env[0]), int(action.env[1]), action.env[2])) not in NodeDict:
NodeList.append(action)
search.append(action)
NodeDict.add(tuple((int(action.env[0]), int(action.env[1]), action.env[2])))
# Sort list of nodes based on cost
NodeList.sort(key=lambda x: x.weight, reverse=True)
else:
return -1, CurrentNode.path(), search
# solve for path
x = CurrentNode.path()
path = []
for node in x:
path.append(node)
return path, search
def Astar(self):
search = []
# Set current node to start and add start node to the node list and node search dictionary
CurrentNode = Node(self.start, self.start, self.goal, self.stepSize)
NodeList = [CurrentNode]
NodeDict = {tuple(CurrentNode.env)}
search.append(CurrentNode)
# Check if the current node is the goal node
while sqrt((CurrentNode.env[0] - self.goal[0])**2 +
(CurrentNode.env[1] - self.goal[1])**2) > 1.5:
# Keep checking if there are nodes in list
if len(NodeList) > 0:
# Set current node to the first node in the list and then delete from list
CurrentNode = NodeList.pop()
Course = Environment(CurrentNode.env, self.clearance)
# Check all of the possible nodes
for node in Course.possibleMoves(self.start, CurrentNode,
self.stepSize):
# Search dictonary and add node to list and dictionary if it hasn't been explored yet
if tuple((int(node.env[0]), int(node.env[1]),
node.env[2])) not in NodeDict:
NodeList.append(node)
search.append(node)
NodeDict.add(
tuple((int(node.env[0]), int(node.env[1]),
node.env[2])))
sub_nodes = node.sub_nodes
for i in range(len(sub_nodes) - 1):
cv2.line(self.map,
(int(sub_nodes[i][0] * 10),
1020 - int(sub_nodes[i][1] * 10)),
(int(sub_nodes[i + 1][0] * 10),
1020 - int(sub_nodes[i + 1][1] * 10)),
(0, 255, 0))
self.video_output.write(self.map)
# Sort list of nodes based on cost
NodeList.sort(key=lambda x: x.cost, reverse=True)
else:
return -1, CurrentNode.path(), search
# solve for path
x = CurrentNode.path()
path = []
for node in x:
path.append(node)
return 0, path, search
