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\""
)
