class DStarLite:
def __init__(self, map: OccupancyGridMap, s_start: (int, int, int),
s_goal: (int, int, int)):
"""
:param map: the ground truth map of the environment provided by gui
:param s_start: start location
:param s_goal: end location
"""
## we need to also update our occupancy grid map
self.new_edges_and_old_costs = None
# algorithm start
self.s_start = s_start # now takes in an x, y and z
self.s_goal = s_goal # now takes in an x, y and z
self.s_last = s_start # now takes in an x, y and z
self.k_m = 0 # accumulation
self.U = PriorityQueue()
self.rhs = np.ones((map.x_dim, map.y_dim, map.z_dim)) * np.inf
self.g = self.rhs.copy()
self.sensed_map = OccupancyGridMap(x_dim=map.x_dim,
y_dim=map.y_dim,
z_dim=map.z_dim,
exploration_setting='26N')
self.rhs[self.s_goal] = 0
self.U.insert(self.s_goal,
Priority(heuristic(self.s_start, self.s_goal), 0))
def calculate_key(self, s: (int, int, int)):
"""
:param s: the vertex we want to calculate key
:return: Priority class of the two keys
"""
k1 = min(self.g[s], self.rhs[s]) + heuristic(self.s_start,
s) + self.k_m
k2 = min(self.g[s], self.rhs[s])
return Priority(k1, k2)
def c(self, u: (int, int, int), v: (int, int, int)) -> float:
"""
calcuclate the cost between nodes
:param u: from vertex
:param v: to vertex
:return: euclidean distance to traverse. inf if obstacle in path
"""
if not self.sensed_map.is_unoccupied(
u) or not self.sensed_map.is_unoccupied(v):
return float('inf')
else:
return heuristic(u, v)
def contain(self, u: (int, int, int)):
return u in self.U.vertices_in_heap
def update_vertex(self, u: (int, int, int)):
if self.g[u] != self.rhs[u] and self.contain(u):
self.U.update(u, self.calculate_key(u))
elif self.g[u] != self.rhs[u] and not self.contain(u):
self.U.insert(u, self.calculate_key(u))
elif self.g[u] == self.rhs[u] and self.contain(u):
self.U.remove(u)
def compute_shortest_path(self):
while self.U.top_key() < self.calculate_key(
self.s_start) or self.rhs[self.s_start] > self.g[self.s_start]:
u = self.U.top()
k_old = self.U.top_key()
k_new = self.calculate_key(u)
if k_old < k_new:
self.U.update(u, k_new)
elif self.g[u] > self.rhs[u]:
self.g[u] = self.rhs[u]
self.U.remove(u)
pred = self.sensed_map.succ(vertex=u)
for s in pred:
if s != self.s_goal:
self.rhs[s] = min(self.rhs[s],
self.c(s, u) + self.g[u])
self.update_vertex(s)
else:
self.g_old = self.g[u]
self.g[u] = float('inf')
pred = self.sensed_map.succ(vertex=u)
pred.append(u)
for s in pred:
if self.rhs[s] == self.c(s, u) + self.g_old:
if s != self.s_goal:
min_s = float('inf')
succ = self.sensed_map.succ(vertex=s)
for s_ in succ:
temp = self.c(s, s_) + self.g[s_]
if min_s > temp:
min_s = temp
self.rhs[s] = min_s
self.update_vertex(u)
def rescan(self) -> Vertices:
new_edges_and_old_costs = self.new_edges_and_old_costs
self.new_edges_and_old_costs = None
return new_edges_and_old_costs
def move_and_replan(self, robot_position: (int, int, int)):
path = [robot_position]
self.s_start = robot_position
self.s_last = self.s_start
self.compute_shortest_path()
while self.s_start != self.s_goal:
assert (self.rhs[self.s_start] !=
float('inf')), "There is no known path!"
succ = self.sensed_map.succ(self.s_start, avoid_obstacles=False)
min_s = float('inf')
arg_min = None
for s_ in succ:
temp = self.c(self.s_start, s_) + self.g[s_]
if temp < min_s:
min_s = temp
arg_min = s_
### algorithm sometimes gets stuck here for some reason !!! FIX
self.s_start = arg_min
path.append(self.s_start)
# scan graph for changed costs
changed_edges_with_old_cost = self.rescan()
#print("len path: {}".format(len(path)))
# if any edge costs changed
if changed_edges_with_old_cost:
self.k_m += heuristic(self.s_last, self.s_start)
self.s_last = self.s_start
# for all directed edges (u,v) with changed edge costs
vertices = changed_edges_with_old_cost.vertices
for vertex in vertices:
v = vertex.pos
succ_v = vertex.edges_and_c_old
for u, c_old in succ_v.items():
c_new = self.c(u, v)
if c_old > c_new:
if u != self.s_goal:
self.rhs[u] = min(self.rhs[u],
self.c(u, v) + self.g[v])
elif self.rhs[u] == c_old + self.g[v]:
if u != self.s_goal:
min_s = float('inf')
succ_u = self.sensed_map.succ(vertex=u)
for s_ in succ_u:
temp = self.c(u, s_) + self.g[s_]
if min_s > temp:
min_s = temp
self.rhs[u] = min_s
self.update_vertex(u)
self.compute_shortest_path()
if len(path) > 1:
return path, self.g, self.rhs
return None, None, None
class Animation:
def __init__(self,
title="Project 5 - Namala & Srivastava",
width=10,
height=10,
margin=0,
x_dim=100,
y_dim=50,
start=(0, 0),
goal=(50, 50),
start2=(0, 0),
goal2=(50, 50),
viewing_range=3):
self.width = width
self.height = height
self.margin = margin
self.x_dim = x_dim
self.y_dim = y_dim
self.start = start
self.current = start
self.start2 = start2
self.current2 = start2
self.observation = {"pos": None, "type": None}
self.observation2 = {"pos": None, "type": None}
self.goal = goal
self.goal2 = goal2
self.viewing_range = viewing_range
pygame.init()
# Set the 'width' and 'height' of the screen
window_size = [(width + margin) * y_dim + margin,
(height + margin) * x_dim + margin]
self.screen = pygame.display.set_mode(window_size)
self.world = OccupancyGridMap(x_dim=x_dim,
y_dim=y_dim,
exploration_setting='8N')
# Set title of screen
pygame.display.set_caption(title)
# set font
pygame.font.SysFont('Comic Sans MS', 36)
# Loop until the user clicks the close button
self.done = False
# used to manage how fast the screen updates
self.clock = pygame.time.Clock()
def get_position(self):
return self.current, self.current2
def set_position(self, pos: (int, int), pos2: (int, int)):
self.current = pos
self.current2 = pos2
def get_goal(self):
return self.goal
def set_goal(self, goal: (int, int)):
self.goal = goal
def set_start(self, start: (int, int)):
self.start = start
def display_path(self, path=None):
if path is not None:
for step in path:
# draw a moving robot, based on current coordinates
step_center = [round(step[1] * (self.width + self.margin) + self.width / 2) + self.margin,
round(step[0] * (self.height + self.margin) + self.height / 2) + self.margin]
# draw robot position as red circle
pygame.draw.circle(self.screen, START, step_center, round(self.width / 2) - 2)
def display_path2(self, path2=None):
if path2 is not None:
for step in path2:
# draw a moving robot, based on current coordinates
step_center = [round(step[1] * (self.width + self.margin) + self.width / 2) + self.margin,
round(step[0] * (self.height + self.margin) + self.height / 2) + self.margin]
# draw robot position as red circle
pygame.draw.circle(self.screen, START2, step_center, round(self.width / 2) - 2)
def display_obs(self, observations=None):
if observations is not None:
for o in observations:
pygame.draw.rect(self.screen, GRAY1, [(self.margin + self.width) * o[1] + self.margin,
(self.margin + self.height) * o[0] + self.margin,
self.width,
self.height])
def collision_detector(self, path, path2):
for i in range(-5 + path[0][0], 5 + path[0][0]):
for j in range(-5 + path[0][1], 5 + path[0][1]):
if (i, j) in path2[:5]:
return True
return False
def run_game(self, path=None, path2=None):
for i in range(self.x_dim):
for j in range(self.y_dim):
if obstacle(i, j):
grid_cell = (i, j)
self.world.set_obstacle(grid_cell)
##### Put the collision checker
if self.collision_detector(path=path, path2=path2):
print("Collision Detected")
f = open("position.txt", "r")
m = int(f.read())
plt.figure()
x_dim = m + len(path)
y_dim = m + len(path2)
obstacle_list = list()
coordination_map = OccupancyGridMap(x_dim, y_dim)
for i in range(-5 + m, 5 + m):
for j in range(-5 + m, 5 + m):
coordination_map.set_obstacle((i, j))
obstacle_list.append((i, j))
obstacle_list = np.array(obstacle_list)
slam = SLAM(map=coordination_map,
view_range=max(x_dim, y_dim))
dstar = DStarLite(map=coordination_map, s_start=(0, 0), s_goal=(x_dim - 1, y_dim - 1))
coordinationPath, g, rhs = dstar.move_and_replan(robot_position=(0, 0))
# slam
new_edges_and_old_costs, slam_map = slam.rescan(global_position=(0, 0))
dstar.new_edges_and_old_costs = new_edges_and_old_costs
dstar.sensed_map = slam_map
coordinationPath1, g, rhs = dstar.move_and_replan(robot_position=(0, 0))
coordinationPath = np.array(coordinationPath1)
print("Coordination Path", coordinationPath)
print("Obstacle LIST", obstacle_list)
plt.plot(coordinationPath[:, 0], coordinationPath[:, 1], '-b')
plt.plot(obstacle_list[:, 0], obstacle_list[:, 1], '.r')
plt.xlabel('Robot 1 Path Steps')
plt.ylabel('Robot 2 Path Steps')
plt.show()
robot1_movements = coordinationPath[:, 0]
robot2_movements = coordinationPath[:, 1]
occurrences1 = list()
occurrences2 = list()
for i in range(min(robot1_movements), max(robot1_movements)):
occurrences1.append(np.count_nonzero(robot1_movements == i))
for i in range(min(robot2_movements), max(robot2_movements)):
occurrences2.append(np.count_nonzero(robot2_movements == i))
robot1_stops = list()
for a in range(0,len(occurrences1)):
if occurrences1[a]>1:
robot1_stops.append((a, occurrences1[a]))
robot2_stops = list()
for a in range(0, len(occurrences2)):
if occurrences2[a] > 1:
robot2_stops.append((a, occurrences2[a]))
print(robot1_stops)
print(robot2_stops)
# The remaining path of robot 1 and robot 2
f = open("R1_Path.txt", "a")
for each in path:
f.write(str(each)+"!")
f.close()
f = open("R2_Path.txt", "a")
for each in path2:
f.write(str(each)+"!")
f.close()
f = open("R1_Path.txt", 'r')
r1_path = f.read()
f.close()
f = open("R2_Path.txt", 'r')
r2_path = f.read()
f.close()
r1_path_list = r1_path.split('!')
r2_path_list = r2_path.split('!')
r1_plot_list = list()
r2_plot_list = list()
for each in r1_path_list:
try:
temp = each[1:-1].split(', ')
r1_plot_list.append((int(temp[0]),int(temp[1])))
except:
pass
for each in r2_path_list:
try:
temp = each[1:-1].split(', ')
r2_plot_list.append((int(temp[0]),int(temp[1])))
except:
pass
main_path_r1 = list()
main_path_r2 = list()
if len(robot1_stops)>0:
for each in robot1_stops:
index = each[0]
iterations = each[1]
counter = -1
for anothereach in r1_plot_list:
counter = counter + 1
main_path_r1.append(anothereach)
if counter == index:
for i in range(iterations - 1):
main_path_r1.append(anothereach)
break
if len(robot2_stops) == 0:
main_path_r1 = r1_plot_list
if len(robot2_stops) > 0:
index_list_r2 = list()
iteration_list_r2 =list()
for each in robot2_stops:
index_list_r2.append(each[0])
iteration_list_r2.append(each[1])
counter = -1
for anothereach in r2_plot_list:
counter = counter + 1
main_path_r2.append(anothereach)
if counter in index_list_r2:
for i in range(0, iteration_list_r2[index_list_r2.index(counter)]-1):
main_path_r2.append(anothereach)
if len(robot2_stops) == 0:
main_path_r2 = r2_plot_list
print("######")
print(r1_plot_list)
print("######")
print(r2_plot_list)
print("MAIN1\n")
print(main_path_r1)
print("MAIN2\n")
print(main_path_r2)
raise SystemExit()
# # x_dim = len(path)+ len(FILE)
# # y_dim = len(path2)+ len(FILE2)
# #### D STAR AGAIN :
# '''
# coordination_map = OccupancyGridMap(x_dim, y_dim)
# dstar = DStarLite(map=coordination_map,
# s_start=(0,0),
# s_goal=(xdim-1, ydim-1))
# path, g, rhs = dstar.move_and_replan(robot_position=s_start)
# '''
#### X_DIM = length of R1
##### Y_DIM = length of R2
pygame.font.init() # you have to call this at the start,
# if you want to use this module.
myfont = pygame.font.SysFont('Times New Roman', 30)
textsurface = myfont.render('Group 7: Alkesh, Prannoy', False, (119, 136, 153))
if path is None:
path = []
if path2 is None:
path2 = []
grid_cell = None
self.cont = False
for event in pygame.event.get():
if event.type == pygame.QUIT: # if user clicked close
print("quit")
self.done = True # flag that we are done so we can exit loop
elif (event.type == pygame.KEYDOWN and event.key == pygame.K_SPACE) or self.cont:
f = open("R1_Path.txt", "a")
f.write(str(path[0]) + ",")
f.close()
f = open("R2_Path.txt", "a")
f.write(str(path2[0]) + ",")
f.close()
# space bar pressed. call next action
if path:
if len(path) > 1:
(x, y) = path[1]
else:
(x, y) = path[0]
if not path:
pass
if path2:
if len(path2) > 1:
(x2, y2) = path2[1]
else:
(x2, y2) = path2[0]
if not path2:
pass
# for each in path: #R1
# if each not in path2 and not obstacle(each[0], each[1]): #R2
# for i in range(each[0] - 10, each[0] + 10):
# for j in range(each[1] - 10, each[1] + 10):
# if i < 100 and j < 100:
# if not self.world.is_unoccupied((i, j)) and not obstacle(i, j):
# self.world.remove_obstacle((i, j))
# for each in path:
# if each in path2:
# for i in range(each[0] - 2, each[0] + 2):
# for j in range(each[1] - 2, each[1] + 2):
# if i < 100 and j < 100:
# self.world.set_obstacle((i, j))
# self.world.set_obstacle(each)
self.set_position((x, y), (x2, y2))
# for i in range(-1 * int(self.viewing_range), int(self.viewing_range)):
# for j in range(-1 * int(self.viewing_range), int(self.viewing_range)):
#
# if obstacle(x + i, y + j):
# grid_cell = (x + i, y + j)
# self.world.set_obstacle(grid_cell)
# if obstacle(x2 + i, y2 + j):
# grid_cell = ((x2 + i, y2 + j))
# self.world.set_obstacle(grid_cell)
elif event.type == pygame.KEYUP and event.key == pygame.K_SPACE:
pass
elif event.type == pygame.KEYDOWN and event.key == pygame.K_BACKSPACE:
print("backspace automates the press space")
if not self.cont:
self.cont = True
else:
self.cont = False
# set obstacle by holding left-click
elif pygame.mouse.get_pressed()[0]:
# User clicks the mouse. Get the position
(col, row) = pygame.mouse.get_pos()
# change the x/y screen coordinates to grid coordinates
x = row // (self.height + self.margin)
y = col // (self.width + self.margin)
# turn pos into cell
grid_cell = (x, y)
# set the location in the grid map
if self.world.is_unoccupied(grid_cell):
self.world.set_obstacle(grid_cell)
self.observation = {"pos": grid_cell, "type": OBSTACLE}
# remove obstacle by holding right-click
elif pygame.mouse.get_pressed()[2]:
# User clicks the mouse. Get the position
(col, row) = pygame.mouse.get_pos()
# change the x/y screen coordinates to grid coordinates
x = row // (self.height + self.margin)
y = col // (self.width + self.margin)
# turn pos into cell
grid_cell = (x, y)
# set the location in the grid map
if not self.world.is_unoccupied(grid_cell):
print("grid cell: ".format(grid_cell))
self.world.remove_obstacle(grid_cell)
self.observation = {"pos": grid_cell, "type": UNOCCUPIED}
# set the screen background
self.screen.fill(BLACK)
# draw the grid
for row in range(self.x_dim):
for column in range(self.y_dim):
# color the cells
pygame.draw.rect(self.screen, colors[self.world.occupancy_grid_map[row][column]],
[(self.margin + self.width) * column + self.margin,
(self.margin + self.height) * row + self.margin,
self.width,
self.height])
self.display_path(path=path)
self.display_path2(path2=path2)
# fill in the goal cell with green
pygame.draw.rect(self.screen, GOAL, [(self.margin + self.width) * self.goal[1] + self.margin,
(self.margin + self.height) * self.goal[0] + self.margin,
self.width,
self.height])
# fill in the goal cell with green
pygame.draw.rect(self.screen, GOAL, [(self.margin + self.width) * self.goal[1] + self.margin,
(self.margin + self.height) * self.goal[0] + self.margin,
self.width,
self.height])
pygame.draw.rect(self.screen, GOAL2, [(self.margin + self.width) * self.goal2[1] + self.margin,
(self.margin + self.height) * self.goal2[0] + self.margin,
self.width,
self.height])
# draw a moving robot, based on current coordinates
robot_center = [round(self.current[1] * (self.width + self.margin) + self.width / 2) + self.margin,
round(
self.current[0] * (self.height + self.margin) + self.height / 2) + self.margin]
robot_center2 = [round(self.current2[1] * (self.width + self.margin) + self.width / 2) + self.margin,
round(
self.current2[0] * (self.height + self.margin) + self.height / 2) + self.margin]
# draw robot position as red circle
pygame.draw.circle(self.screen, START, robot_center, round(self.width / 2) - 2)
# draw robot local grid map (viewing range)
pygame.draw.rect(self.screen, LOCAL_GRID,
[robot_center[0] - self.viewing_range * (self.height + self.margin),
robot_center[1] - self.viewing_range * (self.width + self.margin),
2 * self.viewing_range * (self.height + self.margin),
2 * self.viewing_range * (self.width + self.margin)], 2)
pygame.draw.rect(self.screen, LOCAL_GRID,
[robot_center2[0] - self.viewing_range * (self.height + self.margin),
robot_center2[1] - self.viewing_range * (self.width + self.margin),
2 * self.viewing_range * (self.height + self.margin),
2 * self.viewing_range * (self.width + self.margin)], 2)
# set game tick
self.clock.tick(20)
self.screen.blit(textsurface, (self.screen.get_width() // 2 - 150, 0))
# go ahead and update screen with that we've drawn
pygame.display.flip()
# be 'idle' friendly. If you forget this, the program will hang on exit
pygame.quit()
class Animation:
def __init__(self,
title="D* Lite Path Planning",
width=10,
height=10,
margin=0,
x_dim=100,
y_dim=50,
start=(0, 0),
goal=(50, 50),
viewing_range=3):
self.width = width
self.height = height
self.margin = margin
self.x_dim = x_dim
self.y_dim = y_dim
self.start = start
self.current = start
self.observation = {"pos": None, "type": None}
self.goal = goal
self.viewing_range = viewing_range
pygame.init()
# Set the 'width' and 'height' of the screen
window_size = [(width + margin) * y_dim + margin,
(height + margin) * x_dim + margin]
self.screen = pygame.display.set_mode(window_size)
# create occupancy grid map
"""
set initial values for the map occupancy grid
|----------> y, column
| (x=0,y=2)
|
V (x=2, y=0)
x, row
"""
self.world = OccupancyGridMap(x_dim=x_dim,
y_dim=y_dim,
exploration_setting='8N')
# Set title of screen
pygame.display.set_caption(title)
# set font
pygame.font.SysFont('Comic Sans MS', 36)
# Loop until the user clicks the close button
self.done = False
# used to manage how fast the screen updates
self.clock = pygame.time.Clock()
def get_position(self):
return self.current
def set_position(self, pos: (int, int)):
self.current = pos
def get_goal(self):
return self.goal
def set_goal(self, goal: (int, int)):
self.goal = goal
def set_start(self, start: (int, int)):
self.start = start
def display_path(self, path=None):
if path is not None:
for step in path:
# draw a moving robot, based on current coordinates
step_center = [
round(step[1] *
(self.width + self.margin) + self.width / 2) +
self.margin,
round(step[0] *
(self.height + self.margin) + self.height / 2) +
self.margin
]
# draw robot position as red circle
pygame.draw.circle(self.screen, START, step_center,
round(self.width / 2) - 2)
def display_obs(self, observations=None):
if observations is not None:
for o in observations:
pygame.draw.rect(
self.screen, GRAY1,
[(self.margin + self.width) * o[1] + self.margin,
(self.margin + self.height) * o[0] + self.margin,
self.width, self.height])
def run_game(self, path=None):
if path is None:
path = []
grid_cell = None
self.cont = False
for event in pygame.event.get():
if event.type == pygame.QUIT: # if user clicked close
print("quit")
self.done = True # flag that we are done so we can exit loop
elif (event.type == pygame.KEYDOWN
and event.key == pygame.K_SPACE) or self.cont:
# space bar pressed. call next action
if path:
(x, y) = path[1]
self.set_position((x, y))
elif event.type == pygame.KEYDOWN and event.key == pygame.K_BACKSPACE:
print("backspace automates the press space")
if not self.cont:
self.cont = True
else:
self.cont = False
# set obstacle by holding left-click
elif pygame.mouse.get_pressed()[0]:
# User clicks the mouse. Get the position
(col, row) = pygame.mouse.get_pos()
# change the x/y screen coordinates to grid coordinates
x = row // (self.height + self.margin)
y = col // (self.width + self.margin)
# turn pos into cell
grid_cell = (x, y)
# set the location in the grid map
if self.world.is_unoccupied(grid_cell):
self.world.set_obstacle(grid_cell)
self.observation = {"pos": grid_cell, "type": OBSTACLE}
# remove obstacle by holding right-click
elif pygame.mouse.get_pressed()[2]:
# User clicks the mouse. Get the position
(col, row) = pygame.mouse.get_pos()
# change the x/y screen coordinates to grid coordinates
x = row // (self.height + self.margin)
y = col // (self.width + self.margin)
# turn pos into cell
grid_cell = (x, y)
# set the location in the grid map
if not self.world.is_unoccupied(grid_cell):
print("grid cell: ".format(grid_cell))
self.world.remove_obstacle(grid_cell)
self.observation = {"pos": grid_cell, "type": UNOCCUPIED}
# set the screen background
self.screen.fill(BLACK)
# draw the grid
for row in range(self.x_dim):
for column in range(self.y_dim):
# color the cells
pygame.draw.rect(
self.screen,
colors[self.world.occupancy_grid_map[row][column]],
[(self.margin + self.width) * column + self.margin,
(self.margin + self.height) * row + self.margin,
self.width, self.height])
self.display_path(path=path)
# fill in the goal cell with green
pygame.draw.rect(
self.screen, GOAL,
[(self.margin + self.width) * self.goal[1] + self.margin,
(self.margin + self.height) * self.goal[0] + self.margin,
self.width, self.height])
# draw a moving robot, based on current coordinates
robot_center = [
round(self.current[1] *
(self.width + self.margin) + self.width / 2) + self.margin,
round(self.current[0] *
(self.height + self.margin) + self.height / 2) + self.margin
]
# draw robot position as red circle
pygame.draw.circle(self.screen, START, robot_center,
round(self.width / 2) - 2)
# draw robot local grid map (viewing range)
pygame.draw.rect(self.screen, LOCAL_GRID, [
robot_center[0] - self.viewing_range *
(self.height + self.margin), robot_center[1] - self.viewing_range *
(self.width + self.margin), 2 * self.viewing_range *
(self.height + self.margin), 2 * self.viewing_range *
(self.width + self.margin)
], 2)
# set game tick
self.clock.tick(20)
# go ahead and update screen with that we've drawn
pygame.display.flip()
# be 'idle' friendly. If you forget this, the program will hang on exit
pygame.quit()
class DstarLite(object):
def __init__(self,
world,
s_start: (int, int),
s_goal: (int, int),
view_range=2):
# init the graphs
self.est_global_map = OccupancyGridMap(x_dim=world.x_dim,
y_dim=world.y_dim,
exploration_setting='8N')
# real_graph
self.gt_global_map: OccupancyGridMap = world
# init variables
self.view_range = view_range
self.position = s_start
self.goal = s_goal
self.back_pointers = {}
# procedure initialize
self.U = PriorityQueue()
self.k_m = 0
self.RHS_VALS = {}
self.G_VALS = {} # empty set
self.RHS_VALS[s_goal] = 0.0
self.U.insert(s_goal, self.calculate_key(s_goal))
# keeps track of the best path starting from goal to our position
self.back_pointers[self.goal] = None
# replan
self.compute_shortest_path(s_start=s_start)
def g(self, node: (int, int)):
"""
:param node:
:return:
"""
return self.G_VALS.get(node, float('inf'))
def rhs(self, node: (int, int)):
"""
:param node:
:return:
"""
# if node == self.position:
# return 0
return self.RHS_VALS.get(node,
float('inf')) if node != self.goal else 0
def calculate_key(self, node: (int, int)):
"""
procedure CalculateKey(s)
:param node:
:return:
"""
g_rhs = min([self.g(node), self.rhs(node)])
# return g_rhs + heuristic(node, self.position), g_rhs
return g_rhs + self.est_global_map.cost(
node, self.position) + self.k_m, g_rhs
def update_vertex(self, node: (int, int)):
"""
procedure UpdateVertex(u)
:param node:
:return:
"""
if node != self.goal:
self.RHS_VALS[node] = self.calculate_rhs(node)
if node in self.U:
self.U.delete(node)
if self.g(node) != self.rhs(node):
self.U.insert(node, self.calculate_key(node))
def update_vertices(self, nodes):
"""
:param nodes:
:return:
"""
for node in nodes:
self.update_vertex(node=node)
def lookahead_cost(self, node, neighbor):
"""
:param node:
:param neighbor:
:return:
"""
return self.g(neighbor) + self.est_global_map.cost(neighbor, node)
def lowest_cost_neighbor(self, node: (int, int)) -> (int, int):
"""
:param node:
:return:
"""
cost = partial(self.lookahead_cost, node)
best_choice = min(self.est_global_map.neighbors(node), key=cost)
return best_choice
def calculate_rhs(self, node: (int, int)) -> float:
"""
:param node:
:return:
"""
lowest_cost_neighbor = self.lowest_cost_neighbor(node)
self.back_pointers[node] = lowest_cost_neighbor
return self.lookahead_cost(node, lowest_cost_neighbor)
def compute_shortest_path(self, s_start: (int, int)):
"""
Procedure ComputeShortestPath()
:return:
"""
last_nodes = deque(maxlen=10)
while self.U.top_key(s_start) < self.calculate_key(
s_start) or self.rhs(s_start) != self.g(s_start):
u = self.U.pop_top()
k_old = self.U.top_key(s_start)
# NOT PART OF ALGORITHM!
last_nodes.append(u)
if len(last_nodes) == 10 and len(set(last_nodes)) < 3:
raise Exception("Failed! Stuck in a loop")
# NOT PART OF ALGORITHM!
k_new = self.calculate_key(u)
if k_old < k_new:
self.U.insert(
u, k_new
) # Essentially update, since u is already popped, and we put it back again
elif self.g(node=u) > self.rhs(node=u):
self.G_VALS[u] = self.rhs(node=u)
# since u is already removed, we update vertices
self.update_vertices(nodes=self.est_global_map.neighbors(u))
else:
self.G_VALS[u] = float('inf')
self.update_vertices(nodes=self.est_global_map.neighbors(u) +
[u]) # list + list = list
return self.back_pointers.copy(), self.G_VALS.copy()
def move_and_rescan(self, position: (int, int)):
"""
Procedure Main()
:return:
"""
self.robot_position = position
self.position = position
# rescan local area
local_observation = self.gt_global_map.local_observation(
global_position=self.robot_position, view_range=self.view_range)
# update global map from local data
# return new obstacles added to the map
cells_with_new_cost = self.est_global_map.update_global_from_local_grid(
local_grid=local_observation)
# make current position as last node
s_start = self.position
s_last = s_start
yield s_last, cells_with_new_cost
# while we yet haven't reached the goal
visited = {s_start}
while s_start != self.goal:
print("s_start: {}, position: {}".format(s_start,
self.robot_position))
if self.rhs(s_start) == float('inf'):
raise Exception("No path found!")
# find next lowest cost neighbor
s_start = self.lowest_cost_neighbor(s_start)
# move to next lowest cost neighbor. This might take a while
self.position = s_start
# scan graph for changed edge costs
local_observation = self.est_global_map.local_observation(
global_position=self.position, view_range=self.view_range)
# FIX NEEDED. This should not return cells_with_new_cost if there not in local view
cells_with_new_cost = self.est_global_map.update_global_from_local_grid(
local_grid=local_observation)
# if any edge cost changed
if cells_with_new_cost:
# print("cells with new cost {}".format(cells_with_new_cost))
self.k_m += self.est_global_map.cost(
s_last, s_start) # update heuristics
s_last = s_start
self.update_vertices({
node
for cell in cells_with_new_cost
for node in self.est_global_map.neighbors(cell)
if self.est_global_map.is_unoccupied(node)
}) # this last line is not needed
self.compute_shortest_path(s_start=s_start)
yield s_start, cells_with_new_cost
print("goal found!")
class DStarLite:
def __init__(self, map: OccupancyGridMap, s_start: (int, int),
s_goal: (int, int)):
self.new_edges_and_old_costs = None
self.s_start = s_start
self.s_goal = s_goal
self.s_last = s_start
self.k_m = 0
self.U = PriorityQueue()
self.rhs = np.ones((map.x_dim, map.y_dim)) * np.inf
self.g = self.rhs.copy()
self.sensed_map = OccupancyGridMap(x_dim=map.x_dim,
y_dim=map.y_dim,
exploration_setting='8N')
self.rhs[self.s_goal] = 0
self.U.insert(self.s_goal,
Priority(heuristic(self.s_start, self.s_goal), 0))
def calculate_key(self, s: (int, int)):
k1 = min(self.g[s], self.rhs[s]) + heuristic(self.s_start,
s) + self.k_m
k2 = min(self.g[s], self.rhs[s])
return Priority(k1, k2)
def c(self, u: (int, int), v: (int, int)) -> float:
if not self.sensed_map.is_unoccupied(
u) or not self.sensed_map.is_unoccupied(v):
return float('inf')
else:
return heuristic(u, v)
def contain(self, u: (int, int)) -> (int, int):
return u in self.U.vertices_in_heap
def update_vertex(self, u: (int, int)):
if self.g[u] != self.rhs[u] and self.contain(u):
self.U.update(u, self.calculate_key(u))
elif self.g[u] != self.rhs[u] and not self.contain(u):
self.U.insert(u, self.calculate_key(u))
elif self.g[u] == self.rhs[u] and self.contain(u):
self.U.remove(u)
def compute_shortest_path(self):
while self.U.top_key() < self.calculate_key(
self.s_start) or self.rhs[self.s_start] > self.g[self.s_start]:
u = self.U.top()
k_old = self.U.top_key()
k_new = self.calculate_key(u)
if k_old < k_new:
self.U.update(u, k_new)
elif self.g[u] > self.rhs[u]:
self.g[u] = self.rhs[u]
self.U.remove(u)
pred = self.sensed_map.succ(vertex=u)
for s in pred:
if s != self.s_goal:
self.rhs[s] = min(self.rhs[s],
self.c(s, u) + self.g[u])
self.update_vertex(s)
else:
self.g_old = self.g[u]
self.g[u] = float('inf')
pred = self.sensed_map.succ(vertex=u)
pred.append(u)
for s in pred:
if self.rhs[s] == self.c(s, u) + self.g_old:
if s != self.s_goal:
min_s = float('inf')
succ = self.sensed_map.succ(vertex=s)
for s_ in succ:
temp = self.c(s, s_) + self.g[s_]
if min_s > temp:
min_s = temp
self.rhs[s] = min_s
self.update_vertex(u)
def rescan(self) -> Vertices:
new_edges_and_old_costs = self.new_edges_and_old_costs
self.new_edges_and_old_costs = None
return new_edges_and_old_costs
def move_and_replan(self, robot_position: (int, int)):
path = [robot_position]
self.s_start = robot_position
self.s_last = self.s_start
self.compute_shortest_path()
while self.s_start != self.s_goal:
assert (self.rhs[self.s_start] !=
float('inf')), "There is no known path!"
succ = self.sensed_map.succ(self.s_start, avoid_obstacles=False)
min_s = float('inf')
arg_min = None
for s_ in succ:
temp = self.c(self.s_start, s_) + self.g[s_]
if temp < min_s:
min_s = temp
arg_min = s_
self.s_start = arg_min
path.append(self.s_start)
changed_edges_with_old_cost = self.rescan()
if changed_edges_with_old_cost:
self.k_m += heuristic(self.s_last, self.s_start)
self.s_last = self.s_start
vertices = changed_edges_with_old_cost.vertices
for vertex in vertices:
v = vertex.pos
succ_v = vertex.edges_and_c_old
for u, c_old in succ_v.items():
c_new = self.c(u, v)
if c_old > c_new:
if u != self.s_goal:
self.rhs[u] = min(self.rhs[u],
self.c(u, v) + self.g[v])
elif self.rhs[u] == c_old + self.g[v]:
if u != self.s_goal:
min_s = float('inf')
succ_u = self.sensed_map.succ(vertex=u)
for s_ in succ_u:
temp = self.c(u, s_) + self.g[s_]
if min_s > temp:
min_s = temp
self.rhs[u] = min_s
self.update_vertex(u)
self.compute_shortest_path()
print("path found!")
return path, self.g, self.rhs