def scanForObstacles(graph, heap, s_current, scan_range, k_m): states_to_update = {} range_checked = 0 if scan_range >= 1: for neighbor in graph.graph[s_current].successors: neighbor_coords = stateNameToCoords(neighbor) states_to_update[neighbor] = graph.cells[neighbor_coords[1]][ neighbor_coords[0]] range_checked = 1 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].successors: 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: for neighbor in graph.graph[state].successors: # first time to observe this obstacle where one wasn't before if (graph.graph[state].successors[neighbor] != float('inf')): neighbor_coords = stateNameToCoords(state) graph.cells[neighbor_coords[1]][neighbor_coords[0]] = -2 graph.graph[neighbor].successors[state] = float('inf') graph.graph[state].successors[neighbor] = float('inf') updateVertex(graph, heap, state, s_current, k_m) new_obstacle = True return new_obstacle
def computeShortestPath(self, graph, queue, s_start, k_m): while (graph.graph[s_start].rhs != graph.graph[s_start].g or self.topKey(queue, graph, s_start)< self.calculateKey(graph, s_start, s_start, k_m)): if self.step_is_on: # print(graph.graph[s_start]) # print('topKey') # print(topKey(queue)) # print('calculateKey') # print(calculateKey(graph, s_start, 0)) k_old = self.topKey(queue, graph, s_start) #print(k_old) u = heapq.heappop(queue)[2] if graph.graph[u].g > graph.graph[u].rhs: graph.graph[u].g = graph.graph[u].rhs y, x = stateNameToCoords(u) if graph.cells[x][y] >= 0: graph.cells[x][y] = 2 if graph.graph[u].rhs == graph.graph[u].g else 3 render_all(graph, status="Computer Shortest Path") for i in graph.graph[u].parents: self.updateVertex(graph, queue, i, s_start, k_m) else: graph.graph[u].g = float('inf') y, x = stateNameToCoords(u) if graph.cells[x][y] >= 0: graph.cells[x][y] = 2 if graph.graph[u].rhs == graph.graph[u].g else 3 render_all(graph, status="Computer Shortest Path") self.updateVertex(graph, queue, u, s_start, k_m) for i in graph.graph[u].parents: self.updateVertex(graph, queue, i, s_start, k_m)
def scanForObstacles(self, 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') render_all(graph, status="Update Edge Cost due to Changes") graph.graph[state].children[neighbor] = float('inf') render_all(graph, status="Update Edge Cost due to Changes") self.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 computeShortestPath(graph, heap, s_start, k_m): while (graph.graph[s_start].rhs != graph.graph[s_start].g) or ( topKey(heap) < calculateKey(graph, s_start, s_start, k_m)): k_old = topKey(heap) u = popHeap(graph, heap) if k_old < calculateKey(graph, u, s_start, k_m): insert(graph, heap, u, calculateKey(graph, u, s_start, k_m)) elif graph.graph[u].g > graph.graph[u].rhs: graph.graph[u].g = graph.graph[u].rhs for n in graph.graph[u].neighbors: neighbor_coords = stateNameToCoords(n) # in order to be a predecessor, neighbor is not occupied if (graph.cells[neighbor_coords[1]][neighbor_coords[0]] != -1): updateVertex(graph, heap, n, s_start, k_m) else: graph.graph[u].g = float('inf') updateVertex(graph, heap, u, s_start, k_m) for n in graph.graph[u].neighbors: neighbor_coords = stateNameToCoords(n) # in order to be a predecessor, neighbor is not occupied if (graph.cells[neighbor_coords[1]][neighbor_coords[0]] != -1): updateVertex(graph, heap, n, s_start, k_m)
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) k_m += heuristic_from_s(graph, s_last, s_new) computeShortestPath(graph, queue, s_current, k_m) return s_new, k_m
def updateVertex(self, 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 y, x = stateNameToCoords(id) if graph.cells[x][y] >= 0: graph.cells[x][y] = 2 if graph.graph[id].rhs == graph.graph[id].g else 3 render_all(graph, status="Update Vertex") 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, self.calculateKey(graph, id, s_current, k_m) + (id,)) self.print_queue(queue)
def moveAndRescan(self, graph, queue, s_current, scan_range, k_m): if(s_current == graph.goal): return 'goal', k_m else: results = self.scanForObstacles(graph, queue, s_current, scan_range, k_m) k_m += self.heuristic_from_s(graph, s_current, graph.goal) #self.computeShortestPath(graph, queue, s_current, k_m) x = threading.Thread(target=self.computeShortestPath, args=(graph, queue, s_current, k_m,)) x.start() s_last = s_current s_new = self.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 # print(graph) return s_new, k_m
pygame.display.set_caption("D* Lite Path Planning") # Loop until the user clicks the close button. done = False goal_set = False # Used to manage how fast the screen updates clock = pygame.time.Clock() if __name__ == "__main__": graph = Graph(X_DIM, Y_DIM) # graph = GridWorld(X_DIM, Y_DIM) s_start = 'x1y2' s_current = s_start pos_coords = stateNameToCoords(s_current) # 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, color, [(MARGIN + WIDTH) * column + MARGIN, (MARGIN + HEIGHT) * row + MARGIN, WIDTH, HEIGHT]) node_name = 'x' + str(column) + 'y' + str(row) # draw moving robot, based on pos_coords robot_center = [
s_goal = "x" + str(s_goal_list[0]) + "y" + str(s_goal_list[1]) + "z" + str(s_goal_list[2]) path = [s_start] graph.setStart(s_start) graph.setGoal(s_goal) k_m = 10 s_last = s_start queue = [] obstacles = [] new_obstacles = [] graph, queue, k_m = initDStarLite(graph, queue, s_start, s_goal, k_m) s_current = s_start pos_coords = stateNameToCoords(s_current) for i in range(numOb): obstacles = generateObstacles(worldSize, graph, obstacles) i += 1 # -------- Main Program Loop ----------- while not done: print(s_current) new_obstacles = moveObstacles(worldSize, numOb, graph, obstacles) s_new, k_m = moveAndRescan(graph, queue, s_current, scan_range, k_m) if s_new == 'goal': print('Goal Reached!') done = True