├── Algorithm$py.class ├── Algorithm.py ├── Grid$py.class ├── Grid.py ├── MazeGenerator$py.class ├── MazeGenerator.py ├── PathPlanning.pyde ├── README.md ├── robot$py.class ├── robot.py └── sketch.properties /Algorithm$py.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/anindex/Processing-path_planning/6cc752cd5b979fa8672f287a1f351f3669a05c0c/Algorithm$py.class -------------------------------------------------------------------------------- /Algorithm.py: -------------------------------------------------------------------------------- 1 | ####################################################################################################### 2 | # Author: An T. Le, DOB: 11/11/1997 3 | # Organization: Vietnamese-German University 4 | # Date Updated: March 21th, 2017 5 | # Date Created: July 2nd, 2016 6 | # Application: Occupancy Grid Library and Search-based Path Planning Library 7 | ####################################################################################################### 8 | 9 | from Queue import PriorityQueue 10 | from Grid import PriorityDict 11 | from time import sleep 12 | import copy 13 | 14 | ########################################################## 15 | # A* Search-based algorithm (from start) # 16 | ########################################################## 17 | 18 | def astar_search(grid, startP, goal): 19 | frontier = PriorityQueue() 20 | came_from = {} 21 | cost_so_far = {} 22 | 23 | cost_so_far[startP] = 0 24 | came_from[startP] = startP 25 | frontier.put((heuristic_distance(startP, goal) + cost_so_far[startP], startP)) 26 | 27 | current = startP 28 | while not frontier.empty() and current != goal: 29 | current = frontier.get()[1] 30 | 31 | for next in grid.neighbor(current): 32 | new_cost = cost_so_far[current] + grid.cost(current, next) 33 | if next not in came_from or new_cost < cost_so_far[next]: 34 | came_from[next] = current 35 | cost_so_far[next] = new_cost 36 | frontier.put((heuristic_distance(next, goal) + cost_so_far[next], next)) 37 | 38 | return came_from, cost_so_far if current == goal else None 39 | 40 | ########################################################### 41 | # D* Lite Search-based algorithm (from goal) # 42 | ########################################################### 43 | 44 | #Note: data[node][0] -> g() value 45 | # data[node][1] -> rhs() value 46 | 47 | def calculate_key(node, data, robotPos, km): 48 | return min(data[node][0], data[node][1]) + heuristic_distance(robotPos, node) + km 49 | 50 | def update_vertex(node, robot, goal, frontier, data, km): 51 | if node != goal: data[node][1] = get_lowest_cost_node(robot, node, data)[1] 52 | if node in frontier: del frontier[node] 53 | if data[node][0] != data[node][1]: frontier[node] = calculate_key(node, data, robot.pos, km) 54 | 55 | def get_lowest_cost_node(robot, node, data): 56 | neighbors = {k: (data[k][0]+robot.cost(node, k)) for k in robot.detect_neighbor(node)} 57 | result = min(neighbors, key=neighbors.get) 58 | return result, neighbors[result] 59 | 60 | def get_lowest_rhs_node(robot, node, data): 61 | neighbors = {k: (data[k][1]+robot.cost(node, k)) for k in robot.detect_neighbor(node)} 62 | result = min(neighbors, key=neighbors.get) 63 | return result, neighbors[result] 64 | 65 | def compute_shortest_path(robot, goal, data, frontier, km, closedNode, Pnode): 66 | while frontier and (frontier.top()[1] < (calculate_key(robot.pos, data, robot.pos, km) + 0.1) or data[robot.pos][1] != data[robot.pos][0]): 67 | currNode, kold = frontier.pop() 68 | knew = calculate_key(currNode, data, robot.pos, km) 69 | if kold < knew: 70 | frontier[currNode] = knew 71 | elif data[currNode][0] > data[currNode][1]: 72 | data[currNode][0] = data[currNode][1] 73 | for pred in robot.detect_neighbor(currNode): 74 | update_vertex(pred, robot, goal, frontier, data, km) 75 | else: 76 | data[currNode][0] = float("inf") 77 | for node in robot.detect_neighbor(currNode) + [currNode, ]: 78 | update_vertex(node, robot, goal, frontier, data, km) 79 | closedNode.append(currNode) 80 | Pnode += 1 81 | #sleep(0.1) 82 | return Pnode 83 | 84 | 85 | def dlite_search(grid, robot, startP, goal, path, frontier, closedNode, param_rtl = 0.5, param_lh = 2.28): 86 | 87 | ######### Initialization ############### 88 | km = 0 89 | data = {k: [float("inf"), float("inf")] for k in [(i, j) for i in range(grid.ix) for j in range(grid.iy)]} 90 | overallPath = [] 91 | Pnode = 0 92 | Tnode = 0 93 | 94 | data[goal][1] = 0 95 | frontier[goal] = heuristic_distance(startP, goal) 96 | lastNode = startP 97 | robot.pos = startP 98 | robot.knownWorld = grid.walls[:] 99 | #robot.knownWorld = [] 100 | ############## Main #################### 101 | Pnode = compute_shortest_path(robot, goal, data, frontier, km, closedNode, Pnode) 102 | modify_path(path, reconstruct_path_gradient(robot, data, goal)) 103 | 104 | ################# Linear Heuristic Estimator ###################### 105 | last_heuristic = heuristic_distance(startP, goal) 106 | estimated_LH = len(path) / heuristic_distance(startP, goal) 107 | print "Estimated LH: {0:0.2f}, Heuristic: {1:0.2f}, Path: {2}".format(estimated_LH, heuristic_distance(startP, goal), len(path)) 108 | param_lh = estimated_LH 109 | 110 | ################# Analytic Test Estimator ######################### 111 | 112 | 113 | 114 | 115 | while robot.pos != goal: 116 | robot.pos = get_lowest_cost_node(robot, robot.pos, data)[0] 117 | Tnode += 1 118 | overallPath.append(robot.pos) 119 | changedNode = robot.detect_changes(grid) 120 | if changedNode[0] or changedNode[1]: 121 | del closedNode[:] #reset the display node 122 | current_heuristic = heuristic_distance(robot.pos, goal) 123 | print "Pnode: {0}, Tnode: {1}, Path: {2}, Remain Node: {3}, Ratio: {4:0.2f}, Lheuristic: {5:0.2f}, LHRatio: {6:0.2f}".format(Pnode, Tnode, len(path), len(path) - Tnode, float(Tnode) / len(path), param_lh * current_heuristic, current_heuristic / last_heuristic) # Data Pnode and Tnode at the time robot detects changes 124 | #if float(Tnode) / len(path) > param_rtl: # -------------------- Ratio of traversed length 125 | if Tnode > 3 * param_lh * current_heuristic: # -------------- Linear Heuristic criteria, added ratio of heuristic 126 | 127 | dlite_pnode(changedNode, data, robot, goal, frontier, closedNode, lastNode, km) 128 | 129 | ####################### Reset Rountine ############################### 130 | km = 0 131 | frontier.clear() 132 | Pnode = 0 133 | Tnode = 0 134 | data = {k: [float("inf"), float("inf")] for k in [(i, j) for i in range(grid.ix) for j in range(grid.iy)]} 135 | data[goal][1] = 0 136 | frontier[goal] = heuristic_distance(robot.pos, goal) 137 | lastNode = robot.pos 138 | for node in changedNode[0] + changedNode[1]: 139 | robot.update_cell(node) 140 | Pnode = compute_shortest_path(robot, goal, data, frontier, km, closedNode, Pnode) 141 | print "Pnode reset: {0}, true".format(Pnode) 142 | 143 | 144 | else: 145 | 146 | reset_pnode(changedNode, grid, robot, goal) 147 | 148 | ###################### D*Lite Routine ############################### 149 | km += heuristic_distance(lastNode, robot.pos) 150 | lastNode = robot.pos 151 | for node in changedNode[0] + changedNode[1]: 152 | robot.update_cell(node) # node -> v, next -> u 153 | for next in robot.detect_neighbor(node): 154 | update_vertex(next, robot, goal, frontier, data, km) 155 | update_vertex(node, robot, goal, frontier, data, km) 156 | temp = compute_shortest_path(robot, goal, data, frontier, km, closedNode, 0) 157 | print "Pnode D*Lite: {0}, false".format(temp) 158 | 159 | modify_path(path, reconstruct_path_gradient(robot, data, goal)) 160 | sleep(0.3) 161 | 162 | ########################################################### 163 | # Utility # 164 | ########################################################### 165 | 166 | def modify_path(path, newPath): # update global variable "path" to new path 167 | del path[:] 168 | path += newPath 169 | 170 | def heuristic_distance(startP, goal): # calculate square root heuristic distance on grid 171 | return sqrt((startP[0] - goal[0])**2 + (startP[1] - goal[1])**2) 172 | 173 | def heuristic_manhattan(startP, goal): # calculate manhattan heuristic distance on grid 174 | return abs(startP[0] - goal[0]) + abs(startP[1] - goal[1]) 175 | 176 | def reconstruct_path_gradient(robot, data, goal): # output the path from cost matrix 177 | current = robot.pos 178 | path = [current] 179 | if data[current][1] == float("inf"): 180 | return None 181 | while current != goal: 182 | current = get_lowest_cost_node(robot, current, data)[0] 183 | path.append(current) 184 | return path 185 | 186 | def reset_pnode(changedNode, grid, robot, goal): 187 | km_copy = 0 188 | frontier_copy = PriorityDict() 189 | robot_copy = copy.deepcopy(robot) 190 | closedNode_copy = [] 191 | data_copy = {k: [float("inf"), float("inf")] for k in [(i, j) for i in range(grid.ix) for j in range(grid.iy)]} 192 | 193 | data_copy[goal][1] = 0 194 | frontier_copy[goal] = heuristic_distance(robot_copy.pos, goal) 195 | 196 | for node in changedNode[0] + changedNode[1]: 197 | robot_copy.update_cell(node) 198 | temp = compute_shortest_path(robot_copy, goal, data_copy, frontier_copy, km_copy, closedNode_copy, 0) 199 | print "Pnode reset: {0}".format(temp) 200 | return temp 201 | 202 | def dlite_pnode(changedNode, data, robot, goal, frontier, closedNode, lastNode, km): 203 | data_copy = copy.deepcopy(data) 204 | robot_copy = copy.deepcopy(robot) 205 | frontier_copy = copy.deepcopy(frontier) 206 | closedNode_copy = [] 207 | km_copy = copy.deepcopy(km) 208 | 209 | km_copy += heuristic_distance(lastNode, robot_copy.pos) #lastNode do not rename 210 | for node in changedNode[0] + changedNode[1]: 211 | robot_copy.update_cell(node) # node -> v, next -> u 212 | for next in robot_copy.detect_neighbor(node): 213 | update_vertex(next, robot_copy, goal, frontier_copy, data_copy, km_copy) 214 | update_vertex(node, robot_copy, goal, frontier_copy, data_copy, km_copy) 215 | temp = compute_shortest_path(robot_copy, goal, data_copy, frontier_copy, km_copy, closedNode_copy, 0) 216 | print "Pnode D*Lite: {0}".format(temp) 217 | return temp 218 | 219 | 220 | def reconstruct_path(came_from, startP, goal): # output the path from search tree 221 | if came_from != None: 222 | path = [] 223 | current = goal 224 | path.append(current) 225 | while current != startP: 226 | current = came_from[current] 227 | path.append(current) 228 | path.reverse() 229 | return path 230 | else: 231 | return None -------------------------------------------------------------------------------- /Grid$py.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/anindex/Processing-path_planning/6cc752cd5b979fa8672f287a1f351f3669a05c0c/Grid$py.class -------------------------------------------------------------------------------- /Grid.py: -------------------------------------------------------------------------------- 1 | ####################################################################################################### 2 | # Author: An T. Le, DOB: 11/11/1997 3 | # Organization: Vietnamese-German University 4 | # Date Updated: March 21th, 2017 5 | # Date Created: July 2nd, 2016 6 | # Application: Occupancy Grid Library and Search-based Path Planning Library 7 | ####################################################################################################### 8 | 9 | from heapq import heappush, heappop, heapify 10 | 11 | ################################## PriorityDict Wrapper of dict data type ########################################################### 12 | class PriorityDict(dict): 13 | def __init__(self, *args, **kwargs): 14 | super(PriorityDict, self).__init__(self, *args, **kwargs) 15 | self._rebuild_heap() 16 | 17 | def _rebuild_heap(self): 18 | self._heap = [(v, k) for k, v in self.iteritems()] 19 | heapify(self._heap) 20 | 21 | def pop(self): 22 | v, k = heappop(self._heap) 23 | while k not in self or self[k] != v: 24 | v, k = heappop(self._heap) 25 | del self[k] 26 | return k, v 27 | 28 | def top(self): 29 | heap = self._heap 30 | v, k = heap[0] 31 | while k not in self or self[k] != v: 32 | heappop(heap) 33 | v, k = heap[0] 34 | return k, v 35 | 36 | def __setitem__(self, k, v): 37 | super(PriorityDict, self).__setitem__(k, v) 38 | 39 | if len(self._heap) < 2 * len(self): 40 | heappush(self._heap, (v, k)) 41 | else: 42 | self._rebuild_heap() 43 | 44 | ############################# Define Resolution Exception ############################################# 45 | class InvalidResolution(RuntimeError): 46 | def __init__(self, arg): 47 | self.arg = arg 48 | 49 | ############################ Define draw cell function ################################################ 50 | def draw_cell(x, y, res, attr): 51 | stroke(0) 52 | fill(attr[0], attr[1], attr[2]) 53 | rect(x*res, y*res, res, res) 54 | 55 | def draw_cell_value(x, y, res, value): 56 | stroke(0) 57 | fill(value) 58 | rect(x*res, y*res, res, res) 59 | 60 | ########################## Define draw arrow function (for search tree simulation) #################### 61 | def draw_arrow(x, y, res, dir): 62 | if dir == 'u': 63 | x1 = x*res + res / 2 64 | y1 = y*res + 3*res/4 65 | x2 = x*res + res / 2 66 | y2 = y*res + res /4 67 | line(x1, y1, x2, y2) 68 | pushMatrix() 69 | translate(x2, y2) 70 | a = atan2(x1-x2, y2-y1) 71 | rotate(a) 72 | line(0, 0, -2, -2) 73 | line(0, 0, 2, -2) 74 | popMatrix() 75 | elif dir == 'd': 76 | x1, y1 = x*res + res / 2, y*res + res / 4 77 | x2, y2 = x*res + res / 2, y*res + 3 * res /4 78 | line(x1, y1, x2, y2) 79 | pushMatrix() 80 | translate(x2, y2) 81 | a = atan2(x1-x2, y2-y1) 82 | rotate(a) 83 | line(0, 0, -2, -2) 84 | line(0, 0, 2, -2) 85 | popMatrix() 86 | elif dir == 'l': 87 | x1, y1 = x*res + 3 * res / 4, y*res + res/ 2 88 | x2, y2 = x*res + res / 4, y*res + res /2 89 | line(x1, y1, x2, y2) 90 | pushMatrix() 91 | translate(x2, y2) 92 | a = atan2(x1-x2, y2-y1) 93 | rotate(a) 94 | line(0, 0, -2, -2) 95 | line(0, 0, 2, -2) 96 | popMatrix() 97 | elif dir == 'r': 98 | x1, y1 = x*res + res / 4, y*res + res/ 2 99 | x2, y2 = x*res + 3 * res / 4, y*res + res /2 100 | line(x1, y1, x2, y2) 101 | pushMatrix() 102 | translate(x2, y2) 103 | a = atan2(x1-x2, y2-y1) 104 | rotate(a) 105 | line(0, 0, -2, -2) 106 | line(0, 0, 2, -2) 107 | popMatrix() 108 | elif dir == 'q': 109 | x1 = x*res + 3 * res / 4 110 | y1 = y*res + 3*res/4 111 | x2 = x*res + res / 4 112 | y2 = y*res + res /4 113 | line(x1, y1, x2, y2) 114 | pushMatrix() 115 | translate(x2, y2) 116 | a = atan2(x1-x2, y2-y1) 117 | rotate(a) 118 | line(0, 0, -2, -2) 119 | line(0, 0, 2, -2) 120 | popMatrix() 121 | elif dir == 'e': 122 | x1, y1 = x*res + res / 4, y*res + 3 * res / 4 123 | x2, y2 = x*res + 3 * res / 4, y*res + res /4 124 | line(x1, y1, x2, y2) 125 | pushMatrix() 126 | translate(x2, y2) 127 | a = atan2(x1-x2, y2-y1) 128 | rotate(a) 129 | line(0, 0, -2, -2) 130 | line(0, 0, 2, -2) 131 | popMatrix() 132 | elif dir == 'z': 133 | x1, y1 = x*res + 3 * res / 4, y*res + res/ 4 134 | x2, y2 = x*res + res / 4, y*res + 3* res /4 135 | line(x1, y1, x2, y2) 136 | pushMatrix() 137 | translate(x2, y2) 138 | a = atan2(x1-x2, y2-y1) 139 | rotate(a) 140 | line(0, 0, -2, -2) 141 | line(0, 0, 2, -2) 142 | popMatrix() 143 | elif dir == 'c': 144 | x1, y1 = x*res + res / 4, y*res + res/ 4 145 | x2, y2 = x*res + 3 * res / 4, y*res + 3 * res /4 146 | line(x1, y1, x2, y2) 147 | pushMatrix() 148 | translate(x2, y2) 149 | a = atan2(x1-x2, y2-y1) 150 | rotate(a) 151 | line(0, 0, -2, -2) 152 | line(0, 0, 2, -2) 153 | popMatrix() 154 | 155 | #################################### Occupancy Grid Environment ################################################## 156 | class SquareGrid: 157 | def __init__(self, w, h, resolution): 158 | self.resolution = resolution 159 | self.f = createFont("Arial", resolution / 2, True) 160 | self.walls = [] # map's obstacles 161 | self.knownWorld = [] 162 | self.roughs = [] # rough terran means higher cost to traverse (experimential) 163 | if(w % resolution == 0 and h % resolution == 0): # check if resolution is valid 164 | self.ix = w / resolution 165 | self.iy = h / resolution 166 | else: 167 | raise InvalidResolution("Bad Initialization") 168 | 169 | def in_bounds(self, id): # check if the range has reached outside map 170 | return 0 <= id[0] and id[0] < self.ix and 0 <= id[1] and id[1] < self.iy 171 | 172 | def passable(self, id): # check the robot can go to a position or not 173 | return id not in self.walls 174 | 175 | def neighbor(self, id, excluded = True): # get the neighbor cells from current cell (optionally exclude the current cell) 176 | x, y = id 177 | result = [(x + i, y + j) for i in range(-1, 2) for j in range(-1, 2)] 178 | if excluded: 179 | result.remove(id) 180 | result = filter(self.in_bounds, result) 181 | result = filter(self.passable, result) 182 | if (x+y) % 2 == 0: result.reverse() # for good looking scanning pattern 183 | return result 184 | 185 | def add_walls(self, lp, rp): # utility to add an obstacle at an specific position 186 | for i in range(lp[0], rp[0]+1): 187 | for j in range(lp[1], rp[1]+1): 188 | self.walls.append((i, j)) 189 | 190 | def cost(self, currP, nextP): # get cost to traverse from current to next position 191 | val = 0 192 | if currP in self.knownWorld or nextP in self.knownWorld: # NEED TO CONSIDER 193 | return float("inf") 194 | if abs(nextP[1] - currP[1]) + abs(nextP[0] - currP[0]) == 1: 195 | val += 1 196 | elif abs(nextP[1] - currP[1]) + abs(nextP[0] - currP[0]) == 2: 197 | val += 1.414 198 | if nextP in self.roughs: 199 | val += 4 200 | return val 201 | 202 | def draw_grid(self, startP, goal, bot, distances = None, came_from = None, frontier = None, path = None, closedNode = None): # draw the map routine 203 | for row in range(self.iy): 204 | for col in range(self.ix): 205 | current = (col, row) 206 | if current in self.walls: 207 | draw_cell(col, row, self.resolution, (0, 0, 0)) 208 | elif current == bot.pos: 209 | draw_cell(col, row, self.resolution, (0, 0, 255)) 210 | elif path != None and current in path: 211 | draw_cell(col, row, self.resolution, (255, 0, 0)) 212 | elif closedNode != None and current in closedNode: 213 | num = closedNode.count(current) 214 | draw_cell_value(col, row, self.resolution, 255 - 60*num) 215 | elif frontier != None and current in frontier: 216 | draw_cell(col, row, self.resolution, (255, 255, 0)) 217 | elif distances != None and current in distances: 218 | draw_cell(col, row, self.resolution, (255, 255, 255)) 219 | text(self.f, distances[current], col*self.resolution + self.resolution / 2, row*self.resolution + self.resolution/ 2) 220 | elif came_from != None and current in came_from: 221 | draw_cell(col, row, self.resolution, (255, 255, 255)) 222 | x, y = current 223 | px, py = came_from[current] 224 | if px == x - 1 and py == y: 225 | draw_arrow(col, row, self.resolution, 'l') 226 | elif px == x + 1 and py == y: 227 | draw_arrow(col, row, self.resolution, 'r') 228 | elif py == y - 1 and px == x: 229 | draw_arrow(col, row, self.resolution, 'u') 230 | elif py == y + 1 and px == x: 231 | draw_arrow(col, row, self.resolution, 'd') 232 | elif px == x - 1 and py == y - 1: 233 | draw_arrow(col, row, self.resolution, 'q') 234 | elif px == x + 1 and py == y + 1: 235 | draw_arrow(col, row, self.resolution, 'c') 236 | elif py == y - 1 and px == x + 1: 237 | draw_arrow(col, row, self.resolution, 'e') 238 | elif py == y + 1 and px == x - 1: 239 | draw_arrow(col, row, self.resolution, 'z') 240 | elif current == startP or current == goal: 241 | draw_cell(col, row, self.resolution, (0, 255, 0)) 242 | else: 243 | draw_cell(col, row, self.resolution, (255, 255, 255)) 244 | 245 | 246 | ######################################## Custom Maps ########################################################## 247 | def map1(res): 248 | grid = SquareGrid(width, height, res) 249 | grid.add_walls((8, 20), (14, 20)) 250 | grid.add_walls((8, 18), (12, 18)) 251 | grid.walls.append((8, 19)) 252 | grid.add_walls((12, 12), (12, 17)) 253 | grid.add_walls((14, 18), (14, 19)) 254 | grid.add_walls((14, 12), (14, 14)) 255 | grid.add_walls((8, 20), (14, 20)) 256 | grid.add_walls((14, 15), (17, 15)) 257 | grid.add_walls((14, 17), (17, 17)) 258 | return grid 259 | 260 | def maze1(res): 261 | grid = SquareGrid(width, height, res) 262 | grid.add_walls((0,0), (0, 19)) 263 | grid.add_walls((18,0), (18, 19)) 264 | grid.add_walls((1,19), (17, 19)) 265 | grid.add_walls((3,0), (17, 0)) 266 | grid.walls.append((1, 0)) 267 | grid.walls.append((2, 2)) 268 | grid.walls.append((3, 2)) 269 | grid.walls.append((3, 1)) 270 | grid.walls.append((7, 1)) 271 | grid.walls.append((7, 2)) 272 | grid.walls.append((9, 2)) 273 | grid.add_walls((10,2), (10, 5)) 274 | grid.add_walls((12,1), (12, 4)) 275 | grid.add_walls((14,2), (16, 2)) 276 | grid.add_walls((1,4), (8, 4)) 277 | grid.walls.append((8, 5)) 278 | grid.add_walls((8,6), (13, 6)) 279 | grid.add_walls((14,3), (14, 8)) 280 | grid.add_walls((16,4), (17, 4)) 281 | grid.add_walls((16,6), (16, 8)) 282 | grid.walls.append((15, 8)) 283 | grid.walls.append((6, 7)) 284 | grid.add_walls((2,6), (6, 6)) 285 | grid.add_walls((2,7), (2, 8)) 286 | grid.add_walls((3,8), (4, 8)) 287 | grid.add_walls((4,9), (8, 9)) 288 | grid.add_walls((8,8), (12, 8)) 289 | grid.walls.append((10, 9)) 290 | grid.walls.append((2, 10)) 291 | grid.add_walls((2,11), (5, 11)) 292 | grid.add_walls((7,10), (7, 13)) 293 | grid.walls.append((6, 13)) 294 | grid.add_walls((8,11), (12, 11)) 295 | grid.add_walls((12,10), (16, 10)) 296 | grid.walls.append((17, 13)) 297 | grid.add_walls((1,13), (2, 13)) 298 | grid.add_walls((4,12), (4, 14)) 299 | grid.add_walls((2,15), (2, 19)) 300 | grid.add_walls((3,15), (9, 15)) 301 | grid.add_walls((9,13), (9, 14)) 302 | grid.add_walls((4,17), (11, 17)) 303 | grid.add_walls((12,15), (12, 17)) 304 | grid.add_walls((11,12), (11, 15)) 305 | grid.add_walls((14,12), (14, 17)) 306 | grid.add_walls((15,15), (16, 15)) 307 | grid.add_walls((15,17), (17, 17)) 308 | grid.add_walls((15,17), (17, 17)) 309 | grid.add_walls((16,11), (16, 13)) 310 | grid.walls.append((13, 13)) 311 | grid.walls.remove((18, 15)) 312 | return grid -------------------------------------------------------------------------------- /MazeGenerator$py.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/anindex/Processing-path_planning/6cc752cd5b979fa8672f287a1f351f3669a05c0c/MazeGenerator$py.class -------------------------------------------------------------------------------- /MazeGenerator.py: -------------------------------------------------------------------------------- 1 | ####################################################################################################### 2 | # Author: An T. Le, DOB: 11/11/1997 3 | # Organization: Vietnamese-German University 4 | # Date Updated: March 21th, 2017 5 | # Date Created: July 2nd, 2016 6 | # Application: Occupancy Grid Library and Search-based Path Planning Library 7 | ####################################################################################################### 8 | 9 | from random import randint 10 | from Grid import SquareGrid 11 | from time import sleep 12 | 13 | ############################################# Maze Generator Engine using Recursive Backtracking algorithm ########################################## 14 | class MazeGenerator: 15 | def __init__(self, w, h, res, initial_cell): 16 | self.grid = SquareGrid(w, h, res) # initialize map 17 | self.grid.walls = [(x, y) for x in range(self.grid.ix) for y in range(self.grid.iy) if x % 2 == 0 or y % 2 == 0] # initialize full obstacle map 18 | if initial_cell[0] % 2 == 0 or initial_cell[1] % 2 == 0: 19 | self.fcell = (1, 1) 20 | print "Cell coordinates must be odd, initial cell sets to (1, 1)" 21 | else: 22 | self.fcell = initial_cell 23 | 24 | def in_bounds(self, id): 25 | return 0 <= id[0] and id[0] < self.grid.ix and 0 <= id[1] and id[1] < self.grid.iy 26 | 27 | def neighbors(self, id): 28 | neighbor = [(id[0], id[1] - 2), (id[0] + 2, id[1]), (id[0], id[1] + 2), (id[0] - 2, id[1])] #<-------------------- clockwise 29 | neighbor = filter(self.in_bounds, neighbor) 30 | return neighbor 31 | 32 | 33 | def generate(self): # generate maze routine 34 | #-------------------- Initialize -------------------- 35 | stack = [] 36 | unvisited = [(x, y) for x in range(1, self.grid.ix, 2) for y in range(1, self.grid.iy, 2)] 37 | 38 | stack.append(self.fcell) 39 | unvisited.remove(self.fcell) 40 | current = self.fcell 41 | 42 | # ------------------ Main loop ---------------------- 43 | while unvisited: 44 | movements = [x for x in self.neighbors(current) if x in unvisited] 45 | 46 | if movements: 47 | next_move = movements[randint(0, len(movements) - 1)] 48 | stack.append(next_move) 49 | wall = (current[0] + (next_move[0] - current[0]) / 2, current[1] + (next_move[1] - current[1]) / 2) 50 | self.grid.walls.remove(wall) 51 | 52 | unvisited.remove(next_move) 53 | current = next_move 54 | else: 55 | current = stack.pop() 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | -------------------------------------------------------------------------------- /PathPlanning.pyde: -------------------------------------------------------------------------------- 1 | ####################################################################################################### 2 | # Author: An T. Le, DOB: 11/11/1997 3 | # Organization: Vietnamese-German University 4 | # Date Updated: March 21th, 2017 5 | # Date Created: July 2nd, 2016 6 | # Application: Occupancy Grid Library and Search-based Path Planning Library 7 | ####################################################################################################### 8 | 9 | 10 | from Grid import SquareGrid, PriorityDict, map1, maze1 11 | from Algorithm import astar_search, reconstruct_path, dlite_search 12 | from threading import Thread 13 | from robot import Robot 14 | from MazeGenerator import MazeGenerator 15 | import copy 16 | 17 | 18 | ################################ User Configuration ################################################### 19 | 20 | res = 10 # set map resolution (10 means 10 pixels per square grid) 21 | 22 | startP = None # set starting and goal position of the robot (origin of coordinate is at top left of the window) 23 | goal = None 24 | 25 | sizeX = 510 # set the size of the environment 26 | sizeY = 510 27 | 28 | grid = None 29 | ############################### Main Program ########################################################### 30 | #global variables for display 31 | path = [] 32 | frontier = PriorityDict() 33 | data = (None, None) 34 | closedNode = [] 35 | 36 | #Main 37 | def setup(): 38 | size(sizeX, sizeY, P3D) # set the size of the environnment 39 | 40 | #open_environment((2, 25), (35, 25)) 41 | maze_environment() 42 | 43 | global bot 44 | bot = Robot(2, 1, grid, startP) # initialize robot perception 45 | 46 | def draw(): 47 | grid.draw_grid(startP, goal, bot, None, None, None, path, closedNode) # continuously draw map and robot conditions in real-time 48 | 49 | def mousePressed(): 50 | curr = (mouseX // res, mouseY // res) # identify a grid position when clicked on the windows 51 | if curr in grid.walls: # on-off rountine, if it is an obstacle, then remove it and vice versa. 52 | grid.walls.remove(curr) 53 | else: 54 | grid.walls.append(curr) 55 | 56 | def keyPressed(): 57 | if key == 'd': # trigger d*lite algorithm with LHD criteria 58 | thread = Thread(target=dlite_path_analyst_thread) 59 | thread.start() 60 | if key == 'r': # trigger d*lite algorithm with Ratio criteria 61 | thread = Thread(target=dlr_path_analyst_thread) 62 | thread.start() 63 | elif key == 'a': # trigger a* algorithm with LHD criteria 64 | thread = Thread(target=astar_path_analyst_thread) 65 | thread.start() 66 | elif key == 'b': # move the robot along the calculated path 67 | thread = Thread(target=robot_move) 68 | thread.start() 69 | 70 | 71 | def astar_path_analyst_thread(): # define thread for a* algorithm 72 | global path 73 | global data 74 | data = astar_search(grid, startP, goal) 75 | path = reconstruct_path(data[0], startP, goal) 76 | 77 | 78 | def robot_move(): # define thread for robot move along calculated path 79 | bot.isStop = False 80 | bot.pos = startP 81 | bot.path = path 82 | while not bot.isStop: 83 | print bot.get_data(grid) 84 | bot.traverse() 85 | 86 | def dlite_path_analyst_thread(): # define thread for d*lite algorithm with LHD criteria 87 | global path 88 | global frontier 89 | dlite_search(grid, bot, startP, goal, path, frontier, closedNode, 1) 90 | 91 | def dlr_path_analyst_thread(): # define thread for d*lite algorithm with Ratio criteria 92 | global path 93 | global frontier 94 | dlite_search(grid, bot, startP, goal, path, frontier, closedNode, 0.5) 95 | 96 | def open_environment(set_start, set_goal): 97 | global grid 98 | grid = SquareGrid(width, height, res) 99 | 100 | global startP 101 | startP = set_start 102 | 103 | global goal 104 | goal = set_goal 105 | 106 | def maze_environment(set_start = None, set_goal = None): 107 | global grid 108 | maze = MazeGenerator(width, height, res, (1, 1)) # initialize map 109 | maze.generate() 110 | grid = maze.grid 111 | 112 | global startP 113 | startP = (1, 1) 114 | 115 | global goal 116 | goal = (width / res - 2, height / res - 2) # always set goal at bottom right of the map -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Processing-path_planning 2 | Implement D*Lite and A* Algorithm on Processing environment 3 | -------------------------------------------------------------------------------- /robot$py.class: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/anindex/Processing-path_planning/6cc752cd5b979fa8672f287a1f351f3669a05c0c/robot$py.class -------------------------------------------------------------------------------- /robot.py: -------------------------------------------------------------------------------- 1 | ####################################################################################################### 2 | # Author: An T. Le, DOB: 11/11/1997 3 | # Organization: Vietnamese-German University 4 | # Date Updated: March 21th, 2017 5 | # Date Created: July 2nd, 2016 6 | # Application: Occupancy Grid Library and Search-based Path Planning Library 7 | ####################################################################################################### 8 | 9 | from time import sleep 10 | 11 | 12 | ##################################### Concept Robot with Rnage Sensor in Virtual Map ############################################################### 13 | class Robot: 14 | def __init__(self, sensor_range, speed, grid, startP): 15 | self.sensor_range = sensor_range 16 | self.speed = speed 17 | self.path = None 18 | self.pos = startP 19 | self.knownWorld = [] # robot knowledge about its surroundings 20 | self.isStop = False 21 | self.pred_of = {} 22 | 23 | self.ix = grid.ix 24 | self.iy = grid.iy 25 | 26 | def traverse(self): # traverse routine 27 | if self.pos != self.path[-1] and self.pos in self.path: 28 | next = self.path[self.path.index(self.pos) + 1] 29 | if next not in self.knownWorld: 30 | sleep(1.0 / self.speed) 31 | self.pos = next 32 | else: 33 | self.isStop = True 34 | else: 35 | self.isStop = True 36 | 37 | def get_data(self, grid): # retrieve data about cells' states 38 | result = [(self.pos[0] + i, self.pos[1] + j) for i in range(-self.sensor_range, self.sensor_range + 1) for j in range(-self.sensor_range, self.sensor_range + 1)] 39 | result.remove(self.pos) 40 | dark = filter(lambda item: item in grid.walls, result) 41 | white = filter(lambda item: item not in grid.walls, result) 42 | return dark, white 43 | 44 | def passable(self, id): 45 | return id not in self.knownWorld 46 | 47 | def in_bounds(self, id): 48 | return 0 <= id[0] and id[0] < self.ix and 0 <= id[1] and id[1] < self.iy 49 | 50 | def isPred(self, u, v): 51 | return self.pred_of[v] == u 52 | 53 | def cost(self, currP, nextP): 54 | if currP in self.knownWorld or nextP in self.knownWorld: 55 | return float("inf") 56 | val = 0 57 | if abs(nextP[1] - currP[1]) + abs(nextP[0] - currP[0]) == 1: 58 | val += 1 59 | elif abs(nextP[1] - currP[1]) + abs(nextP[0] - currP[0]) == 2: 60 | val += 1.414 61 | return val 62 | 63 | def get_pred(self, id): 64 | x, y = id 65 | result = [(x + i, y + j) for i in range(-1, 2) for j in range(-1, 2)] 66 | result.remove(id) 67 | result = filter(self.in_bounds, result) 68 | result = filter(self.passable, result) 69 | result = filter(lambda item: self.isPred(item, id), result) 70 | if (x+y) % 2 == 0: result.reverse() 71 | return result 72 | 73 | def detect_neighbor(self, id): 74 | x, y = id 75 | result = [(x + i, y + j) for i in range(-1, 2) for j in range(-1, 2)] 76 | result.remove(id) 77 | result = filter(self.in_bounds, result) 78 | result = filter(self.passable, result) 79 | if (x+y) % 2 == 0: result.reverse() 80 | return result 81 | 82 | def detect_changes(self, grid): # detect change in states of robot's surroundings 83 | sensorData = self.get_data(grid) 84 | toHigh = filter(lambda item: item not in self.knownWorld, sensorData[0]) 85 | toLow = filter(lambda item: item in self.knownWorld, sensorData[1]) 86 | return toHigh, toLow 87 | 88 | def update_cell(self, id): # update a cell in robot knowledge 89 | if id in self.knownWorld: 90 | self.knownWorld.remove(id) 91 | else: 92 | self.knownWorld.append(id) 93 | 94 | def update_map(self, changedNode): # update robot knowledge 95 | self.knownWorld += changedNode[0] 96 | for node in changedNode[1]: 97 | self.knownWorld.remove(node) -------------------------------------------------------------------------------- /sketch.properties: -------------------------------------------------------------------------------- 1 | mode=Python 2 | mode.id=jycessing.mode.PythonMode 3 | --------------------------------------------------------------------------------