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