def solve(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 draw():
global count
pygame.draw.polygon(display, (138, 132, 226), hexagon)
pygame.draw.polygon(display, (138, 132, 226), rectangle)
pygame.draw.polygon(display, (138, 132, 226), diamond)
pygame.draw.circle(display, (138, 132, 226), ((225 * multiplier), height - (150 * multiplier)), 25 * multiplier)
pygame.draw.ellipse(display, (138, 132, 226), pygame.Rect(ellipse))
if count > 0:
env = Environment(coordinates[0], clearance)
if env.possiblePostion([int(coordinates[0][0] / multiplier), int(200 - coordinates[0][1] / multiplier)]):
if radius != 0:
pygame.draw.circle(display, (0, 0, 255), (coordinates[0][0], coordinates[0][1]), radius * multiplier, 1)
pygame.draw.rect(display, (0, 0, 255),
pygame.Rect(coordinates[0][0], coordinates[0][1], multiplier, multiplier))
textsurface = myfont.render("Initial Postion", False, (255, 0, 0))
if height - coordinates[0][1] > 40:
display.blit(textsurface, (coordinates[0][0] - 10 * multiplier, coordinates[0][1] + multiplier))
else:
display.blit(textsurface, (coordinates[0][0] - 10 * multiplier, coordinates[0][1] + multiplier - 40))
else:
print("Invalid position")
count = 0
coordinates.pop(0)
if count > 1:
env = Environment(coordinates[1], clearance)
if env.possiblePostion([int(coordinates[1][0] / multiplier), int(200 - coordinates[1][1] / multiplier)]):
if radius != 0:
pygame.draw.circle(display, (0, 0, 255), (coordinates[1][0], coordinates[1][1]), radius * multiplier, 1)
pygame.draw.rect(display, (0, 0, 255),
pygame.Rect(coordinates[1][0], coordinates[1][1], multiplier, multiplier))
textsurface = myfont.render("Goal Postion", False, (255, 0, 0))
if height - coordinates[1][1] > 40:
display.blit(textsurface, (coordinates[1][0] - 10 * multiplier, coordinates[1][1] + multiplier))
else:
display.blit(textsurface, (coordinates[1][0] - 10 * multiplier, coordinates[1][1] + multiplier - 40))
else:
print("Invalid position")
count = 1
coordinates.pop(1)
pygame.display.flip()
clock.tick(ticks)
clearance = 0
radius = 0
else:
# add a radius and clearance for a non-point robot
radius = int(input("Enter radius: "))
clearance = int(input("Enter Clearance: "))
clearance += radius
multiplier = 4
height, width = 200 * multiplier, 300 * multiplier
running = True
count = 0
coordinates = []
env = Environment([0, 0], clearance)
startBool = True
goalBool = True
# Manual Mode
if manual:
while startBool:
# Get start node from user
startPos = input("Enter start position: ")
# Format input to use in code
if "," in startPos:
startPos = startPos.replace(" ", "")
startPos = startPos.split(",")
else:
startPos = startPos.split(" ")