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 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 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 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)
class ServerGUI:
"""
Master file to run autonomous path planning and display visualization real-time
Instance Attributes:
master (Tk): Tkinter GUI object
canvas (Canvas): Tkinter Canvas that represents the GUI
tile_dict (Dict[Tile, Rectangle]): Mapping from tiles to rectangles on GUI
grid (Grid): grid that represents the environment
heading (int): integer to represent the angle that the robot is facing
"""
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 processEndPoint(self, endPoint):
"""
Processes the endpoint and returns the corresponding tuple.
Input examples:
("move forward", m) where m is in meters
("turn", deg) where deg is positive for clockwise turns, negative for counterclockwise
(x, y) where x and y are coordinates to move to
Updates self.endPoint to be the final coordinates to move to,
and updates self.desired_heading
"""
# firstNum = firstNum + tile_num_width * tile_size / 2
# secondNum = -secondNum + tile_num_height * tile_size / 2
start = endPoint.find("(")
comma = endPoint.find(",")
end = endPoint.find(")")
processedEndPoint = (endPoint[start + 1:comma],
float(endPoint[comma + 2:end]))
"""
print(
f"EndPoint[0]: {processedEndPoint[0]} EndPoint[1]: {processedEndPoint[1]}")
"""
if processedEndPoint[0] == "'move forward'":
self.endPoint = (self.curr_tile.x,
self.curr_tile.y + processedEndPoint[1] * 100)
self.desired_heading = self.heading
elif processedEndPoint[0] == "'turn'":
self.endPoint = (self.curr_tile.x, self.curr_tile.y)
self.desired_heading = self.heading + processedEndPoint[1]
#print(f"Ang[0]: {self.heading} Ang[1]: {self.desired_heading}")
else:
self.endPoint = (self.curr_tile.x -
int(processedEndPoint[0]) * 100,
self.curr_tile.y + processedEndPoint[1] * 100)
self.desired_heading = self.heading
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 nextLoc(self):
next_tile = self.path[self.pathIndex]
d = math.sqrt((self.curr_tile.x - next_tile.x)**2 +
(self.curr_tile.y - next_tile.y)**2)
return d <= reached_tile_bound and abs(self.heading -
self.desired_heading) <= 2
def updateDesiredHeading(self, next_tile):
"""
Calculates the degrees between the current tile and the next tile and updates desired_heading. Estimates the
degrees to the nearing int.
"""
x_change = next_tile.x - self.curr_tile.x
y_change = next_tile.y - self.curr_tile.y
#print(f"x: {x_change} y: {y_change}")
if x_change == 0 and y_change == 0:
# no movement, only turning command was given
self.desired_heading = self.desired_heading
else:
# there's some movement necessary
self.desired_heading = self.heading + \
round(math.degrees(math.atan2(y_change, x_change))) - \
90.0 # -90 fixes the transformed value
if self.desired_heading < -180.0:
self.desired_heading = self.desired_heading + 360
elif self.desired_heading > 180.0:
self.desired_heading = self.desired_heading - 360
def computeMotorSpeed(self):
"""
Currently assuming:
if desired angle > current angle, turn right
if desired angle < current angle, turn left
Threshold of 2 degrees, will only try to rotate if the rotation
is more than 2 degrees.
Threshold of (5 centimeters, need to change after testing) for the x and y end points.
"""
# TODO: Test angle and distance thresholds with C1C0
"""
print(
f"curr tile x: {self.curr_tile.x} curr tile y {self.curr_tile.y}")
print(
f"end point x: {self.endPoint[0]} end point y {self.endPoint[1]}")
print(
f"self.desired_heading: {self.desired_heading} self.heading {self.heading}")
"""
if abs(self.curr_tile.x - self.endPoint[0]) <= 30 and abs(
self.curr_tile.y - self.endPoint[1]) <= 30 and (
abs(self.desired_heading - self.heading) <= 3):
return ()
elif self.desired_heading - self.heading > 3:
return rotation_right
elif self.desired_heading - self.heading < -3:
return rotation_left
else:
return motor_speed
def update_grid_wrapper(self):
t_bot, t_mid, t_top = self.sensor_state.get_terabee()
lidar_data = self.filter_lidar(self.sensor_state.lidar)
lidar_ret = self.grid.update_grid_tup_data(self.curr_tile.x,
self.curr_tile.y,
lidar_data, Tile.lidar,
robot_radius, bloat_factor,
self.path_set)
bot_ter_ret = self.grid.update_grid_tup_data(
self.curr_tile.x, self.curr_tile.y, t_bot, Tile.bottom_terabee,
robot_radius, bloat_factor, self.path_set)
# mid_ter_ret = self.grid.update_grid_tup_data(self.curr_tile.x, self.curr_tile.y, t_mid, Tile.mid_terabee,
# robot_radius, bloat_factor, self.path_set)
# top_ter_ret = self.grid.update_grid_tup_data(self.curr_tile.x, self.curr_tile.y, t_top, Tile.top_terabee,
# robot_radius, bloat_factor, self.path_set)
self.heading = self.sensor_state.heading
return lidar_ret and bot_ter_ret
def filter_lidar(self, lidar):
# print(lidar)
lidar_ret = []
for (ang, dist) in lidar:
if dist > 400:
lidar_ret.append(((ang + lidar_shift_ang) % 360, dist))
return lidar_ret
def filter_terabee(self, terabee):
terabee_ret = []
for (ang, dist) in terabee:
if 100 < dist < 50000:
terabee_ret.append((ang, dist))
return terabee_ret
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 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
"""
#print('calc vector was called')
vect = (0, 0)
if self.pathIndex < len(self.path):
vect = self.pid.newVec()
if self.prev_vector is not None:
# delete old drawings from previous iteration
self.canvas.delete(self.prev_vector)
start = self._scale_coords((self.curr_tile.x, self.curr_tile.y))
end = self._scale_coords(
(self.curr_tile.x + vector_draw_length * vect[0],
self.curr_tile.y + vector_draw_length * vect[1]))
self.prev_vector = self.canvas.create_line(start[0],
start[1],
end[0],
end[1],
arrow='last',
fill='red')
return vect
def refresh_bloating(self):
for row in self.grid.grid:
for tile in row:
if tile.is_bloated:
tile.is_bloated = False
tile.is_obstacle = False
for row in self.grid.grid:
for tile in row:
if tile.is_obstacle and not tile.is_bloated:
self.grid.bloat_tile(tile, 80, 2, set())
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.grid.num_rows - 1)
upper_col = min(col + index_radius_outer, self.grid.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.grid.grid[int(coords[0])][int(coords[1])]
curr_rec = self.tile_dict[curr_tile]
if curr_tile.is_bloated:
self.canvas.itemconfig(curr_rec,
outline="#ffc0cb",
fill="#ffc0cb") # pink
elif curr_tile.is_obstacle:
self.canvas.itemconfig(curr_rec,
outline="#ff621f",
fill="#ff621f") # red
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"""
# 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 = 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 drawPath(self):
# change previous 5 paths with green gradual gradient
# set default/initial color
color = self.color_list[4]
# if there is any path that the bot walked through, it gets added to set_of_prev_path
if self.prev_line_id:
self.set_of_prev_path.append(self.prev_line_id)
# if there is any previous path in the set_of_prev_path, then we check if there is less than 5 lines,
# if so we change the color of the newest path to a color from the list. If there is more than 5 lines,
# we delete the oldest line and change the colors of remaining previous colors to a lighter shade.
if self.set_of_prev_path:
if len(self.set_of_prev_path) > 4:
for fst_id in self.set_of_prev_path[0]:
self.canvas.delete(fst_id)
self.set_of_prev_path.pop(0)
for x in range(len(self.set_of_prev_path)):
for ids in self.set_of_prev_path[x]:
self.canvas.itemconfig(ids, fill=self.color_list[x])
# clear current path
self.prev_line_id = []
# continuously draw segments of the path, and add it to the prev_line_id list
idx = 1
while idx < len(self.path):
x1, y1 = self._scale_coords(
(self.path[idx - 1].x, self.path[idx - 1].y))
x2, y2 = self._scale_coords((self.path[idx].x, self.path[idx].y))
canvas_id = self.canvas.create_line(x1,
y1,
x2,
y2,
fill=color,
width=1.5)
self.prev_line_id.append(canvas_id)
idx += 1
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 generatePathSet(self):
self.path_set = 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.grid.get_tile(new_coor)
if new_tile not in self.path_set:
self.path_set.add(new_tile)
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")
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")