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 __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 __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
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()
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
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")
def __init__(self, master, inputMap, path, squareList): super().__init__(master, inputMap, None, path, squareList, len(path) - 1) self.create_widgets(False) self.generate_sensor = GenerateSensorData(self.gridFull)
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()