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
