From 715b7f164ba15d9f013a4246aefcc7f644dbaa11 Mon Sep 17 00:00:00 2001 From: mikoMaz Date: Sun, 16 Apr 2023 12:29:50 +0200 Subject: [PATCH] add graph_search --- graph_search.py | 65 +++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 65 insertions(+) create mode 100644 graph_search.py diff --git a/graph_search.py b/graph_search.py new file mode 100644 index 00000000..2612d594 --- /dev/null +++ b/graph_search.py @@ -0,0 +1,65 @@ +class Node: + def __init__(self, state, parent='', action=''): + self.state = state + self.parent = parent + self.action = action + + +class Search: + def __init__(self, cell_size): + self.cell_size = cell_size + + # WARNING! IT EXCEEDS THE PLANE!!! + def succ(self, state): # successor function + x = state[0] + y = state[1] + angle = state[2] + if angle == 0: + return [['move', x, y - self.cell_size, 0], ['left', x, y, 270], ['right', x, y, 90]] + if angle == 90: + return [['move', x + self.cell_size, y, 90], ['left', x, y, 0], ['right', x, y, 180]] + if angle == 180: + return [['move', x, y + self.cell_size, 180], ['left', x, y, 90], ['right', x, y, 270]] + if angle == 270: + return [['move', x - self.cell_size, y, 270], ['left', x, y, 180], ['right', x, y, 0]] + + def graphsearch(self, istate, goaltest): + x = istate[0] + y = istate[1] + angle = istate[2] + + fringe = [Node([x, y, angle])] # queue (moves/states to check) + explored = [] + + while True: + if len(fringe) == 0: + return False + + elem = fringe.pop(0) + + # if goal_test(elem.state): + # return + # print(elem.state[0], elem.state[1], elem.state[2]) + if elem.state[0] == goaltest[0] and elem.state[1] == goaltest[1]: # checks if we reached the given point + steps = [] + + while elem.parent != '': + steps.append([elem.action, elem.state[0], elem.state[1]]) # should return only elem.action in prod + elem = elem.parent + + steps.reverse() + print(steps) # only for dev + return steps + + explored.append(elem) + + for (action, state_x, state_y, state_angle) in self.succ(elem.state): + if [state_x, state_y, state_angle] not in fringe and [state_x, state_y, state_angle] not in explored: + x = Node([state_x, state_y, state_angle]) + x.parent = elem + x.action = action + fringe.append(x) + + +se = Search(50) +se.graphsearch(istate=[50, 50, 0], goaltest=[250, 100])