class Mock_Jetson:
def __init__(self, end_point):
"""
"""
self.grid = grid.Grid(tile_num_height, tile_num_width, tile_size)
self.client = Client()
self.sensor_state = SensorState()
#Visualization
self.master: Tk = Tk()
self.canvas: Canvas = None
#TODO: update heading
self.heading = 0
self.prev_draw_c1c0_ids = [None, None]
self.tile_dict: Dict[grid.Tile, int] = None
# Generates random enviroment on the grid
generator = RandomObjects(self.grid)
generator.create_env(22, 0, 0, 22, 9)
self.sensor_generator = GenerateSensorData(self.grid)
self.create_widgets()
# starting location for middle
self.curr_tile = self.grid.grid[int(tile_num_width / 2)][int(
tile_num_height / 2)]
self.client.send_data({'end_point': end_point})
print("sent")
self.main_loop()
self.master.mainloop()
def main_loop(self):
"""
"""
# TODO: update the curr tile from read
new_coor = self.client.listen()
self.curr_tile = self.grid.grid[new_coor[0]][new_coor[1]]
self.drawC1C0()
self.sensor_state = SensorState()
self.sensor_state.set_lidar(
self.sensor_generator.generateLidar(degree_freq,
self.curr_tile.row,
self.curr_tile.col))
self.client.send_data(self.sensor_state)
self.drawC1C0()
self.master.after(1, self.main_loop)
def create_widgets(self):
"""
Creates the canvas of the size of the inputted grid
"""
self.master.geometry("+900+100")
canvas = Canvas(self.master,
width=len(self.grid.grid[0]) * GUI_tile_size,
height=len(self.grid.grid) * GUI_tile_size)
offset = GUI_tile_size / 2
tile_dict = {}
for row in self.grid.grid:
for tile in row:
x = tile.x / tile_scale_fac
y = tile.y / tile_scale_fac
tile_dict[tile] = canvas.create_rectangle(
x - offset,
y - offset,
x + offset,
y + offset,
outline=tile.get_color(),
fill=tile.get_color())
canvas.pack()
self.canvas = canvas
self.tile_dict = tile_dict
def drawC1C0(self):
"""Draws C1C0's current location on the simulation"""
def _scale_coords(coords):
"""scales coords (a tuple (x, y)) from real life cm to pixels"""
scaled_x = coords[0] / tile_scale_fac
scaled_y = coords[1] / tile_scale_fac
return scaled_x, scaled_y
# coordinates of robot center right now (in cm)
center_x = self.curr_tile.x
center_y = self.curr_tile.y
# converting heading to radians, and adjusting so that facing right = 0 deg
heading_adj_rad = math.radians(self.heading + 90)
if self.prev_draw_c1c0_ids is not None:
# delete old drawings from previous iteration
self.canvas.delete(self.prev_draw_c1c0_ids[0])
self.canvas.delete(self.prev_draw_c1c0_ids[1])
# coordinates of bounding square around blue circle
top_left_coords = (center_x - robot_radius, center_y + robot_radius)
bot_right_coords = (center_x + robot_radius, center_y - robot_radius)
# convert coordinates from cm to pixels
top_left_coords_scaled = _scale_coords(top_left_coords)
bot_right_coords_scaled = _scale_coords(bot_right_coords)
# draw blue circle
self.prev_draw_c1c0_ids[0] = self.canvas.create_oval(
top_left_coords_scaled[0],
top_left_coords_scaled[1],
bot_right_coords_scaled[0],
bot_right_coords_scaled[1],
outline='black',
fill='blue')
center_coords_scaled = _scale_coords((center_x, center_y))
# finding endpoint coords of arrow
arrow_end_x = center_x + robot_radius * math.cos(heading_adj_rad)
arrow_end_y = center_y + robot_radius * math.sin(heading_adj_rad)
arrow_end_coords_scaled = _scale_coords((arrow_end_x, arrow_end_y))
# draw white arrow
self.prev_draw_c1c0_ids[1] = self.canvas.create_line(
center_coords_scaled[0],
center_coords_scaled[1],
arrow_end_coords_scaled[0],
arrow_end_coords_scaled[1],
arrow='last',
fill='white')
class DynamicGUI():
def __init__(self, master, fullMap, emptyMap, path, endPoint):
"""A class to represent a GUI with a map
Arguments:
master {Tk} -- Tkinter GUI generator
inputMap {grid} -- The grid to draw on
path {tile list} -- the path of grid tiles visited
FIELDS:
master {Tk} -- Tkinter GUI generator
tile_dict {dict} -- a dictionary that maps tiles to rectangles
canvas {Canvas} -- the canvas the GUI is made on
path {Tile list} -- the list of tiles visited by robot on path
pathSet {Tile Set} -- set of tiles that have already been drawn (So GUI
does not draw over the tiles)
pathIndex {int} -- the index of the path the GUI is at in anumation
curr_tile {Tile} -- the current tile the robot is at in animation
grid {Grid} -- the Grid object that the simulation was run on
"""
# Tinker master, used to create GUI
self.master = master
self.tile_dict = None
self.canvas = None
self.path = path
self.initPhase = True
self.brokenPath = None
self.brokenPathIndex = 0
self.visitedSet = set()
self.pathSet = set()
for i in self.path:
self.pathSet.add(i)
self.pathIndex = 0
self.curr_tile = None
self.gridFull = fullMap
self.gridEmpty = emptyMap
self.recalc = False
self.stepsSinceRecalc = 0
self.create_widgets()
self.generate_sensor = GenerateSensorData(self.gridFull)
self.endPoint = endPoint
self.next_tile = None
self.recalc_count = recalc_wait
self.recalc_cond = False
self.last_iter_seen = set(
) # set of tiles that were marked as available path in simulation's previous iteration
# TODO: This is a buggggg..... we must fix the entire coordinate system? change init heading to 0
self.heading = 180
# TODO: change to custom type or enum
self.output_state = "stopped"
self.desired_heading = None
self.angle_trace = None
self.des_angle_trace = None
self.prev_draw_c1c0_ids = [
None, None
] # previous IDs representing drawing of C1C0 on screen
def create_widgets(self, empty=True):
"""Creates the canvas of the size of the inputted grid
Create left side GUI with all obstacles visible
If empty=True, draw empty grid (where robot doesn't know its surroundings)
(function is run only at initialization of grid)
"""
self.master.geometry("+900+100")
if empty:
map = self.gridEmpty.grid
else:
map = self.gridFull.grid
width = len(map[0]) * GUI_tile_size
height = len(map) * GUI_tile_size
visMap = Canvas(self.master, width=width, height=height)
offset = GUI_tile_size / 2
if empty:
tile_dict = {}
for row in map:
for tile in row:
x = tile.x / tile_scale_fac
y = tile.y / tile_scale_fac
x1 = x - offset
y1 = y - offset
x2 = x + offset
y2 = y + offset
if tile.is_bloated:
color = "#ffc0cb"
elif tile.is_obstacle:
color = "#ffCC99"
else:
color = "#545454"
if empty:
tile_dict[tile] = visMap.create_rectangle(x1,
y1,
x2,
y2,
outline=color,
fill=color)
visMap.pack()
self.canvas = visMap
if (empty):
self.tile_dict = tile_dict
def visibilityDraw(self, lidar_data):
"""Draws a circle of visibility around the robot
"""
# coloring all tiles that were seen in last iteration light gray
while self.last_iter_seen:
curr_rec = self.last_iter_seen.pop()
self.canvas.itemconfig(curr_rec, outline="#C7C7C7",
fill="#C7C7C7") # light gray
row = self.curr_tile.row
col = self.curr_tile.col
index_radius_inner = int(vis_radius / tile_size)
index_radius_outer = index_radius_inner + 2
# the bounds for the visibility circle
lower_row = max(0, row - index_radius_outer)
lower_col = max(0, col - index_radius_outer)
upper_row = min(row + index_radius_outer, self.gridFull.num_rows - 1)
upper_col = min(col + index_radius_outer, self.gridFull.num_cols - 1)
lidar_data_copy = copy.copy(lidar_data)
rad_inc = int(GUI_tile_size / 3) # radius increment to traverse tiles
def _color_normally(r, angle_rad):
"""
Colors the tile at the location that is at a distance r at heading angle_rad from robot's current location.
Colors the tile based on its known attribute (obstacle, bloated, visited, or visible).
"""
coords = (r * math.sin(angle_rad) + row,
r * math.cos(angle_rad) + col
) # (row, col) of tile we want to color
# make sure coords are in bounds of GUI window
if (coords[0] >= lower_row) and (coords[0] <= upper_row) \
and (coords[1] >= lower_col) and (coords[1] <= upper_col):
curr_tile = self.gridEmpty.grid[int(coords[0])][int(coords[1])]
curr_rec = self.tile_dict[curr_tile]
if curr_tile.is_bloated:
if 11 > curr_tile.bloat_score > 0:
color = bloat_colors[curr_tile.bloat_score]
self.canvas.itemconfig(curr_rec,
outline=color,
fill=color) # blue
if curr_tile.bloat_score >= 11:
self.canvas.itemconfig(curr_rec,
outline="#000000",
fill="#000000") # black
elif curr_tile.is_obstacle:
# avg = math.ceil(sum(curr_tile.obstacle_score)/len(curr_tile.obstacle_score))
if curr_tile.obstacle_score[3] < 6:
color_pos = round(
len(obstacle_colors) / obstacle_threshold *
curr_tile.obstacle_score[3]) - 1
color = obstacle_colors[max(color_pos, 1)]
self.canvas.itemconfig(curr_rec,
outline=color,
fill=color) # yellow
else:
self.canvas.itemconfig(curr_rec,
outline="#cc8400",
fill="#cc8400") # darker yellow
else:
self.canvas.itemconfig(curr_rec,
outline="#fff",
fill="#fff") # white
self.last_iter_seen.add(curr_rec)
# iterating through 360 degrees surroundings of robot in increments of degree_freq
for deg in range(0, 360, degree_freq):
angle_rad = deg * math.pi / 180
if len(lidar_data_copy) == 0 or lidar_data_copy[0][0] != deg:
# no object in sight at deg; color everything normally up to visibility radius
for r in range(0, index_radius_inner, rad_inc):
_color_normally(r, angle_rad)
else: # obstacle in sight
# color everything normally UP TO obstacle, and color obstacle red
for r in range(
0,
math.ceil(lidar_data_copy[0][1] / tile_size) + rad_inc,
rad_inc):
_color_normally(r, angle_rad)
lidar_data_copy.pop(0)
def drawC1C0(self):
"""Draws C1C0's current location on the simulation"""
def _scale_coords(coords):
"""scales coords (a tuple (x, y)) from real life cm to pixels"""
scaled_x = coords[0] / tile_scale_fac
scaled_y = coords[1] / tile_scale_fac
return scaled_x, scaled_y
# coordinates of robot center right now (in cm)
center_x = self.curr_tile.x
center_y = self.curr_tile.y
# converting heading to radians, and adjusting so that facing right = 0 deg
heading_adj_rad = math.radians(self.heading + 90)
if self.prev_draw_c1c0_ids is not None:
# delete old drawings from previous iteration
self.canvas.delete(self.prev_draw_c1c0_ids[0])
self.canvas.delete(self.prev_draw_c1c0_ids[1])
# coordinates of bounding square around blue circle
top_left_coords = (center_x - robot_radius, center_y + robot_radius)
bot_right_coords = (center_x + robot_radius, center_y - robot_radius)
# convert coordinates from cm to pixels
top_left_coords_scaled = _scale_coords(top_left_coords)
bot_right_coords_scaled = _scale_coords(bot_right_coords)
# draw blue circle
self.prev_draw_c1c0_ids[0] = self.canvas.create_oval(
top_left_coords_scaled[0],
top_left_coords_scaled[1],
bot_right_coords_scaled[0],
bot_right_coords_scaled[1],
outline='black',
fill='blue')
center_coords_scaled = _scale_coords((center_x, center_y))
# finding endpoint coords of arrow
arrow_end_x = center_x + robot_radius * math.cos(heading_adj_rad)
arrow_end_y = center_y + robot_radius * math.sin(heading_adj_rad)
arrow_end_coords_scaled = _scale_coords((arrow_end_x, arrow_end_y))
# draw white arrow
self.prev_draw_c1c0_ids[1] = self.canvas.create_line(
center_coords_scaled[0],
center_coords_scaled[1],
arrow_end_coords_scaled[0],
arrow_end_coords_scaled[1],
arrow='last',
fill='white')
def breakUpLine(self, curr_tile, next_tile):
current_loc = (curr_tile.x, curr_tile.y)
next_loc = (next_tile.x, next_tile.y)
# calculate the slope, rise/run
x_change = next_loc[0] - current_loc[0]
y_change = next_loc[1] - current_loc[1]
dist = math.sqrt(x_change**2 + y_change**2)
# if (dist < tile_size):
# return [(current_loc[0] + x_change, current_loc[1] + y_change)]
num_steps = int(dist / tile_size)
returner = []
if y_change == 0:
x_step = tile_size
y_step = 0
elif x_change == 0:
x_step = 0
y_step = tile_size
else:
inv_slope = x_change / y_change
# x^2+y^2 = tile_size^2 && x/y = x_change/y_change
y_step = math.sqrt(tile_size**2 / (inv_slope**2 + 1))
y_step = y_step
x_step = math.sqrt(
(tile_size**2 * inv_slope**2) / (inv_slope**2 + 1))
x_step = x_step
if x_change < 0 and x_step > 0:
x_step = -x_step
if y_change < 0 and y_step:
y_step = -y_step
for i in range(1, num_steps + 1):
new_coor = (current_loc[0] + i * x_step,
current_loc[1] + i * y_step)
returner.append(new_coor)
new_tile = self.gridEmpty.get_tile(new_coor)
if new_tile not in self.pathSet:
self.pathSet.add(new_tile)
return returner
def getPathSet(self):
"""
"""
prev_tile = self.curr_tile
for next_tile in self.path:
if next_tile not in self.pathSet:
self.pathSet.add(next_tile)
self.breakUpLine(prev_tile, next_tile)
prev_tile = next_tile
def printTiles(self):
for row in self.gridEmpty.grid:
for col in row:
print(str(col.x) + ', ' + str(col.y))
def checkRecalc(self):
for x, y in self.brokenPath:
check_tile = self.gridEmpty.get_tile((x, y))
if check_tile.is_obstacle or check_tile.is_bloated:
self.recalc = True
def updateDesiredHeading(self):
"""
calculates the degrees between the current tile and the next tile and updates desired_heading. Estimates the
degrees to the nearing int.
"""
x_change = self.next_tile.x - self.curr_x
y_change = self.next_tile.y - self.curr_y
if y_change == 0:
arctan = 90 if x_change < 0 else -90
else:
arctan = math.atan(x_change / y_change) * (180 / math.pi)
if x_change >= 0 and y_change > 0:
self.desired_heading = (360 - arctan) % 360
elif x_change < 0 and y_change > 0:
self.desired_heading = -arctan
else:
self.desired_heading = 180 - arctan
self.desired_heading = round(self.desired_heading)
# print("updated desired heading to : " + str(self.desired_heading))
def get_direction_coor(self, curr_x, curr_y, angle):
"""
returns a coordinate on on the visibility radius at angle [angle] from the robot
"""
x2 = math.cos(math.radians(angle + 90)) * vis_radius + curr_x
y2 = math.sin(math.radians(angle + 90)) * vis_radius + curr_y
return (x2 / tile_scale_fac, y2 / tile_scale_fac)
def draw_line(self, curr_x, curr_y, next_x, next_y):
"""
Draw a line from the coordinate (curr_x, curr_y) to the coordinate (next_x, next_y)
"""
self.canvas.create_line(curr_x / tile_scale_fac,
curr_y / tile_scale_fac,
next_x / tile_scale_fac,
next_y / tile_scale_fac,
fill="#339933",
width=1.5)
def draw_headings(self):
"""
Draws a line showing the heading and desired heading of the robot
"""
self.canvas.delete(self.angle_trace)
self.canvas.delete(self.des_angle_trace)
line_coor = self.get_direction_coor(self.curr_tile.x, self.curr_tile.y,
self.heading)
self.angle_trace = self.canvas.create_line(
self.curr_tile.x / tile_scale_fac,
self.curr_tile.y / tile_scale_fac,
line_coor[0],
line_coor[1],
fill='#FF69B4',
width=1.5)
des_line_coor = self.get_direction_coor(self.curr_tile.x,
self.curr_tile.y,
self.desired_heading)
self.des_angle_trace = self.canvas.create_line(
self.curr_tile.x / tile_scale_fac,
self.curr_y / tile_scale_fac,
des_line_coor[0],
des_line_coor[1],
fill='#FF0000',
width=1.5)
self.canvas.pack()
def updateGridSmoothed(self):
"""
updates the grid in a smoothed fashion
"""
try:
if self.desired_heading is not None and self.heading == self.desired_heading:
self.drawC1C0()
self.output_state = "move forward"
print(self.output_state)
if self.desired_heading is not None and self.heading != self.desired_heading:
self.drawC1C0()
if self.heading < self.desired_heading:
cw_turn_degrees = 360 + self.heading - self.desired_heading
ccw_turn_degrees = self.desired_heading - self.heading
else:
cw_turn_degrees = self.heading - self.desired_heading
ccw_turn_degrees = 360 - self.heading + self.desired_heading
if abs(self.desired_heading - self.heading) < turn_speed:
self.heading = self.desired_heading
else:
if cw_turn_degrees < ccw_turn_degrees: # turn clockwise
self.heading = self.heading - turn_speed
print('turn left')
self.output_state = "turn right"
else: # turn counter clockwise
self.heading = self.heading + turn_speed
print('turn right')
self.output_state = "turn left"
if self.heading < 0:
self.heading = 360 + self.heading
elif self.heading >= 360:
self.heading = self.heading - 360
self.master.after(speed_dynamic, self.updateGridSmoothed)
elif self.initPhase:
curr_tile = self.path[0]
self.curr_tile = curr_tile
self.curr_x = self.curr_tile.x
self.curr_y = self.curr_tile.y
self.visitedSet.add(curr_tile)
self.getPathSet()
lidar_data = self.generate_sensor.generateLidar(
degree_freq, curr_tile.row, curr_tile.col)
self.getPathSet()
self.recalc = self.gridEmpty.update_grid_tup_data(
curr_tile.x, curr_tile.y, lidar_data, Tile.lidar,
robot_radius, bloat_factor, self.pathSet)
self.next_tile = self.path[1]
self.brokenPath = self.breakUpLine(self.curr_tile,
self.next_tile)
self.getPathSet()
self.brokenPathIndex = 0
self.visibilityDraw(lidar_data)
self.updateDesiredHeading()
self.initPhase = False
self.master.after(speed_dynamic, self.updateGridSmoothed)
# If we need to iterate through a brokenPath
elif self.brokenPathIndex < len(self.brokenPath):
lidar_data = self.generate_sensor.generateLidar(
degree_freq, self.curr_tile.row, self.curr_tile.col)
self.recalc = self.gridEmpty.update_grid_tup_data(
self.curr_tile.x, self.curr_tile.y, lidar_data, Tile.lidar,
robot_radius, bloat_factor, self.pathSet)
self.recalc_cond = self.recalc_cond or self.recalc
# Relcalculate the path if needed
if self.recalc_cond and self.recalc_count >= recalc_wait:
# print('recalculating!')
self.path = search.a_star_search(
self.gridEmpty, (self.curr_tile.x, self.curr_tile.y),
self.endPoint, search.euclidean)
self.path = search.segment_path(self.gridEmpty, self.path)
self.pathIndex = 0
self.smoothed_window.path = self.path
self.smoothed_window.drawPath()
self.draw_line(self.curr_x, self.curr_y, self.path[0].x,
self.path[0].y)
self.curr_tile = self.path[0]
self.curr_x = self.curr_tile.x
self.curr_y = self.curr_tile.y
self.next_tile = self.path[1]
self.brokenPath = self.breakUpLine(self.curr_tile,
self.next_tile)
self.updateDesiredHeading()
self.getPathSet()
self.visibilityDraw(lidar_data)
self.pathSet = set()
self.getPathSet()
self.pathIndex = 0
self.brokenPathIndex = 0
self.recalc = False
self.recalc_count = 0
self.recalc_cond = False
else:
# print('not')
if self.brokenPathIndex == 0:
x1 = self.curr_x
y1 = self.curr_y
else:
x1 = self.brokenPath[self.brokenPathIndex - 1][0]
y1 = self.brokenPath[self.brokenPathIndex - 1][1]
x2 = self.brokenPath[self.brokenPathIndex][0]
y2 = self.brokenPath[self.brokenPathIndex][1]
future_tile = self.gridEmpty.get_tile((x2, y2))
if future_tile.is_obstacle or future_tile.is_bloated:
self.recalc_count = recalc_wait
else:
self.draw_line(x1, y1, x2, y2)
self.curr_x = x2
self.curr_y = y2
self.curr_tile = future_tile
self.visitedSet.add(self.curr_tile)
self.visibilityDraw(lidar_data)
self.drawC1C0()
self.brokenPathIndex += 1
self.recalc_count += 1
# MAYBE CHANGE WIDTH TO SEE IF IT LOOKS BETTER?
self.master.after(speed_dynamic, self.updateGridSmoothed)
# If we have finished iterating through a broken path, we need to go to the
# Next tile in path, and create a new broken path to iterate through
else:
lidar_data = self.generate_sensor.generateLidar(
degree_freq, self.curr_tile.row, self.curr_tile.col)
if self.curr_tile == self.gridEmpty.get_tile(self.endPoint):
print(
'C1C0 has ARRIVED AT THE DESTINATION, destination tile'
)
return
self.pathSet = set()
self.pathIndex += 1
if self.pathIndex == len(self.path) - 1:
print(
'C1C0 has ARRIVED AT THE DESTINATION, destination tile'
)
return
self.draw_line(self.curr_x, self.curr_y,
self.path[self.pathIndex].x,
self.path[self.pathIndex].y)
self.curr_tile = self.path[self.pathIndex]
self.curr_x = self.curr_tile.x
self.curr_y = self.curr_tile.y
self.next_tile = self.path[self.pathIndex + 1]
self.brokenPath = self.breakUpLine(self.curr_tile,
self.next_tile)
self.getPathSet()
self.brokenPathIndex = 0
self.visibilityDraw(lidar_data)
self.updateDesiredHeading()
self.master.after(speed_dynamic, self.updateGridSmoothed)
except Exception as e:
print(e)
print(
"C1C0: \"There is no path to the desired location. Beep Boop\""
)
def runSimulation(self):
"""Runs a sumulation of this map, with its enviroment and path
"""
self.smoothed_window = StaticGUI.SmoothedPathGUI(
Toplevel(), self.gridFull, self.path)
self.smoothed_window.drawPath()
self.updateGridSmoothed()
self.master.mainloop()
class DynamicGUI():
def __init__(self, master, fullMap, emptyMap, path, endPoint):
"""A class to represent a GUI with a map
Arguments:
master {Tk} -- Tkinter GUI generator
inputMap {grid} -- The grid to draw on
path {tile list} -- the path of grid tiles visited
FIELDS:
master {Tk} -- Tkinter GUI generator
tile_dict {dict} -- a dictionary that maps tiles to rectangles
canvas {Canvas} -- the canvas the GUI is made on
path {Tile list} -- the list of tiles visited by robot on path
pathSet {Tile Set} -- set of tiles that have already been drawn (So GUI
does not draw over the tiles)
pathIndex {int} -- the index of the path the GUI is at in anumation
curr_tile {Tile} -- the current tile the robot is at in animation
grid {Grid} -- the Grid object that the simulation was run on
"""
# Tinker master, used to create GUI
self.master = master
self.tile_dict = None
self.canvas = None
self.path = path
self.brokenPath = None
self.brokenPathIndex = 0
self.visitedSet = set()
self.pathSet = set()
for i in self.path:
self.pathSet.add(i)
self.pathIndex = 0
self.curr_tile = None
self.prev_tile = None
self.gridFull = fullMap
self.gridEmpty = emptyMap
self.recalc = False
self.create_widgets()
self.generate_sensor = GenerateSensorData(self.gridFull)
self.endPoint = endPoint
self.next_tile = None
self.prev_vector = None
self.recalc_count = recalc_wait
self.recalc_cond = False
self.way_point = None
self.last_iter_seen = set(
) # set of tiles that were marked as available path in simulation's previous iteration
# TODO: This is a buggggg..... we must fix the entire coordinate system? change init heading to 0
self.heading = 180
# TODO: change to custom type or enum
self.output_state = "stopped"
self.desired_heading = None
self.angle_trace = None
self.des_angle_trace = None
self.oldError = 0
self.errorHistory = 0
self.mean = random.randint(-1, 1) / 12.0
self.standard_deviation = random.randint(0, 1.0) / 10.0
self.pid = None
self.counter = 0
self.prev_draw_c1c0_ids = [
None, None
] # previous IDs representing drawing of C1C0 on screen
def create_widgets(self, empty=True):
"""Creates the canvas of the size of the inputted grid
Create left side GUI with all obstacles visible
If empty=True, draw empty grid (where robot doesn't know its surroundings)
(function is run only at initialization of grid)
"""
self.master.geometry("+900+100")
if empty:
map = self.gridEmpty.grid
else:
map = self.gridFull.grid
width = len(map[0]) * GUI_tile_size
height = len(map) * GUI_tile_size
visMap = Canvas(self.master, width=width, height=height)
offset = GUI_tile_size / 2
if empty:
tile_dict = {}
for row in map:
for tile in row:
x = tile.x / tile_scale_fac
y = tile.y / tile_scale_fac
x1 = x - offset
y1 = y - offset
x2 = x + offset
y2 = y + offset
if tile.is_bloated:
color = "#ffc0cb"
elif tile.is_obstacle:
color = "#ffCC99"
else:
color = "#545454"
if empty:
tile_dict[tile] = visMap.create_rectangle(x1,
y1,
x2,
y2,
outline=color,
fill=color)
visMap.pack()
self.canvas = visMap
if (empty):
self.tile_dict = tile_dict
def calcVector(self):
"""
Returns the vector between the current location and the end point of the current line segment
and draws this vector onto the canvas
"""
vect = (0, 0)
if self.pathIndex + 1 < len(self.path):
vect = self.pid.newVec()
# print(vect)
if self.prev_vector is not None:
# delete old drawings from previous iteration
self.canvas.delete(self.prev_vector)
mag = (vect[0]**2 + vect[1]**2)**(1 / 2)
norm_vect = (int(vector_draw_length * (vect[0] / mag)),
int(vector_draw_length * (vect[1] / mag)))
end = self._scale_coords(
(self.curr_x + norm_vect[0], self.curr_y + norm_vect[1]))
start = self._scale_coords((self.curr_x, self.curr_y))
self.prev_vector = self.canvas.create_line(start[0],
start[1],
end[0],
end[1],
arrow='last',
fill='red')
# self.canvas.tag_raise(self.prev_vector)
return vect
def visibilityDraw(self, lidar_data):
"""Draws a circle of visibility around the robot
"""
# coloring all tiles that were seen in last iteration light gray
while self.last_iter_seen:
curr_rec = self.last_iter_seen.pop()
self.canvas.itemconfig(curr_rec, outline="#C7C7C7",
fill="#C7C7C7") # light gray
row = self.curr_tile.row
col = self.curr_tile.col
index_radius_inner = int(vis_radius / tile_size)
index_radius_outer = index_radius_inner + 2
# the bounds for the visibility circle
lower_row = max(0, row - index_radius_outer)
lower_col = max(0, col - index_radius_outer)
upper_row = min(row + index_radius_outer, self.gridFull.num_rows - 1)
upper_col = min(col + index_radius_outer, self.gridFull.num_cols - 1)
lidar_data_copy = copy.copy(lidar_data)
rad_inc = int(GUI_tile_size / 3) # radius increment to traverse tiles
def _color_normally(r, angle_rad):
"""
Colors the tile at the location that is at a distance r at heading angle_rad from robot's current location.
Colors the tile based on its known attribute (obstacle, bloated, visited, or visible).
"""
coords = (r * math.sin(angle_rad) + row,
r * math.cos(angle_rad) + col
) # (row, col) of tile we want to color
# make sure coords are in bounds of GUI window
if (coords[0] >= lower_row) and (coords[0] <= upper_row) \
and (coords[1] >= lower_col) and (coords[1] <= upper_col):
curr_tile = self.gridEmpty.grid[int(coords[0])][int(coords[1])]
curr_rec = self.tile_dict[curr_tile]
if curr_tile.is_bloated:
if 11 > curr_tile.bloat_score > 0:
color = bloat_colors[curr_tile.bloat_score]
self.canvas.itemconfig(curr_rec,
outline=color,
fill=color) # blue
if curr_tile.bloat_score >= 11:
self.canvas.itemconfig(curr_rec,
outline="#000000",
fill="#000000") # black
elif curr_tile.is_obstacle:
# avg = math.ceil(sum(curr_tile.obstacle_score)/len(curr_tile.obstacle_score))
if curr_tile.obstacle_score[3] < 6:
color_pos = round(
len(obstacle_colors) / obstacle_threshold *
curr_tile.obstacle_score[3]) - 1
color = obstacle_colors[max(color_pos, 1)]
self.canvas.itemconfig(curr_rec,
outline=color,
fill=color) # yellow
else:
self.canvas.itemconfig(curr_rec,
outline="#cc8400",
fill="#cc8400") # darker yellow
else:
self.canvas.itemconfig(curr_rec,
outline="#fff",
fill="#fff") # white
self.last_iter_seen.add(curr_rec)
# iterating through 360 degrees surroundings of robot in increments of degree_freq
for deg in range(0, 360, degree_freq):
angle_rad = deg * math.pi / 180
if len(lidar_data_copy) == 0 or lidar_data_copy[0][0] != deg:
# no object in sight at deg; color everything normally up to visibility radius
for r in range(0, index_radius_inner, rad_inc):
_color_normally(r, angle_rad)
else: # obstacle in sight
# color everything normally UP TO obstacle, and color obstacle red
for r in range(
0,
math.ceil(lidar_data_copy[0][1] / tile_size) + rad_inc,
rad_inc):
_color_normally(r, angle_rad)
lidar_data_copy.pop(0)
def _scale_coords(self, coords):
"""scales coords (a tuple (x, y)) from real life cm to pixels"""
scaled_x = coords[0] / tile_scale_fac
scaled_y = coords[1] / tile_scale_fac
return scaled_x, scaled_y
def drawC1C0(self):
"""Draws C1C0's current location on the simulation"""
# coordinates of robot center right now (in cm)
center_x = self.curr_x
center_y = self.curr_y
# converting heading to radians, and adjusting so that facing right = 0 deg
heading_adj_rad = math.radians(self.heading + 90)
if self.prev_draw_c1c0_ids is not None:
# delete old drawings from previous iteration
self.canvas.delete(self.prev_draw_c1c0_ids[0])
self.canvas.delete(self.prev_draw_c1c0_ids[1])
# coordinates of bounding square around blue circle
top_left_coords = (center_x - robot_radius, center_y + robot_radius)
bot_right_coords = (center_x + robot_radius, center_y - robot_radius)
# convert coordinates from cm to pixels
top_left_coords_scaled = self._scale_coords(top_left_coords)
bot_right_coords_scaled = self._scale_coords(bot_right_coords)
# draw blue circle
self.prev_draw_c1c0_ids[0] = self.canvas.create_oval(
top_left_coords_scaled[0],
top_left_coords_scaled[1],
bot_right_coords_scaled[0],
bot_right_coords_scaled[1],
outline='black',
fill='blue')
center_coords_scaled = self._scale_coords((center_x, center_y))
# finding endpoint coords of arrow
arrow_end_x = center_x + robot_radius * math.cos(heading_adj_rad)
arrow_end_y = center_y + robot_radius * math.sin(heading_adj_rad)
arrow_end_coords_scaled = self._scale_coords(
(arrow_end_x, arrow_end_y))
# draw white arrow
self.prev_draw_c1c0_ids[1] = self.canvas.create_line(
center_coords_scaled[0],
center_coords_scaled[1],
arrow_end_coords_scaled[0],
arrow_end_coords_scaled[1],
arrow='last',
fill='white')
def generatePathSet(self):
self.pathSet = set()
for i in range(len(self.path) - 1):
self.breakUpLine(self.path[i], self.path[i + 1])
def breakUpLine(self, curr_tile, next_tile):
current_loc = (curr_tile.x, curr_tile.y)
next_loc = (next_tile.x, next_tile.y)
# calculate the slope, rise/run
x_change = next_loc[0] - current_loc[0]
y_change = next_loc[1] - current_loc[1]
dist = math.sqrt(x_change**2 + y_change**2)
# if (dist < tile_size):
# return [(current_loc[0] + x_change, current_loc[1] + y_change)]
num_steps = int(dist / tile_size)
returner = []
if y_change == 0:
x_step = tile_size
y_step = 0
elif x_change == 0:
x_step = 0
y_step = tile_size
else:
inv_slope = x_change / y_change
# x^2+y^2 = tile_size^2 && x/y = x_change/y_change
y_step = math.sqrt(tile_size**2 / (inv_slope**2 + 1))
y_step = y_step
x_step = math.sqrt(
(tile_size**2 * inv_slope**2) / (inv_slope**2 + 1))
x_step = x_step
if x_change < 0 and x_step > 0:
x_step = -x_step
if y_change < 0 and y_step:
y_step = -y_step
for i in range(1, num_steps + 1):
new_coor = (current_loc[0] + i * x_step,
current_loc[1] + i * y_step)
returner.append(new_coor)
new_tile = self.gridEmpty.get_tile(new_coor)
if new_tile not in self.pathSet:
self.pathSet.add(new_tile)
def printTiles(self):
for row in self.gridEmpty.grid:
for col in row:
print(str(col.x) + ', ' + str(col.y))
def updateDesiredHeading(self, next_x=None, next_y=None):
"""
calculates the degrees between the current tile and the next tile and updates desired_heading. Estimates the
degrees to the nearing int.
"""
next_x = self.next_tile.x if next_x is None else next_x
next_y = self.next_tile.y if next_y is None else next_y
x_change = next_x - self.curr_x
y_change = next_y - self.curr_y
if y_change == 0:
arctan = 90 if x_change < 0 else -90
else:
arctan = math.atan(x_change / y_change) * (180 / math.pi)
if x_change >= 0 and y_change > 0:
self.desired_heading = (360 - arctan) % 360
elif x_change < 0 and y_change > 0:
self.desired_heading = -arctan
else:
self.desired_heading = 180 - arctan
self.desired_heading = round(self.desired_heading)
# print("updated desired heading to : " + str(self.desired_heading))
def draw_line(self, curr_x, curr_y, next_x, next_y):
"""
Draw a line from the coordinate (curr_x, curr_y) to the coordinate (next_x, next_y)
"""
self.canvas.create_line(curr_x / tile_scale_fac,
curr_y / tile_scale_fac,
next_x / tile_scale_fac,
next_y / tile_scale_fac,
fill="#339933",
width=1.5)
def init_phase(self):
curr_tile = self.path[0]
self.prev_tile = self.curr_tile
self.curr_tile = curr_tile
self.curr_x = self.curr_tile.x
self.curr_y = self.curr_tile.y
self.prev_x = self.curr_tile.x
self.prev_y = self.curr_tile.y
self.visitedSet.add(curr_tile)
self.generatePathSet()
lidar_data = self.generate_sensor.generateLidar(
degree_freq, curr_tile.row, curr_tile.col)
self.generatePathSet()
self.recalc = self.gridEmpty.update_grid_tup_data(
curr_tile.x, curr_tile.y, lidar_data, Tile.lidar, robot_radius,
bloat_factor, self.pathSet)
self.next_tile = self.path[1]
self.visibilityDraw(lidar_data)
self.updateDesiredHeading()
self.pid = PID(self.path, self.pathIndex + 1, self.curr_x, self.curr_y)
self.master.after(fast_speed_dynamic, self.main_loop)
def turn(self):
while self.desired_heading is not None and self.heading != self.desired_heading:
self.drawC1C0()
if self.heading < self.desired_heading:
cw_turn_degrees = 360 + self.heading - self.desired_heading
ccw_turn_degrees = self.desired_heading - self.heading
else:
cw_turn_degrees = self.heading - self.desired_heading
ccw_turn_degrees = 360 - self.heading + self.desired_heading
if abs(self.desired_heading - self.heading) < turn_speed:
self.heading = self.desired_heading
else:
if cw_turn_degrees < ccw_turn_degrees: # turn clockwise
self.heading = self.heading - turn_speed
self.output_state = "turn right"
else: # turn counter clockwise
self.heading = self.heading + turn_speed
self.output_state = "turn left"
if self.heading < 0:
self.heading = 360 + self.heading
elif self.heading >= 360:
self.heading = self.heading - 360
def recalculate_path(self, lidar_data):
self.path = search.a_star_search(self.gridEmpty,
(self.curr_x, self.curr_y),
self.endPoint, search.euclidean)
self.path = search.segment_path(self.gridEmpty, self.path)
self.pathIndex = 0
self.smoothed_window.path = self.path
self.smoothed_window.drawPath()
self.draw_line(self.curr_x, self.curr_y, self.path[0].x,
self.path[0].y)
self.prev_tile = self.curr_tile
self.curr_tile = self.path[0]
self.curr_x = self.curr_tile.x
self.curr_y = self.curr_tile.y
self.next_tile = self.path[1]
self.updateDesiredHeading()
self.generatePathSet()
self.visibilityDraw(lidar_data)
self.pathIndex = 0
self.recalc = False
self.recalc_count = 0
self.recalc_cond = False
self.drawWayPoint(self.next_tile)
self.pid = PID(self.path, self.pathIndex + 1, self.curr_x, self.curr_y)
def get_next_pos(self, vec, lidar_data):
mag = math.sqrt(vec[0]**2 + vec[1]**2)
error = np.random.normal(self.mean, self.standard_deviation)
# error = 0
norm_vec = (20 * vec[0] / mag, 20 * vec[1] / mag)
check_tile = self.gridEmpty.get_tile(
(self.curr_x + norm_vec[0], self.curr_y + norm_vec[1]))
if check_tile.is_obstacle and not check_tile.is_bloated:
print('had to recalculate :(')
self.recalculate_path(lidar_data)
norm_vec = (norm_vec[0] * math.cos(error) -
norm_vec[1] * math.sin(error),
norm_vec[0] * math.sin(error) +
norm_vec[1] * math.cos(error))
x2 = self.curr_x + norm_vec[0]
y2 = self.curr_y + norm_vec[1]
return x2, y2
def step(self, lidar_data, vec=None):
"""
steps in the direction of the vector
"""
if vec is None:
vec = self.calcVector()
x2, y2 = self.get_next_pos(vec, lidar_data)
self.draw_line(self.curr_x, self.curr_y, x2, y2)
self.prev_x = self.curr_x
self.prev_y = self.curr_y
self.curr_x = x2
self.curr_y = y2
self.pid.update_PID(self.curr_x, self.curr_y)
self.prev_tile = self.curr_tile
self.curr_tile = self.gridEmpty.get_tile((x2, y2))
self.visitedSet.add(self.curr_tile)
self.visibilityDraw(lidar_data)
self.drawC1C0()
self.recalc_count += 1
def emergency_step(self, lidar_data):
"""
Find the nearest obstacle to the
"""
min_obs_dist = float('inf')
min_obs: Tile = None
max_dist = int(bloat_factor * robot_radius)
step_dist = int(tile_size / 2)
for ang_deg in range(0, 360, 5):
for dist in range(0, max_dist, step_dist):
ang_rad = math.radians(ang_deg)
x2 = self.curr_x + dist * math.cos(math.radians(ang_rad))
y2 = self.curr_y + dist * math.cos(math.radians(ang_rad))
curr_tile = self.gridEmpty.get_tile((x2, y2))
if curr_tile.is_obstacle and not curr_tile.is_bloated:
break
if dist < min_obs_dist:
min_obs_dist = dist
min_obs = curr_tile
vec = (-self.curr_x + min_obs.x, -self.curr_y + min_obs.y)
x2, y2 = self.get_next_pos(vec, lidar_data)
self.updateDesiredHeading(x2, y2)
self.step(lidar_data, vec)
def main_loop(self):
"""
updates the grid in a smoothed fashion
"""
if self.curr_tile == self.gridEmpty.get_tile(self.endPoint):
print('C1C0 has ARRIVED AT THE DESTINATION, destination tile')
return
lidar_data = self.generate_sensor.generateLidar(
degree_freq, self.curr_tile.row, self.curr_tile.col)
self.recalc = self.gridEmpty.update_grid_tup_data(
self.curr_x, self.curr_y, lidar_data, Tile.lidar, robot_radius,
bloat_factor, self.pathSet)
self.turn()
self.recalc_cond = self.recalc_cond or self.recalc
# if (self.curr_tile.is_obstacle and self.curr_tile.bloat_score > 6) or \
# (self.curr_tile.is_obstacle and not self.curr_tile.is_bloated):
# self.emergency_step(lidar_data)
# self.recalc_cond = True
# else:
# if we need to recalculate then recurse
if self.recalc_cond and self.recalc_count >= recalc_wait:
try:
self.recalculate_path(lidar_data)
except Exception as e:
print('error occured, ', e)
self.emergency_step(lidar_data)
self.recalc_cond = True
elif self.nextLoc():
self.mean = random.randint(-1, 1) / 12.0
self.standard_deviation = random.randint(0, 1) / 10.0
self.pathIndex += 1
self.next_tile = self.path[self.pathIndex]
self.drawWayPoint(self.next_tile)
self.updateDesiredHeading()
self.pid = PID(self.path, self.pathIndex + 1, self.curr_x,
self.curr_y)
else:
#print(self.path[self.pathIndex].row, self.path[self.pathIndex].col)
#print(self.path[self.pathIndex+1].row, self.path[self.pathIndex+1].col)
#print(self.curr_tile.is_bloated)
self.step(lidar_data)
self.drawC1C0()
self.master.after(fast_speed_dynamic, self.main_loop)
def nextLoc(self):
# (xp−xc)2+(yp−yc)2 with r2. (xp−xc)2+(yp−yc)2 with r2.
d = math.sqrt((self.curr_x - self.next_tile.x)**2 +
(self.curr_y - self.next_tile.y)**2)
# print(d)
return d <= reached_tile_bound
def runSimulation(self):
"""Runs a sumulation of this map, with its enviroment and path
"""
self.smoothed_window = StaticGUI.SmoothedPathGUI(
Toplevel(), self.gridFull, self.path)
self.smoothed_window.drawPath()
self.init_phase()
self.master.mainloop()
def drawWayPoint(self, new_tile):
if self.way_point is not None:
self.canvas.delete(self.way_point)
offset = GUI_tile_size
x = new_tile.x / tile_scale_fac
y = new_tile.y / tile_scale_fac
self.way_point = self.canvas.create_oval(x - offset,
y - offset,
x + offset,
y + offset,
outline="#FF0000",
fill="#FF0000")
class MapPathGUI():
def __init__(self, master, inputMap, path):
"""A class to represent a GUI with a map
Arguments:
master {Tk} -- Tkinter GUI generator
inputMap {grid} -- The grid to draw on
path {tile list} -- the path of grid tiles visited
FIELDS:
master {Tk} -- Tkinter GUI generator
tile_dict {dict} -- a dictionary that maps tiles to rectangles
canvas {Canvas} -- the canvas the GUI is made on
path {Tile list} -- the list of tiles visited by robot on path
pathSet {Tile Set} -- set of tiles that have already been drawn (So GUI
does not draw over the tiles)
pathIndex {int} -- the index of the path the GUI is at in anumation
curr_tile {Tile} -- the current tile the robot is at in animation
grid {Grid} -- the Grid object that the simulation was run on
"""
# Tinker master, used to create GUI
self.master = master
self.tile_dict = None
self.canvas = None
self.path = path
self.pathSet = set()
self.pathIndex = len(path) - 1
self.curr_tile = None
self.grid = inputMap
self.create_widgets()
self.generate_sensor = GenerateSensorData(self.grid)
def create_widgets(self):
"""Creates the canvas of the size of the inputted grid
"""
map = self.grid.grid
width = len(map[0]) * GUI_tile_size
height = len(map) * GUI_tile_size
visMap = Canvas(self.master, width=width, height=height)
offset = GUI_tile_size / 2
tile_dict = {}
for row in map:
for tile in row:
x = tile.x / tile_scale_fac
y = tile.y / tile_scale_fac
x1 = x - offset
y1 = y - offset
x2 = x + offset
y2 = y + offset
if (tile.is_bloated):
color = "#ffc0cb"
elif (tile.is_obstacle):
color = "#ffCC99"
else:
color = "#545454"
tile_dict[tile] = visMap.create_rectangle(x1,
y1,
x2,
y2,
outline=color,
fill=color)
visMap.pack()
self.canvas = visMap
self.tile_dict = tile_dict
def visibilityDraw(self):
"""Draws a circle of visibility around the robot
"""
index_radius_inner = int(vis_radius / tile_size)
index_rad_outer = index_radius_inner + 2
row = self.curr_tile.row
col = self.curr_tile.col
lower_row = int(max(0, row - index_rad_outer))
lower_col = int(max(0, col - index_rad_outer))
upper_row = int(min(row + index_rad_outer, self.grid.num_rows))
upper_col = int(min(col + index_rad_outer, self.grid.num_cols))
for i in range(lower_row, upper_row):
for j in range(lower_col, upper_col):
curr_tile = self.grid.grid[i][j]
curr_rec = self.tile_dict[curr_tile]
x_dist = abs(i - row)
y_dist = abs(j - col)
dist = math.sqrt(x_dist * x_dist + y_dist * y_dist)
if (dist < index_radius_inner):
if (curr_tile.is_obstacle and curr_tile.is_bloated):
self.canvas.itemconfig(curr_rec,
outline="#ffc0cb",
fill="#ffc0cb")
elif (curr_tile.is_obstacle and not curr_tile.is_bloated):
self.canvas.itemconfig(curr_rec,
outline="#ff621f",
fill="#ff621f")
elif (curr_tile.is_obstacle):
self.canvas.itemconfig(curr_rec,
outline="#ffCC99",
fill="#ffCC99")
elif (curr_tile not in self.pathSet):
self.canvas.itemconfig(curr_rec,
outline="#fff",
fill="#fff")
else:
if (curr_tile.is_obstacle == False
and curr_tile not in self.pathSet):
self.canvas.itemconfig(curr_rec,
outline="#545454",
fill="#545454")
def updateGrid(self):
"""USED TO ANIMATE THE SIMULATION
Update function that is continuously called using the
master.after command, any code before that will automatically
run at every iteration, according to global variable, speed.
"""
if (self.pathIndex != -1):
curr_tile = self.path[self.pathIndex]
curr_rec = self.tile_dict[curr_tile]
self.curr_tile = curr_tile
self.pathSet.add(curr_tile)
lidar_data = self.generate_sensor.generateLidar(
10, curr_tile.row, curr_tile.col)
self.visibilityDraw()
self.canvas.itemconfig(curr_rec, outline="#339933", fill="#339933")
self.pathIndex = self.pathIndex - 1
self.master.after(speed_static, self.updateGrid)
def runSimulation(self, smoothPath):
"""Runs a sumulation of this map, with its enviroment and path
"""
if smoothPath:
segmented_path = search.segment_path(self.grid, self.path, 0.01)
# print([(tile.x, tile.y) for tile in segmented_path])
top = Toplevel()
smoothed_window = SmoothedPathGUI(top, self.grid, segmented_path)
smoothed_window.drawPath()
self.updateGrid()
self.master.mainloop()
class MovingObGUI(GUI):
def __init__(self, master, fullMap, emptyMap, path, endPoint, squareList,
state):
"""A class to represent a GUI with a map
Arguments:
master {Tk} -- Tkinter GUI generator
inputMap {grid} -- The grid to draw on
path {tile list} -- the path of grid tiles visited
squareList -- the list of all the square objects
FIELDS:
master {Tk} -- Tkinter GUI generator
tile_dict {dict} -- a dictionary that maps tiles to rectangles
canvas {Canvas} -- the canvas the GUI is made on
path {Tile list} -- the list of tiles visited by robot on path
pathSet {Tile Set} -- set of tiles that have already been drawn (So GUI
does not draw over the tiles)
pathIndex {int} -- the index of the path the GUI is at in anumation
curr_tile {Tile} -- the current tile the robot is at in animation
grid {Grid} -- the Grid object that the simulation was run on
squareList -- the list of all the square objects
"""
# Tinker master, used to create GUI
super().__init__(master, fullMap, emptyMap, path, squareList, 0)
self.initPhase = True
self.brokenPath = None
self.brokenPathIndex = 0
self.visitedSet = set()
for i in self.path:
self.pathSet.add(i)
self.recalc_count = recalc_wait
self.recalc_cond = False
self.stepsSinceRecalc = 0
self.create_widgets(True)
self.generate_sensor = GenerateSensorData(self.gridFull)
self.endPoint = endPoint
self.next_tile = None
self.obstacleState = state
# set of tiles that were marked as available path in simulation's previous iteration
self.last_iter_seen = set()
# TODO: This is a buggggg..... we must fix the entire coordinate system? change init heading to 0
self.heading = 180
# TODO: change to custom type or enum
self.output_state = "stopped"
self.desired_heading = None
self.angle_trace = None
self.des_angle_trace = None
def visibilityDraw(self, lidar_data):
"""Draws a circle of visibility around the robot
"""
# coloring all tiles that were seen in last iteration light gray
while self.last_iter_seen:
curr_rec = self.last_iter_seen.pop()
self.canvas.itemconfig(curr_rec, outline="#C7C7C7",
fill="#C7C7C7") # light gray
row = self.curr_tile.row
col = self.curr_tile.col
index_radius_inner = int(vis_radius / tile_size)
index_radius_outer = index_radius_inner + 2
# the bounds for the visibility circle
lower_row = max(0, row - index_radius_outer)
lower_col = max(0, col - index_radius_outer)
upper_row = min(row + index_radius_outer, self.gridFull.num_rows - 1)
upper_col = min(col + index_radius_outer, self.gridFull.num_cols - 1)
lidar_data_copy = copy.copy(lidar_data)
rad_inc = int(GUI_tile_size / 3) # radius increment to traverse tiles
def _color_normally(r, angle_rad):
"""
Colors the tile at the location that is at a distance r at heading angle_rad from robot's current location.
Colors the tile based on its known attribute (obstacle, bloated, visited, or visible).
"""
coords = (r * math.sin(angle_rad) + row,
r * math.cos(angle_rad) + col
) # (row, col) of tile we want to color
# make sure coords are in bounds of GUI window
if (coords[0] >= lower_row) and (coords[0] <= upper_row) \
and (coords[1] >= lower_col) and (coords[1] <= upper_col):
curr_tile = self.gridEmpty.grid[int(coords[0])][int(coords[1])]
curr_rec = self.tile_dict[curr_tile]
if curr_tile.is_bloated:
if 11 > curr_tile.bloat_score > 0:
color = bloat_colors[curr_tile.bloat_score]
self.canvas.itemconfig(curr_rec,
outline=color,
fill=color) # blue
if curr_tile.bloat_score >= 11:
self.canvas.itemconfig(curr_rec,
outline="#000000",
fill="#000000") #black
elif curr_tile.is_obstacle:
#avg = math.ceil(sum(curr_tile.obstacle_score)/len(curr_tile.obstacle_score))
if curr_tile.obstacle_score[3] < 6:
color_pos = round(
len(obstacle_colors) / obstacle_threshold *
curr_tile.obstacle_score[3]) - 1
color = obstacle_colors[max(color_pos, 1)]
self.canvas.itemconfig(curr_rec,
outline=color,
fill=color) # yellow
else:
self.canvas.itemconfig(curr_rec,
outline="#cc8400",
fill="#cc8400") #darker yellow
# if curr_tile.is_obstacle and curr_tile.bloat_score==0:
# self.canvas.itemconfig(
# curr_rec, outline="#ff621f", fill="#ff621f") # orange
# elif curr_tile.bloat_score > 0:
# if not curr_tile.is_bloated: #something weird happening here
# print("something's wrong")
# color = bloat_scores[curr_tile.bloat_score]
# self.canvas.itemconfig(curr_rec, outline=color, fill=color) # blue
# if curr_tile.bloat_score >= 6:
# self.canvas.itemconfig(
# curr_rec, outline="#000000", fill="#000000") #black
else:
self.canvas.itemconfig(curr_rec,
outline="#fff",
fill="#fff") # white
self.last_iter_seen.add(curr_rec)
# iterating through 360 degrees surroundings of robot in increments of degree_freq
for deg in range(0, 360, degree_freq):
angle_rad = deg * math.pi / 180
if len(lidar_data_copy) == 0 or lidar_data_copy[0][0] != deg:
# no object in sight at deg; color everything normally up to visibility radius
for r in range(0, index_radius_inner, rad_inc):
_color_normally(r, angle_rad)
else: # obstacle in sight
# color everything normally UP TO obstacle, and color obstacle red
for r in range(
0,
math.ceil(lidar_data_copy[0][1] / tile_size) + rad_inc,
rad_inc):
_color_normally(r, angle_rad)
lidar_data_copy.pop(0)
def breakUpLine(self, curr_tile, next_tile):
current_loc = (curr_tile.x, curr_tile.y)
next_loc = (next_tile.x, next_tile.y)
# calculate the slope, rise/run
x_change = next_loc[0] - current_loc[0]
y_change = next_loc[1] - current_loc[1]
dist = math.sqrt(x_change**2 + y_change**2)
# if (dist < tile_size):
# return [(current_loc[0] + x_change, current_loc[1] + y_change)]
num_steps = int(dist / tile_size)
returner = []
if y_change == 0:
x_step = tile_size
y_step = 0
elif x_change == 0:
x_step = 0
y_step = tile_size
else:
inv_slope = x_change / y_change
# x^2+y^2 = tile_size^2 && x/y = x_change/y_change
y_step = math.sqrt(tile_size**2 / (inv_slope**2 + 1))
y_step = y_step
x_step = math.sqrt(
(tile_size**2 * inv_slope**2) / (inv_slope**2 + 1))
x_step = x_step
if x_change < 0 and x_step > 0:
x_step = -x_step
if y_change < 0 and y_step:
y_step = -y_step
for i in range(1, num_steps + 1):
new_coor = (current_loc[0] + i * x_step,
current_loc[1] + i * y_step)
returner.append(new_coor)
new_tile = self.gridEmpty.get_tile(new_coor)
if new_tile not in self.pathSet:
self.pathSet.add(new_tile)
return returner
def getPathSet(self):
"""
"""
prev_tile = self.curr_tile
for next_tile in self.path:
if next_tile not in self.pathSet:
self.pathSet.add(next_tile)
self.breakUpLine(prev_tile, next_tile)
prev_tile = next_tile
def printTiles(self):
for row in self.gridEmpty.grid:
for col in row:
print(str(col.x) + ', ' + str(col.y))
def checkRecalc(self):
for x, y in self.brokenPath:
check_tile = self.gridEmpty.get_tile((x, y))
if check_tile.isObstacle or check_tile.isBloated:
self.recalc = True
def updateDesiredHeading(self):
"""
calculates the degrees between the current tile and the next tile and updates desired_heading. Estimates the
degrees to the nearing int.
"""
x_change = self.next_tile.x - self.curr_x
y_change = self.next_tile.y - self.curr_y
if y_change == 0:
arctan = 90 if x_change < 0 else -90
else:
arctan = math.atan(x_change / y_change) * (180 / math.pi)
if x_change >= 0 and y_change > 0:
self.desired_heading = (360 - arctan) % 360
elif x_change < 0 and y_change > 0:
self.desired_heading = -arctan
else:
self.desired_heading = 180 - arctan
self.desired_heading = round(self.desired_heading)
# print("updated desired heading to : " + str(self.desired_heading))
def get_direction_coor(self, curr_x, curr_y, angle):
"""
returns a coordinate on on the visibility radius at angle [angle] from the robot
"""
x2 = math.cos(math.radians(angle + 90)) * vis_radius + curr_x
y2 = math.sin(math.radians(angle + 90)) * vis_radius + curr_y
return (x2 / tile_scale_fac, y2 / tile_scale_fac)
def draw_line(self, curr_x, curr_y, next_x, next_y):
"""
Draw a line from the coordinate (curr_x, curr_y) to the coordinate (next_x, next_y)
"""
self.canvas.create_line(curr_x / tile_scale_fac,
curr_y / tile_scale_fac,
next_x / tile_scale_fac,
next_y / tile_scale_fac,
fill="#339933",
width=1.5)
def draw_headings(self):
"""
Draws a line showing the heading and desired heading of the robot
"""
self.canvas.delete(self.angle_trace)
self.canvas.delete(self.des_angle_trace)
line_coor = self.get_direction_coor(self.curr_tile.x, self.curr_tile.y,
self.heading)
self.angle_trace = self.canvas.create_line(
self.curr_tile.x / tile_scale_fac,
self.curr_tile.y / tile_scale_fac,
line_coor[0],
line_coor[1],
fill='#FF69B4',
width=1.5)
des_line_coor = self.get_direction_coor(self.curr_tile.x,
self.curr_tile.y,
self.desired_heading)
self.des_angle_trace = self.canvas.create_line(
self.curr_tile.x / tile_scale_fac,
self.curr_y / tile_scale_fac,
des_line_coor[0],
des_line_coor[1],
fill='#FF0000',
width=1.5)
self.canvas.pack()
def moveDynamic(self, square):
"""
The function that handles all move of squares
check if it is a valid move by calling checkBounds
if valid, return the valid direction
if invalid, recurse the function to find a valid direction
"""
rand_nums = []
for i in range(4):
for j in range(square.dir_req_list[i]):
rand_nums.append(i + 1)
rand_num = rand_nums.pop(random.randrange(len(rand_nums)))
while True:
if self.canMove(square, rand_num):
return rand_num
else:
filter(lambda el: el != rand_num, rand_nums)
if len(rand_nums) == 0:
#print('cant move object located at: ' + str(square.x) + ', ' + str(square.y))
return -1
rand_num = rand_nums.pop(random.randrange(len(rand_nums)))
def updateGridSmoothed(self):
"""
updates the grid in a smoothed fashion
"""
# try:
if self.obstacleState == "dynamic":
for i in range(0, len(self.squareList)):
direc = self.moveDynamic(self.squareList[i])
if direc != -1:
self.move(self.squareList[i], direc)
self.smoothed_window.move(self.squareList[i], direc)
# If this is the first tile loop is being iterated through we need to initialize
if self.desired_heading is not None and self.heading == self.desired_heading:
self.draw_headings()
self.output_state = "move forward"
#print(self.output_state)
if self.desired_heading is not None and self.heading != self.desired_heading:
self.draw_headings()
if self.heading < self.desired_heading:
cw_turn_degrees = 360 + self.heading - self.desired_heading
ccw_turn_degrees = self.desired_heading - self.heading
else:
cw_turn_degrees = self.heading - self.desired_heading
ccw_turn_degrees = 360 - self.heading + self.desired_heading
if abs(self.desired_heading - self.heading) < turn_speed:
self.heading = self.desired_heading
else:
if cw_turn_degrees < ccw_turn_degrees: # turn clockwise
self.heading = self.heading - turn_speed
#print('turn left')
self.output_state = "turn right"
else: # turn counter clockwise
self.heading = self.heading + turn_speed
#print('turn right')
self.output_state = "turn left"
if self.heading < 0:
self.heading = 360 + self.heading
elif self.heading >= 360:
self.heading = self.heading - 360
self.master.after(speed_dynamic, self.updateGridSmoothed)
elif self.initPhase:
curr_tile = self.path[0]
self.curr_tile = curr_tile
self.curr_x = self.curr_tile.x
self.curr_y = self.curr_tile.y
self.visitedSet.add(curr_tile)
self.getPathSet()
lidar_data = self.generate_sensor.generateLidar(
degree_freq, curr_tile.row, curr_tile.col)
self.getPathSet()
self.recalc = self.gridEmpty.update_grid_tup_data(
curr_tile.x, curr_tile.y, lidar_data, Tile.lidar, robot_radius,
bloat_factor, self.pathSet)
self.next_tile = self.path[1]
self.brokenPath = self.breakUpLine(self.curr_tile, self.next_tile)
self.getPathSet()
self.brokenPathIndex = 0
self.visibilityDraw(lidar_data)
self.updateDesiredHeading()
self.update_robot_tile_set()
self.initPhase = False
self.master.after(speed_dynamic, self.updateGridSmoothed)
# If we need to iterate through a brokenPath
elif self.brokenPathIndex < len(self.brokenPath):
lidar_data = self.generate_sensor.generateLidar(
degree_freq, self.curr_tile.row, self.curr_tile.col)
self.recalc = self.gridEmpty.update_grid_tup_data(
self.curr_tile.x, self.curr_tile.y, lidar_data, Tile.lidar,
robot_radius, bloat_factor, self.pathSet)
self.recalc_cond = self.recalc_cond or self.recalc
# Relcalculate the path if needed
if self.recalc_cond and self.recalc_count >= recalc_wait:
# print('recalculating!')
self.path = search.a_star_search(
self.gridEmpty, (self.curr_tile.x, self.curr_tile.y),
self.endPoint, search.euclidean)
self.path = search.segment_path(self.gridEmpty, self.path)
self.pathIndex = 0
self.smoothed_window.path = self.path
self.smoothed_window.drawPath()
self.draw_line(self.curr_x, self.curr_y, self.path[0].x,
self.path[0].y)
self.curr_tile = self.path[0]
self.curr_x = self.curr_tile.x
self.curr_y = self.curr_tile.y
self.next_tile = self.path[1]
self.brokenPath = self.breakUpLine(self.curr_tile,
self.next_tile)
self.updateDesiredHeading()
self.getPathSet()
self.visibilityDraw(lidar_data)
self.update_robot_tile_set()
self.pathSet = set()
self.getPathSet()
self.pathIndex = 0
self.brokenPathIndex = 0
self.recalc = False
self.recalc_count = 0
self.recalc_cond = False
else:
# print('not')
if self.brokenPathIndex == 0:
x1 = self.curr_x
y1 = self.curr_y
else:
x1 = self.brokenPath[self.brokenPathIndex - 1][0]
y1 = self.brokenPath[self.brokenPathIndex - 1][1]
x2 = self.brokenPath[self.brokenPathIndex][0]
y2 = self.brokenPath[self.brokenPathIndex][1]
future_tile = self.gridEmpty.get_tile((x2, y2))
if future_tile.is_obstacle or future_tile.is_bloated:
self.recalc_count = recalc_wait
else:
self.draw_line(x1, y1, x2, y2)
self.curr_x = x2
self.curr_y = y2
self.curr_tile = future_tile
self.visitedSet.add(self.curr_tile)
self.visibilityDraw(lidar_data)
self.brokenPathIndex += 1
self.recalc_count += 1
# MAYBE CHANGE WIDTH TO SEE IF IT LOOKS BETTER?
self.master.after(speed_dynamic, self.updateGridSmoothed)
# If we have finished iterating through a broken path, we need to go to the
# Next tile in path, and create a new broken path to iterate through
else:
lidar_data = self.generate_sensor.generateLidar(
degree_freq, self.curr_tile.row, self.curr_tile.col)
if self.curr_tile == self.gridEmpty.get_tile(self.endPoint):
print('C1C0 has ARRIVED AT THE DESTINATION, destination tile')
return
self.pathSet = set()
self.pathIndex += 1
if self.pathIndex == len(self.path) - 1:
print('C1C0 has ARRIVED AT THE DESTINATION, destination tile')
return
self.draw_line(self.curr_x, self.curr_y,
self.path[self.pathIndex].x,
self.path[self.pathIndex].y)
self.curr_tile = self.path[self.pathIndex]
self.curr_x = self.curr_tile.x
self.curr_y = self.curr_tile.y
self.next_tile = self.path[self.pathIndex + 1]
self.brokenPath = self.breakUpLine(self.curr_tile, self.next_tile)
self.getPathSet()
self.brokenPathIndex = 0
self.visibilityDraw(lidar_data)
self.update_robot_tile_set()
self.updateDesiredHeading()
self.master.after(speed_dynamic, self.updateGridSmoothed)
# except Exception as e:
# print(e)
# print("C1C0: \"There is no path to the desired location. Beep Boop\"")
def runSimulation(self):
"""Runs a sumulation of this map, with its enviroment and path
"""
self.smoothed_window = MovingObStaticGUI.SmoothedPathGUI(
Toplevel(), self.gridFull, self.path, self.squareList)
self.smoothed_window.drawPath()
# if self.obstacleState == "dynamic":
# for i in range(0, len(self.squareList)):
# self.smoothed_window.move(self.squareList[i], self.moveDynamic(self.squareList[i]))
self.updateGridSmoothed()
self.master.mainloop()
class DynamicGUI():
def __init__(self, master, fullMap, emptyMap, path, endPoint):
"""A class to represent a GUI with a map
Arguments:
master {Tk} -- Tkinter GUI generator
inputMap {grid} -- The grid to draw on
path {tile list} -- the path of grid tiles visited
FIELDS:
master {Tk} -- Tkinter GUI generator
tile_dict {dict} -- a dictionary that maps tiles to rectangles
canvas {Canvas} -- the canvas the GUI is made on
path {Tile list} -- the list of tiles visited by robot on path
pathSet {Tile Set} -- set of tiles that have already been drawn (So GUI
does not draw over the tiles)
pathIndex {int} -- the index of the path the GUI is at in anumation
curr_tile {Tile} -- the current tile the robot is at in animation
grid {Grid} -- the Grid object that the simulation was run on
"""
# Tinker master, used to create GUI
self.master = master
self.tile_dict = None
self.canvas = None
self.path = path
self.visitedSet = set()
self.pathSet = set()
for i in self.path:
self.pathSet.add(i)
self.pathIndex = len(path) - 1
self.curr_tile = None
self.gridFull = fullMap
self.gridEmpty = emptyMap
self.recalc = False
self.stepsSinceRecalc = 0
self.create_widgets()
self.generate_sensor = GenerateSensorData(self.gridFull)
self.endPoint = endPoint
def create_widgets(self, empty=True):
"""Creates the canvas of the size of the inputted grid
"""
if (empty):
map = self.gridEmpty.grid
else:
map = self.gridFull.grid
width = len(map[0]) * GUI_tile_size
height = len(map) * GUI_tile_size
visMap = Canvas(self.master, width=width, height=height)
offset = GUI_tile_size / 2
if (empty):
tile_dict = {}
for row in map:
for tile in row:
x = tile.x / tile_scale_fac
y = tile.y / tile_scale_fac
x1 = x - offset
y1 = y - offset
x2 = x + offset
y2 = y + offset
if (tile.is_bloated):
color = "#ffc0cb"
elif (tile.is_obstacle):
color = "#ffCC99"
else:
color = "#545454"
if (empty):
tile_dict[tile] = visMap.create_rectangle(x1,
y1,
x2,
y2,
outline=color,
fill=color)
visMap.pack()
self.canvas = visMap
if (empty):
self.tile_dict = tile_dict
def visibilityDraw(self):
"""Draws a circle of visibility around the robot
"""
index_radius_inner = int(vis_radius / tile_size)
index_rad_outer = index_radius_inner + 2
row = self.curr_tile.row
col = self.curr_tile.col
lower_row = int(max(0, row - index_rad_outer))
lower_col = int(max(0, col - index_rad_outer))
upper_row = int(min(row + index_rad_outer, self.gridFull.num_rows))
upper_col = int(min(col + index_rad_outer, self.gridFull.num_cols))
for i in range(lower_row, upper_row):
for j in range(lower_col, upper_col):
curr_tile = self.gridEmpty.grid[i][j]
curr_rec = self.tile_dict[curr_tile]
x_dist = abs(i - row)
y_dist = abs(j - col)
dist = math.sqrt(x_dist * x_dist + y_dist * y_dist)
if (dist < index_radius_inner):
if (curr_tile.is_obstacle and not curr_tile.is_bloated):
self.canvas.itemconfig(curr_rec,
outline="#ff621f",
fill="#ff621f")
elif (curr_tile.is_bloated):
self.canvas.itemconfig(curr_rec,
outline="#ffc0cb",
fill="#ffc0cb")
# elif (curr_tile in self.visitedSet):
# self.canvas.itemconfig(
# curr_rec, outline="#0C9F34", fill="#0C9F34")
elif (curr_tile not in self.visitedSet):
self.canvas.itemconfig(curr_rec,
outline="#fff",
fill="#fff")
else:
if (curr_tile.is_obstacle == False
and curr_tile.is_bloated == False
and curr_tile not in self.visitedSet):
self.canvas.itemconfig(curr_rec,
outline="#545454",
fill="#545454")
def updateGrid(self):
"""USED TO ANIMATE THE SIMULATION
Update function that is continuously called using the
master.after command, any code before that will automatically
run at every iteration, according to global variable, speed.
"""
try:
if (self.pathIndex != -1):
curr_tile = self.path[self.pathIndex]
curr_rec = self.tile_dict[curr_tile]
self.curr_tile = curr_tile
self.visitedSet.add(curr_tile)
lidar_data = self.generate_sensor.generateLidar(
10, curr_tile.row, curr_tile.col)
if (self.gridEmpty.update_grid_tup_data(
curr_tile.x, curr_tile.y, lidar_data, 3, robot_radius,
bloat_factor, self.pathSet, self.gridFull)):
self.recalc = True
nextTileIndex = min(self.pathIndex + 2, len(self.path) - 1)
emergencyRecalc = False
if (self.path[nextTileIndex].is_bloated
or self.path[nextTileIndex].is_obstacle):
emergencyRecalc = True
if ((self.stepsSinceRecalc >= steps_to_recalc and self.recalc)
or emergencyRecalc):
print('recalculating!')
start = (curr_tile.x, curr_tile.y)
dists, self.path = search.a_star_search(
self.gridEmpty, start, self.endPoint, search.euclidean)
self.pathSet = set()
for i in self.path:
self.pathSet.add(i)
self.pathIndex = len(self.path) - 1
self.recalc = False
emergencyRecalc = False
self.stepsSinceRecalc = 0
self.visibilityDraw()
self.canvas.itemconfig(curr_rec,
outline="#00FF13",
fill="#00FF13")
self.pathIndex = self.pathIndex - 1
self.stepsSinceRecalc = self.stepsSinceRecalc + 1
self.master.after(speed_dynamic, self.updateGrid)
except:
print(
"C1C0: \"There is no path to the desired location. Beep Boop\""
)
def runSimulation(self):
"""Runs a sumulation of this map, with its enviroment and path
"""
top = Toplevel()
newGUI = StaticGUI.MapPathGUI(top, self.gridFull, [])
self.updateGrid()
self.master.mainloop()