From 3c3b5161454395bad383832ec1a847c6bdb228ca Mon Sep 17 00:00:00 2001 From: mdeyo Date: Tue, 11 Jul 2017 16:59:24 -0400 Subject: [PATCH] first commit --- README.md | 1 + __pycache__/d_star_lite.cpython-35.pyc | Bin 0 -> 4235 bytes __pycache__/graph.cpython-35.pyc | Bin 0 -> 2591 bytes __pycache__/grid.cpython-35.pyc | Bin 0 -> 2104 bytes __pycache__/utils.cpython-35.pyc | Bin 0 -> 320 bytes d_star_lite.py | 152 +++++++++++++++++++++++ graph.py | 100 +++++++++++++++ grid.py | 55 +++++++++ main.py | 162 +++++++++++++++++++++++++ utils.py | 4 + 10 files changed, 474 insertions(+) create mode 100644 README.md create mode 100644 __pycache__/d_star_lite.cpython-35.pyc create mode 100644 __pycache__/graph.cpython-35.pyc create mode 100644 __pycache__/grid.cpython-35.pyc create mode 100644 __pycache__/utils.cpython-35.pyc create mode 100644 d_star_lite.py create mode 100644 graph.py create mode 100644 grid.py create mode 100644 main.py create mode 100644 utils.py diff --git a/README.md b/README.md new file mode 100644 index 0000000..86f60be --- /dev/null +++ b/README.md @@ -0,0 +1 @@ +# d-star-lite diff --git a/__pycache__/d_star_lite.cpython-35.pyc b/__pycache__/d_star_lite.cpython-35.pyc new file mode 100644 index 0000000000000000000000000000000000000000..4b25aa0791585fe346495c073f11c70e1a79ad59 GIT binary patch literal 4235 zcmai1&2JlN8GqjSupP%~(xk2HZdbUgvM{Kd?k++|G>W=vSb-=3HLI4jR9UWP61y3D zoOj05jS`S3pAtd>i3=R}fIxyjf&)h`tG$f4aq5*Dd!YP&&%}-!7S)Nz&-?v+{GQ+Q z&ZVWL>N~fh5C3sV{0%Nw_FsO=V12lJK0$6$zImTu~X5&r7(fa#g}L2`{LOg+_LQi_f8v zYzHPkpJtsb+U^XZ50X1cV#Bm+IED{@U3~B0n|=e2Baz1<&oGi1Hl7*ow(p5uXA^RR zRyy(%M;besBd`l|X2Px`JD9K&c-xknKc|Cow?Ets+}ukOo8@jC4Re1tPC6Ns^AE<+ zIQpa51x)<(&6~Ys5ZxSv(bME+_!bnmZ^ivAx*4`9w<)iWp5~=28GS2y>SIk>!9z^d zSv9M!U4-1FMf`A>U$yQR5HkG3EF+JhxKZO^225zs28=kLIdU1J$L`Ckz5l!V?1--R zBy;5m{}_df0)IO*kl?%H0e+612V z*$W(4NZZ}9wGq7G?zabL&8u}gad#X8*ucR>EJ%qoG4?8+U7ErVK~=_=h)7?DyJ?2V zu`fS|?909@_6Z|jpy9YAlRG-c;e^IxSGFF$BgbVmw{$XcRK|$^@-+6<;TG(0@UA>K zn$G1e?UMXVWEY2My8Kk#wna=;xcm7mpNb4FVtIFd=Mk=dOXBuAM(4!2ssJGFit!!HYp-WuyLzl9HkxJjd z1G-|1hnMBp=tiK1l2{yTu={CQg#p+4eBX8Kdo-W0B1KDj5e=v zcOJDMht2UQL`?5THj5_f8*zd#r<)L_lkE~F9-bQ{Hfmi7f$8fUVN)_2UqjvUT=d=A*#}(;~0l@5JNi>((YVFWG32d_T(lUep;K zEyN}lV8m*t{_LRGVwB`1@?f!(6&&@7jy70 zd4SW91xP4>SfE4!rch&XQUKZO zkn*yEDjC_&YiraY*`*p>#cWVMg;`!swE|{}vX;N!PGVGpvHc?VB+Z@XR=&ox8fVc? z5A`-mv+s7Y-j^^XB}~O^m1<_qyk-JG=?Vb$x^u}~!Y@iZV5Kw)fhob27w*|mrDA4E4=yW!LeRbkY!$L^j zrF@L2dOZ2yE;ttK_B(=hC&P6~uOXmE4FfJ8RQ+)E!(`lqTbm(SBF)C#{ln|sWE_Xh zVUjg>`@^uQl+fHwY%{Z+VcO67$?zInKVuDLh}<7p6lP1VDtLo*7?g?#Y5IQw9WA8) zFq&i##J|#{Bsa`Ty&5~1h-f$>a@%0@0HpXV9t%e2( zNaOPf(!Q^C4Ix4*z$@_nzAyHXOx~MfzMzAxhc~sN!Ixzw1gj7NfpDUxU`m6(Nb?4r zUzUx70N~cjHuZ7-jU!}YRW>Hyo}P?8M0)GtEuM@` zF=4+f_9I>#U!?{1q81+^jenq(70P^-HZHJ^A4Jg3mE%R3{Jl`*Z*z*k4uYp_CVxJw z%I{T`f`bb6a7+WFh3X5huy(E+s#iU666)<|`e|df=7d^!Jphv7fU_gBb2h-Km`$#N z(-hUuKs>1OFyvqzj8ZlajuvrULPtY#)y|R;>pwFbO+g+icTfRMD86kr)?`z{}}T(8c`-7nos zl#{;9@#(!J?xYM92y2I3E#B0Hvepr!UYj?TxOp}MQOW;6n&)Y=T23p^N75%F-XF3 e9Bne;Q{F3UX2q>lYo(f3J72q0TdUout^F4RmkyQy literal 0 HcmV?d00001 diff --git a/__pycache__/graph.cpython-35.pyc b/__pycache__/graph.cpython-35.pyc new file mode 100644 index 0000000000000000000000000000000000000000..8b69dbf597a38028146b233ec793ebb17804b4b1 GIT binary patch literal 2591 zcmb_eTW{P%6h7nIUg}L!(vqe@!Y?5FkVZ)*kfH#QNEIQ$@(@I6L}i(^vvJrD(QDO_rCT1y097%W)5ij} zi;?q`ViyEp#yJpHeGW$*p{KI32TG>0y-K!my5l76u~dHepT`#%`KOq!6kQQTL>Xuh zswONdphVG4xxu;p3RMKtt*)`N(NX9bD@#n{G}h^;$W60%oDCAt#yuL0-hBe$?2tOMo~7(iYQ{s zb3R0><=|&6T9Xd@_oUv@Q@o0Y|5=MTx())az`qls6-05RDBmo0Zw2|P8ORwJ2cgrQ zcr183m^eWnvlqbr?I3i-c4Oy-)(wc&O-!NEEvDJDa=Hh~G@=OGqUb(ea>;<2_?5Y` zaLZb@Sgd6!rKvX@W7UQD1@PXY1<}#zDbzfeQBy~yT)8{_9oAE)U(zY699CtO4lzCN z)xvpl8W(9)C3|H?>#{g^d%YVrIDw;hm`0IlN6~PcoDZ1xqv-5B9+WE@KqGNeFEX`s zTeC!#gQ_`9JaP`aulz;d_HE(zz?eQ9?p=)hD@^|pMoC+s25j9!K78Zub7kI29;=h& zaZoELq2}uR8CQz=?hK0S@3E~_jVy@#H*4C)9?^lRr7)UCs>3yP{I#kSkl$2~&bi>2W}X%Y zERL_(BoFc566sYK_g$%g>*Tj z?AoD=j~E7fF9W)E>B^=nhfdG|UD~N$fE&D=Edv7%R{=_32Y=Fq%Pym(C%OZMb5UrjMf33aok`Ql}NU_8on23$JftCoZU`1}`5TuBR)yV5^#`ccKcF#mO zR(uLqeh7bsJD0w4%9SG*c-1rUCYBKyyK8EytGlXRSHImxqxSeq^}|Jl=uf)yc+g*A zq@O`Z`~}5CV?pPF#txl3Bu=D5(xqQ8>5_8j+NE=sq(`TLe}h#NIL7Nvq%!`A?o9`M zw%tB1lvfz(cMytrTSURO2vqEgSav(ORQ|$*R?1tdV?V^)ksi!Z%Fy zL=WZJM91>`zK00>_!z8zgOQFQt^{2Xg-{%;AhMtcMu&EI!lC&ml)3zUE$H=|XS75T zxr?waBP{gjc-En_EBIlJM?>rf#b-Gx(KV4kWv7AD)dE3-vhnAk9FC2DnZNofj#K07 zUJ@x&=}(eG^|LRGA2Dd=!$>7c_cCQM{8mrKTw358FIDkRnv2pU7F7r%hQE=14DIiO z&R{ZDov~Dxla73mWmXp&ZD5g%E^_W&MXZAow4?&fxX z-R@cY=ZG?E0&oFPOWj4u0`qcFt+m?;y6t6q;k6^kQdl%_Z zZklN$BICjbpbv(z)G7&FUAMF2G|~-d7Ry|0@kBWcu`mpG@R9OHBub(wT<1(Ip4>&t zdab}>(0Ev|cp3Kwac`Ukq8kXk3maY>d_5%2NvkPh?qf1_AV}OlnS6Q`j9vJ2Y_LzE z;B;^}z*@{dvXQx&n|xZB0gOB|+o7|fO&42-hwW7@?ZZRdV}GiLNv6x#EpQ5q^yKID zT&vcN*B-z#<^CX113-)I)rLnapH;@}2Lj47{uucey|n8f!J=nuvmn#gZ~hV7R{#*w zp}nO`^BYk(+SajNe+XPwTwah#<`q{s??HU!odmxnt|>Ujqjw(Z-z_8fGy=Wcu0keH z;kUN(n5hGf;GX|;w2ao`bj$P(r`r+5+WSFT4-K->5ww(-@au-%@9%tD(1sHJ%h8ga zEumsd`0F7-Y@QDZQaI*(S$z-a{cQN_c=k1&&Gw5%^}9EGrHq&xahX$VO>-9+H&%&l zRn1pHKVe&W+Qa?MQoX|_OPH2A`7#EjyrVrOBb8tKJzgV5#$!fV>vH`Flg}|y7EA#9 zq9OL2=b|nG{I;#Ut1k}TcmGB@?*d8#h9w(6C-TlcNK8+eunbA*^Jta;NurgEHT^A*Ew324{- E7X@^MO#lD@ literal 0 HcmV?d00001 diff --git a/__pycache__/utils.cpython-35.pyc b/__pycache__/utils.cpython-35.pyc new file mode 100644 index 0000000000000000000000000000000000000000..a274b4b964083de2edeec40f9fe7824619ae2345 GIT binary patch literal 320 zcmaJ+y$ZrG82r*!P@$lU;O5#vI}0L0H-|2|=q4s;rEN-+f(18y8DA-@lds_9rGhy5 zk-P8jE+GTo@8(mnsfp+Te8WMt08|geaTwi*wkRB01@4|0_ov49{9dQkGn~Fe>|5Vr3T`2cO*B_(L7+Ona6| LUZrB*xUA_7E8{)8 literal 0 HcmV?d00001 diff --git a/d_star_lite.py b/d_star_lite.py new file mode 100644 index 0000000..b82677b --- /dev/null +++ b/d_star_lite.py @@ -0,0 +1,152 @@ +import heapq +from utils import stateNameToCoords + + +def topKey(queue): + queue.sort() + # print(queue) + if len(queue) > 0: + return queue[0][:2] + else: + # print('empty queue!') + return (float('inf'), float('inf')) + + +def heuristic_from_s(graph, id, s): + x_distance = abs(int(id.split('x')[1][0]) - int(s.split('x')[1][0])) + y_distance = abs(int(id.split('y')[1][0]) - int(s.split('y')[1][0])) + return max(x_distance, y_distance) + + +def calculateKey(graph, id, s_current, k_m): + return (min(graph.graph[id].g, graph.graph[id].rhs) + heuristic_from_s(graph, id, s_current) + k_m, min(graph.graph[id].g, graph.graph[id].rhs)) + + +def updateVertex(graph, queue, id, s_current, k_m): + s_goal = graph.goal + if id != s_goal: + min_rhs = float('inf') + for i in graph.graph[id].children: + min_rhs = min( + min_rhs, graph.graph[i].g + graph.graph[id].children[i]) + graph.graph[id].rhs = min_rhs + id_in_queue = [item for item in queue if id in item] + if id_in_queue != []: + if len(id_in_queue) != 1: + raise ValueError('more than one ' + id + ' in the queue!') + queue.remove(id_in_queue[0]) + if graph.graph[id].rhs != graph.graph[id].g: + heapq.heappush(queue, calculateKey(graph, id, s_current, k_m) + (id,)) + + +def computeShortestPath(graph, queue, s_start, k_m): + while (graph.graph[s_start].rhs != graph.graph[s_start].g) or (topKey(queue) < calculateKey(graph, s_start, s_start, k_m)): + # print(graph.graph[s_start]) + # print('topKey') + # print(topKey(queue)) + # print('calculateKey') + # print(calculateKey(graph, s_start, 0)) + k_old = topKey(queue) + u = heapq.heappop(queue)[2] + if k_old < calculateKey(graph, u, s_start, k_m): + heapq.heappush(queue, calculateKey(graph, u, s_start, k_m) + (u,)) + elif graph.graph[u].g > graph.graph[u].rhs: + graph.graph[u].g = graph.graph[u].rhs + for i in graph.graph[u].parents: + updateVertex(graph, queue, i, s_start, k_m) + else: + graph.graph[u].g = float('inf') + updateVertex(graph, queue, u, s_start, k_m) + for i in graph.graph[u].parents: + updateVertex(graph, queue, i, s_start, k_m) + + +def nextInShortestPath(graph, s_current): + min_rhs = float('inf') + s_next = None + if graph.graph[s_current].rhs == float('inf'): + print('You are done stuck') + else: + for i in graph.graph[s_current].children: + # print(i) + child_cost = graph.graph[i].g + graph.graph[s_current].children[i] + # print(child_cost) + if (child_cost) < min_rhs: + min_rhs = child_cost + s_next = i + if s_next: + return s_next + else: + raise ValueError('could not find child for transition!') + + +def scanForObstacles(graph, queue, s_current, scan_range, k_m): + states_to_update = {} + range_checked = 0 + if scan_range >= 1: + for neighbor in graph.graph[s_current].children: + neighbor_coords = stateNameToCoords(neighbor) + states_to_update[neighbor] = graph.cells[neighbor_coords[1] + ][neighbor_coords[0]] + range_checked = 1 + # print(states_to_update) + + while range_checked < scan_range: + new_set = {} + for state in states_to_update: + new_set[state] = states_to_update[state] + for neighbor in graph.graph[state].children: + if neighbor not in new_set: + neighbor_coords = stateNameToCoords(neighbor) + new_set[neighbor] = graph.cells[neighbor_coords[1] + ][neighbor_coords[0]] + range_checked += 1 + states_to_update = new_set + + new_obstacle = False + for state in states_to_update: + if states_to_update[state] < 0: # found cell with obstacle + # print('found obstacle in ', state) + for neighbor in graph.graph[state].children: + # first time to observe this obstacle where one wasn't before + if(graph.graph[state].children[neighbor] != float('inf')): + neighbor_coords = stateNameToCoords(state) + graph.cells[neighbor_coords[1]][neighbor_coords[0]] = -2 + graph.graph[neighbor].children[state] = float('inf') + graph.graph[state].children[neighbor] = float('inf') + updateVertex(graph, queue, state, s_current, k_m) + new_obstacle = True + # elif states_to_update[state] == 0: #cell without obstacle + # for neighbor in graph.graph[state].children: + # if(graph.graph[state].children[neighbor] != float('inf')): + + # print(graph) + return new_obstacle + + +def moveAndRescan(graph, queue, s_current, scan_range, k_m): + if(s_current == graph.goal): + return 'goal', k_m + else: + s_last = s_current + s_new = nextInShortestPath(graph, s_current) + new_coords = stateNameToCoords(s_new) + + if(graph.cells[new_coords[1]][new_coords[0]] == -1): # just ran into new obstacle + s_new = s_current # need to hold tight and scan/replan first + + results = scanForObstacles(graph, queue, s_new, scan_range, k_m) + # print(graph) + k_m += heuristic_from_s(graph, s_last, s_new) + computeShortestPath(graph, queue, s_current, k_m) + + return s_new, k_m + + +def initDStarLite(graph, queue, s_start, s_goal, k_m): + graph.graph[s_goal].rhs = 0 + heapq.heappush(queue, calculateKey( + graph, s_goal, s_start, k_m) + (s_goal,)) + computeShortestPath(graph, queue, s_start, k_m) + + return (graph, queue, k_m) diff --git a/graph.py b/graph.py new file mode 100644 index 0000000..ea08be3 --- /dev/null +++ b/graph.py @@ -0,0 +1,100 @@ +class Node: + def __init__(self, id): + self.id = id + # dictionary of parent node ID's + # key = id of parent + # value = (edge cost,) + self.parents = {} + # dictionary of children node ID's + # key = id of child + # value = (edge cost,) + self.children = {} + # g approximation + self.g = float('inf') + # rhs value + self.rhs = float('inf') + + def __str__(self): + return 'Node: ' + self.id + ' g: ' + str(self.g) + ' rhs: ' + str(self.rhs) + + def __repr__(self): + return self.__str__() + + def udpate_parents(self, parents): + self.parents = parents + + +class Graph: + def __init__(self): + self.graph = {} + + def __str__(self): + msg = 'Graph:' + for i in self.graph: + msg += '\n node: ' + i + ' g: ' + \ + str(self.graph[i].g) + ' rhs: ' + str(self.graph[i].rhs) + return msg + + def __repr__(self): + return self.__str__() + + def setStart(self, id): + if(self.graph[id]): + self.start = id + else: + raise ValueError('start id not in graph') + + def setGoal(self, id): + if(self.graph[id]): + self.goal = id + else: + raise ValueError('goal id not in graph') + + +def addNodeToGraph(graph, id, neighbors, edge=1): + node = Node(id) + for i in neighbors: + # print(i) + node.parents[i] = edge + node.children[i] = edge + graph[id] = node + return graph + + +def makeGraph(): + graph = {} + + # 8-connected graph (w diagonals) + # Impossible to find path - 2 obstacles in middle + # graph = addNodeToGraph(graph, 'x1y1', ['x1y2', 'x2y1', 'x2y2']) + # graph = addNodeToGraph( + # graph, 'x2y1', ['x1y1', 'x1y2', 'x3y1', 'x2y2', 'x3y2'], float('inf')) + # graph = addNodeToGraph(graph, 'x1y2', ['x1y1', 'x2y1', 'x2y2']) + # graph = addNodeToGraph( + # graph, 'x2y2', ['x1y1', 'x1y2', 'x3y1', 'x2y1', 'x3y2'], float('inf')) + # graph = addNodeToGraph(graph, 'x3y1', ['x3y2', 'x2y1', 'x2y2']) + # graph = addNodeToGraph(graph, 'x3y2', ['x3y1', 'x2y1', 'x2y2']) + + # 8-connected graph (w diagonals) + # Impossible to find path - 2 obstacles in middle + # graph = addNodeToGraph(graph, 'x1y1', ['x1y2', 'x2y1', 'x2y2']) + # graph = addNodeToGraph( + # graph, 'x2y1', ['x1y1', 'x1y2', 'x3y1', 'x2y2', 'x3y2'], float('inf')) + # graph = addNodeToGraph(graph, 'x1y2', ['x1y1', 'x2y1', 'x2y2']) + # graph = addNodeToGraph( + # graph, 'x2y2', ['x1y1', 'x1y2', 'x3y1', 'x2y1', 'x3y2']) + # graph = addNodeToGraph(graph, 'x3y1', ['x3y2', 'x2y1', 'x2y2']) + # graph = addNodeToGraph(graph, 'x3y2', ['x3y1', 'x2y1', 'x2y2']) + + # 4-connected graph (w/out diagonals) + graph = addNodeToGraph(graph, 'x1y1', ['x1y2', 'x2y1']) + graph = addNodeToGraph(graph, 'x2y1', ['x1y1', 'x3y1', 'x2y2']) + graph = addNodeToGraph(graph, 'x1y2', ['x1y1', 'x2y2']) + graph = addNodeToGraph(graph, 'x2y2', ['x1y2', 'x2y1', 'x3y2']) + graph = addNodeToGraph(graph, 'x3y1', ['x3y2', 'x2y1']) + graph = addNodeToGraph(graph, 'x3y2', ['x3y1', 'x2y2']) + + g = GridWorld(X_DIM, Y_DIM) + # g.graph = graph + # print(g) + return g diff --git a/grid.py b/grid.py new file mode 100644 index 0000000..0a3c2a8 --- /dev/null +++ b/grid.py @@ -0,0 +1,55 @@ +from graph import Node, Graph + + +class GridWorld(Graph): + def __init__(self, x_dim, y_dim, connect8=True): + self.x_dim = x_dim + self.y_dim = y_dim + # First make an element for each row (height of grid) + self.cells = [0] * y_dim + # Go through each element and replace with row (width of grid) + for i in range(y_dim): + self.cells[i] = [0] * x_dim + # will this be an 8-connected graph or 4-connected? + self.connect8 = connect8 + self.graph = {} + + self.generateGraphFromGrid() + # self.printGrid() + + def __str__(self): + msg = 'Graph:' + for i in self.graph: + msg += '\n node: ' + i + ' g: ' + \ + str(self.graph[i].g) + ' rhs: ' + str(self.graph[i].rhs) + \ + ' neighbors: ' + str(self.graph[i].children) + return msg + + def __repr__(self): + return self.__str__() + + def printGrid(self): + print('** GridWorld **') + for row in self.cells: + print(row) + + def generateGraphFromGrid(self): + edge = 1 + for i in range(len(self.cells)): + row = self.cells[i] + for j in range(len(row)): + # print('graph node ' + str(i) + ',' + str(j)) + node = Node('x' + str(i) + 'y' + str(j)) + if i > 0: # not top row + node.parents['x' + str(i - 1) + 'y' + str(j)] = edge + node.children['x' + str(i - 1) + 'y' + str(j)] = edge + if i + 1 < self.y_dim: # not bottom row + node.parents['x' + str(i + 1) + 'y' + str(j)] = edge + node.children['x' + str(i + 1) + 'y' + str(j)] = edge + if j > 0: # not left col + node.parents['x' + str(i) + 'y' + str(j - 1)] = edge + node.children['x' + str(i) + 'y' + str(j - 1)] = edge + if j + 1 < self.x_dim: # not right col + node.parents['x' + str(i) + 'y' + str(j + 1)] = edge + node.children['x' + str(i) + 'y' + str(j + 1)] = edge + self.graph['x' + str(i) + 'y' + str(j)] = node diff --git a/main.py b/main.py new file mode 100644 index 0000000..ccbe834 --- /dev/null +++ b/main.py @@ -0,0 +1,162 @@ +import heapq +import pygame + +from graph import Node, Graph +from grid import GridWorld +from utils import stateNameToCoords +from d_star_lite import initDStarLite, moveAndRescan + +# Define some colors +BLACK = (0, 0, 0) +WHITE = (255, 255, 255) +GREEN = (0, 255, 0) +RED = (255, 0, 0) +GRAY1 = (145, 145, 102) +GRAY2 = (77, 77, 51) +BLUE = (0, 0, 80) + +colors = { + 0: WHITE, + 1: GREEN, + -1: GRAY1, + -2: GRAY2 +} + +# This sets the WIDTH and HEIGHT of each grid location +WIDTH = 40 +HEIGHT = 40 + +# This sets the margin between each cell +MARGIN = 5 + +# Create a 2 dimensional array. A two dimensional +# array is simply a list of lists. +grid = [] +for row in range(10): + # Add an empty array that will hold each cell + # in this row + grid.append([]) + for column in range(10): + grid[row].append(0) # Append a cell + +# Set row 1, cell 5 to one. (Remember rows and +# column numbers start at zero.) +grid[1][5] = 1 + +# Initialize pygame +pygame.init() + +X_DIM = 12 +Y_DIM = 12 +VIEWING_RANGE = 3 + + +# Set the HEIGHT and WIDTH of the screen +WINDOW_SIZE = [(WIDTH + MARGIN) * X_DIM + MARGIN, + (HEIGHT + MARGIN) * Y_DIM + MARGIN] +screen = pygame.display.set_mode(WINDOW_SIZE) + +# Set title of screen +pygame.display.set_caption("D* Lite Path Planning") + +# Loop until the user clicks the close button. +done = False + +# Used to manage how fast the screen updates +clock = pygame.time.Clock() + +if __name__ == "__main__": + graph = GridWorld(X_DIM, Y_DIM) + s_start = 'x1y2' + s_goal = 'x5y4' + goal_coords = stateNameToCoords(s_goal) + + graph.setStart(s_start) + graph.setGoal(s_goal) + k_m = 0 + s_last = s_start + queue = [] + + graph, queue, k_m = initDStarLite(graph, queue, s_start, s_goal, k_m) + + s_current = s_start + pos_coords = stateNameToCoords(s_current) + + basicfont = pygame.font.SysFont('Comic Sans MS', 36) + + # -------- Main Program Loop ----------- + while not done: + for event in pygame.event.get(): # User did something + if event.type == pygame.QUIT: # If user clicked close + done = True # Flag that we are done so we exit this loop + elif event.type == pygame.KEYDOWN and event.key == pygame.K_SPACE: + # print('space bar! call next action') + s_new, k_m = moveAndRescan( + graph, queue, s_current, VIEWING_RANGE, k_m) + if s_new == 'goal': + print('Goal Reached!') + done = True + else: + # print('setting s_current to ', s_new) + s_current = s_new + pos_coords = stateNameToCoords(s_current) + # print('got pos coords: ', pos_coords) + + elif event.type == pygame.MOUSEBUTTONDOWN: + # User clicks the mouse. Get the position + pos = pygame.mouse.get_pos() + # Change the x/y screen coordinates to grid coordinates + column = pos[0] // (WIDTH + MARGIN) + row = pos[1] // (HEIGHT + MARGIN) + # Set that location to one + if(graph.cells[row][column] == 0): + graph.cells[row][column] = -1 + + # Set the screen background + screen.fill(BLACK) + + # Draw the grid + for row in range(Y_DIM): + for column in range(X_DIM): + color = WHITE + # if grid[row][column] == 1: + # color = GREEN + pygame.draw.rect(screen, colors[graph.cells[row][column]], + [(MARGIN + WIDTH) * column + MARGIN, + (MARGIN + HEIGHT) * row + MARGIN, WIDTH, HEIGHT]) + node_name = 'x' + str(column) + 'y' + str(row) + if(graph.graph[node_name].g != float('inf')): + # text = basicfont.render( + # str(graph.graph[node_name].g), True, (0, 0, 200), (255, + # 255, 255)) + text = basicfont.render( + str(graph.graph[node_name].g), True, (0, 0, 200)) + textrect = text.get_rect() + textrect.centerx = int( + column * (WIDTH + MARGIN) + WIDTH / 2) + MARGIN + textrect.centery = int( + row * (HEIGHT + MARGIN) + HEIGHT / 2) + MARGIN + screen.blit(text, textrect) + + # fill in goal cell with GREEN + pygame.draw.rect(screen, GREEN, [(MARGIN + WIDTH) * goal_coords[0] + MARGIN, + (MARGIN + HEIGHT) * goal_coords[1] + MARGIN, WIDTH, HEIGHT]) + # print('drawing robot pos_coords: ', pos_coords) + # draw moving robot, based on pos_coords + robot_center = [int(pos_coords[0] * (WIDTH + MARGIN) + WIDTH / 2) + + MARGIN, int(pos_coords[1] * (HEIGHT + MARGIN) + HEIGHT / 2) + MARGIN] + pygame.draw.circle(screen, RED, robot_center, int(WIDTH / 2) - 2) + + # draw robot viewing range + pygame.draw.rect( + screen, BLUE, [robot_center[0] - VIEWING_RANGE * (WIDTH + MARGIN), robot_center[1] - VIEWING_RANGE * (HEIGHT + MARGIN), 2 * VIEWING_RANGE * (WIDTH + MARGIN), 2 * VIEWING_RANGE * (HEIGHT + MARGIN)], 2) + + # Limit to 60 frames per second + clock.tick(20) + + # Go ahead and update the screen with what we've drawn. + pygame.display.flip() + + # Be IDLE friendly. If you forget this line, the program will 'hang' + # on exit. + pygame.quit() diff --git a/utils.py b/utils.py new file mode 100644 index 0000000..d41cc87 --- /dev/null +++ b/utils.py @@ -0,0 +1,4 @@ + + +def stateNameToCoords(name): + return [int(name.split('x')[1].split('y')[0]), int(name.split('x')[1].split('y')[1])]