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()