def dynamicGridSimulation(text_file: str): emptyMap = grid.Grid(tile_num_height, tile_num_width, tile_size) fullMap = grid.Grid(tile_num_height, tile_num_width, tile_size) # Generates random enviroment on the grid generator = RandomObjects(fullMap) # You can change the number of every type of object you want if text_file == '': generator.create_env(22, 0, 0, 22, 9) else: generator.create_env_json(text_file) # starting location for middle midX = tile_size * tile_num_width / 2 midY = tile_size * tile_num_height / 2 # Calculate and point and change coordinate system from user inputted CICO @(0,0) to the grid coordinates endPoint = userInput() # Run algorithm to get path path = search.a_star_search(emptyMap, (midX, midY), endPoint, search.euclidean) # start GUI and run animation simulation = DynamicGUI(Tk(), fullMap, emptyMap, search.segment_path(emptyMap, path), endPoint) simulation.runSimulation()
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()
def __init__(self, input_server, init_input=None): self.run_mock = init_input is not None self.master: Tk = Tk() self.canvas: Canvas = None self.tile_dict: Dict[Tile, int] = None self.grid = grid.Grid(tile_num_height, tile_num_width, tile_size) self.last_iter_seen = set() self.heading: int = 0 self.curr_tile = self.grid.grid[int(self.grid.num_rows / 2)][int( self.grid.num_cols / 2)] # planned path of tiles self.prev_draw_c1c0_ids = [None, None] self.create_widgets() self.server = input_server self.processEndPoint(self.server.recieve_data_init()['end_point']) #print('got the end point to be, ', self.endPoint) self.path = search.a_star_search(self.grid, (self.curr_tile.x, self.curr_tile.y), self.endPoint, search.euclidean) self.path = search.segment_path(self.grid, self.path) self.path_set = set() self.generatePathSet() self.pathIndex = 0 self.prev_tile = None self.prev_vector = None self.way_point = None self.loop_it = 0 self.prev_line_id = [] self.set_of_prev_path = [] self.color_list = [ '#2e5200', '#347800', '#48a600', '#54c200', '#60de00', 'None' ] self.index_fst_4 = 0 self.drawPath() self.pid = PID(self.path, self.pathIndex, self.curr_tile.x, self.curr_tile.y) self.drawWayPoint(self.path[self.pathIndex]) self.updateDesiredHeading(self.path[self.pathIndex]) self.gps = GPS(self.grid, self.pid) self.prev_tile, self.curr_tile = self.gps.update_loc(self.curr_tile) self.main_loop() if self.curr_tile == self.path[-1]: print("Reached endpoint") self.hedge.stop() return self.master.mainloop()
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 main_loop(self): """ """ self.loop_it += 1 if self.loop_it % 6 == 0: self.refresh_bloating() # update location based on indoor GPS self.prev_tile, self.curr_tile = self.gps.update_loc(self.curr_tile) self.drawC1C0() if self.run_mock: self.server.send_update((self.curr_tile.row, self.curr_tile.col)) else: motor_speed = self.computeMotorSpeed() self.server.send_update(motor_speed) # TODO 2: Update environment based on sensor data self.sensor_state = self.server.receive_data() self.update_grid_wrapper() self.visibilityDraw(self.filter_lidar(self.sensor_state.lidar)) if self.grid.update_grid_tup_data( self.curr_tile.x, self.curr_tile.y, self.filter_lidar(self.sensor_state.lidar), Tile.lidar, robot_radius, bloat_factor, self.path_set): self.generatePathSet() #print('current location x', self.curr_tile.x) #print('current location y', self.curr_tile.y) try: self.path = search.a_star_search( self.grid, (self.curr_tile.x, self.curr_tile.y), self.endPoint, search.euclidean) self.path = search.segment_path(self.grid, self.path) self.pathIndex = 0 self.pid = PID(self.path, self.pathIndex, self.curr_tile.x, self.curr_tile.y) self.drawWayPoint(self.path[self.pathIndex]) self.updateDesiredHeading(self.path[self.pathIndex]) self.generatePathSet() except Exception as e: print(e, 'in an obstacle right now... oof ') # recalculate path if C1C0 is totally off course (meaning that PA + PB > 2*AB) if self.pathIndex != 0: # distance to previous waypoint dist1 = (self.curr_tile.x - self.path[self.pathIndex - 1].x)**2 + ( self.curr_tile.y - self.path[self.pathIndex - 1].y)**2 # distance to next waypoint dist2 = (self.curr_tile.x - self.path[self.pathIndex].x)**2 + ( self.curr_tile.y - self.path[self.pathIndex].y)**2 # distance between waypoints dist = (self.path[self.pathIndex-1].x - self.path[self.pathIndex].x) ** 2\ + (self.path[self.pathIndex-1].y - self.path[self.pathIndex].y) ** 2 if 4 * dist < dist1 + dist2: try: self.path = search.a_star_search( self.grid, (self.curr_tile.x, self.curr_tile.y), self.endPoint, search.euclidean) self.path = search.segment_path(self.grid, self.path) self.pathIndex = 0 self.pid = PID(self.path, self.pathIndex, self.curr_tile.x, self.curr_tile.y) self.generatePathSet() except Exception as e: print(e, 'in an obstacle right now... oof ') self.drawPath() self.calcVector() if self.nextLoc(): self.pathIndex += 1 if self.pathIndex >= len(self.path): return self.pid = PID(self.path, self.pathIndex, self.curr_tile.x, self.curr_tile.y) self.drawWayPoint(self.path[self.pathIndex]) self.updateDesiredHeading(self.path[self.pathIndex]) # return if we are at the end destination if self.curr_tile == self.path[-1] and abs(self.heading - self.desired_heading) <= 2: return # recursively loop self.master.after(1, self.main_loop)
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\"" )