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 staticGridSimulation(): wMap = grid.Grid(tile_num_height, tile_num_width, tile_size) # Generates random enviroment on the grid generator = RandomObjects(wMap) # You can change the number of every type of object you want generator.create_env(20, 0, 0, 20, 7) generator.bloatTiles(robot_radius, bloat_factor) # Starting location topLeftX = 2.0 topLeftY = 2.0 # Ending Location botRightX = tile_size * tile_num_width - 2.0 botRightY = tile_size * tile_num_height - 2.0 # Run algorithm to get path try: dists, path = search.a_star_search( wMap, (topLeftX, topLeftY), (botRightX, botRightY), search.euclidean) root = Tk() # start GUI and run animation simulation = MapPathGUI(root, wMap, path, generator.squares) simulation.runSimulation(True) except: print("C1C0: \"There is no path to the desired location. Beep Boop\"") if __name__ == "__main__": staticGridSimulation()
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 __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\"" )