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