class Server:
def __init__(self):
# create the grid
self.grid_width = 2.5
self.grid_height = 3.5
self.grid_x = 20
self.grid_y = 28
self.offset_x = 0
self.offset_y = 0
self.border_thickness = 1
self.use_diagonals = True
self.grid = Grid(self.grid_width, self.grid_height, self.grid_x,
self.grid_y, self.offset_x, self.offset_y,
self.border_thickness, self.use_diagonals)
# initialize default variables
self.can_run = True
self.rover_position = self.grid.get_node(0, 0)
self.destinations = []
self.home = self.grid.get_node(1, 1)
self.current_path = []
self.simple_path = []
self.mode = 0
self.status = 0
self.position_log = []
self.position_log.append(self.home)
self.sim_type = 0
self.sim_destinations = [
] # where destinations are held during certain simulations
# initialize the gpg
self.gpg = AdvancedGoPiGo3(25, use_mutex=True) # was true
volt = self.gpg.volt()
print("Current Voltage: ", volt)
if volt < 8:
print(
"Critical! voltage is very low, please charge the batteries before continuing! You have been warned!"
)
elif volt < 9:
print(
"Warning, voltage is getting low, impaired performance is expected."
)
# initialize send queue
self.send_queue = queue.Queue()
# initialize the socket
print("Awaiting connection")
self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
address = (HOST, PORT)
self.socket.bind(address)
self.socket.listen(1)
# set up states
self.change_status(IDLE)
self.change_mode(AUTO)
# get the connection
self.conn, self.addr = self.socket.accept()
print("Successful connection from ", self.addr)
# initialize gps
self.gps_queue = queue.Queue()
self.gps = GPS(10, 1, self.gpg, q=self.gps_queue, debug_mode=False)
self.gps.set_min_distance(50)
self.gps_can_run = False
self.gps.set_obstacle_callback(self.obstacle_found)
self.gps.set_position_callback(self.rover_position_change)
self.gps.set_no_obstacle_callback(self.on_no_obstacles)
self.gps.set_reached_point_callback(self.destination_reached)
self.gps.set_speed(800)
# start remote control thread
self.remote_can_run = False
# start the video server
self.video = VideoServer()
self.video.start()
self.gps.start()
# this method manages incoming and outgoing commands
def manage_commands(self):
while self.can_run:
# empty the queue and send all the data to the client
while not self.send_queue.empty():
data = self.send_queue.get_nowait()
if self.mode != SIM:
self.conn.send(str.encode(data))
# wait a moment for a response
received, _, _ = select.select([self.conn], [], [], .1)
# if not response then continue
if received:
data = self.conn.recv(1024).decode('utf-8').split()
self.parse_data(data)
if self.mode == SIM:
self.update_sim_behavior()
# this method parses the incoming data commands. Each command comprises of atleast one letter that is removed
# from the list.
def parse_data(self, data):
while len(data) > 0:
command = data.pop(0)
print(command)
# GUI INPUT COMMANDS
# node has been changed
if command == "MODE":
mode = int(data.pop(0))
self.change_mode(mode)
if self.mode == SIM:
data = []
self.start_sim_behavior()
if self.mode != SIM:
if command == 'N':
x = int(data.pop(0))
y = int(data.pop(0))
node_type = int(data.pop(0))
self.node_changed(x, y, node_type)
# destination change
elif command == 'D':
x = int(data.pop(0))
y = int(data.pop(0))
if x == -1 and y == -1:
print("cleared destinations")
self.destinations = []
self.current_path = []
self.simple_path = []
self.gps.cancel_movement()
else:
node = self.grid.nodes[x][y]
if node.node_type == OPEN_SPACE and node != self.rover_position:
self.destinations.append(node)
self.find_path()
# home change
elif command == 'H':
x = int(data.pop(0))
y = int(data.pop(0))
node = self.grid.nodes[x][y]
if node.node_type == OPEN_SPACE:
self.home = node
# go
elif command == 'GO':
self.start_navigation()
# REMOTE CONTROL COMMANDS
# STOP
elif command == 'S':
self.stop_navigation()
elif command == 'Q':
print("Quitting")
self.can_run = False
# move
elif command == 'M':
# the numbers coming in should be integers, but aren't always
x_speed = int(float(data.pop(0)))
y_speed = int(float(data.pop(0)))
# adjust to proper speeds
if x_speed == 0 and y_speed == 0:
self.gpg.stop()
elif x_speed == 0:
self.gpg.rotate_right_forever()
elif y_speed == 0:
self.gpg.rotate_left_forever()
else:
self.gpg.set_left_wheel(abs(x_speed))
self.gpg.set_right_wheel(abs(y_speed))
if y_speed > 25 and x_speed > 25:
self.gpg.backward()
else:
self.gpg.forward()
# LEDS on
elif command == "LON":
print("Turning LED on")
self.gpg.led_on(0)
self.gpg.led_on(1)
self.gpg.open_left_eye()
# LEDS off
elif command == "LOFF":
print("Turning LED off")
self.gpg.led_off(0)
self.gpg.led_off(1)
self.gpg.close_left_eye()
elif command == "SIM":
self.sim_type = int(data.pop(0))
def start_sim_behavior(self):
if self.sim_type == WAIT:
self.sim_run = self.gps_can_run
self.stop_navigation()
self.gpg.stop()
self.change_status(IDLE)
elif self.sim_type == GO_BACK:
self.stop_navigation()
self.gpg.stop()
if len(self.position_log) > 0:
self.destinations = []
self.destinations.append(self.position_log.pop(-1))
if self.destinations[0] == self.rover_position and len(
self.position_log) > 0:
self.destinations = []
self.destinations.append(self.position_log.pop(-1))
print("log pulled")
self.start_navigation()
elif self.sim_type == CONTINUE:
print(self.gps_can_run)
self.sim_run = self.gps_can_run
self.stop_navigation()
self.gpg.stop()
if self.sim_run:
self.start_navigation()
elif self.sim_type == GO_HOME:
print("GO home start")
self.sim_destinations = list(self.destinations)
self.destinations = []
self.destinations.append(self.home)
self.start_navigation()
def update_sim_behavior(self):
if self.sim_type == WAIT:
print("Can run: ", self.gps_can_run)
self.stop_navigation()
self.change_status(IDLE)
if self.sim_type == CONTINUE and len(self.destinations) > 0:
if self.status == IDLE:
self.start_navigation()
elif len(self.destinations) == 0:
self.change_status(IDLE)
if self.sim_type == GO_BACK and self.status == IDLE:
if len(self.position_log) > 0:
self.destinations = []
self.destinations.append(self.position_log.pop(-1))
if self.destinations[0] == self.rover_position and len(
self.position_log) > 0:
self.destinations = []
self.destinations.append(self.position_log.pop(-1))
self.start_navigation()
elif self.rover_position != self.home:
self.destinations.append(self.home)
self.start_navigation()
def end_sim_behavior(self):
print("ending simulation")
if self.sim_type == WAIT:
self.gps_can_run = self.sim_run
print("GPS can run again", self.gps_can_run)
if self.gps_can_run:
self.start_navigation()
if self.sim_type == GO_HOME:
self.stop_navigation()
self.destinations = list(self.sim_destinations)
def start_navigation(self):
print("starting navigation")
self.find_path()
self.gps_can_run = True
if len(
self.destinations
) > 0 and self.home is not None and self.destinations[0] == self.home:
self.change_status(TO_HOME)
else:
self.change_status(TO_POINT)
self.next_gps_point()
def stop_navigation(self):
print("stopping navigation")
self.gps.cancel_movement()
self.gpg.stop()
self.gps_can_run = False
self.change_status(IDLE)
def node_changed(self, x, y, node_type):
self.grid.set_node(x, y, node_type)
if node_type == OBSTACLE:
self.add_obstacle(self.grid.nodes[x][y])
self.find_path()
def node_changed_to_open(self, node, node_type):
self.grid.set_node_type(node, node_type)
self.send_message("N " + str(node.gridPos.x) + " " +
str(node.gridPos.y) + " 0")
self.find_path()
def on_no_obstacles(self, position):
if position.x > 0 and position.y > 0 and position.x <= self.grid_width and position.y <= self.grid_height:
node = self.grid.node_from_global_coord(position)
path, _ = self.grid.find_path(self.rover_position, node, False)
for p in path:
if p.node_type != 0:
if p.gridPos.x > 0 and p.gridPos.y > 0 and p.gridPos.x < self.grid.nodes_in_x and\
p.gridPos.y < self.grid.nodes_in_y:
self.node_changed_to_open(p, 0)
def rover_position_change(self, position):
self.send_message("RT " + str(position.x) + " " + str(position.y))
# We only care about it if it is in the grid.
if position.x > 0 and position.y > 0 and position.x <= self.grid_width and position.y <= self.grid_height:
node = self.grid.node_from_global_coord(position)
# we still only care if it moves it to a new node.
if node != self.rover_position:
print("callback-rover position changed")
self.rover_position = node
# if we arrived at a destination we are done.
## if len(self.destinations) > 0 and node == self.destinations[0]:
## self.destinations = []
## self.current_path = []
# we need a new full route.
self.send_path()
# send info along
self.send_message("R " + str(node.gridPos.x) + " " +
str(node.gridPos.y))
def destination_reached(self, pos):
print("callback-point reached", pos)
# send the next point to the gps
self.next_gps_point()
# if we are at our final destination, we are done.
if len(self.destinations
) > 0 and self.destinations[0] == self.rover_position:
print("final destination reached")
self.send_message("DR")
self.stop_navigation()
if len(self.destinations) > 0:
if self.mode != SIM:
self.position_log.append(self.destinations[0])
self.destinations.pop(0)
self.start_navigation()
else:
self.find_path()
def obstacle_found(self, position):
# We only care about it if it is in the grid.
if position.x > 0 and position.y > 0 and position.x <= self.grid_width and position.y <= self.grid_height:
node = self.grid.node_from_global_coord(position)
# We still only care if this changes anything
if node.node_type != 1:
print("callback-obstacle found")
# we have an obstacle!
self.add_obstacle(node)
path, _ = self.grid.find_path(self.rover_position, node, False)
if len(path) > 0:
path.pop(-1)
for p in path:
if p.node_type != 0:
if p.gridPos.x > 0 and p.gridPos.y > 0 and p.gridPos.x < self.grid.nodes_in_x and \
p.gridPos.y < self.grid.nodes_in_y:
self.node_changed_to_open(p, 0)
def add_obstacle(self, node):
# if we have a valid node to work with.
if node.node_type != 1 and node != self.rover_position:
# we need to spread the grid.
self.grid.set_node_type(node, 1)
border = self.grid.all_borders
self.send_message("N " + str(node.gridPos.x) + " " +
str(node.gridPos.y) + " 1")
print("N", node.gridPos.x, node.gridPos.y)
# IF there is a destination in play and it is hit by the border, it needs to be cleared.
if len(self.destinations) > 0:
if border.__contains__(
self.destinations[0]) or node == self.destinations[0]:
self.destinations.pop(0)
self.current_path = []
self.simple_path = []
# we need a new path
print("obstacle, new path")
self.find_path()
# if we are currently in motion. let's go!
if self.gps_can_run and len(self.simple_path) > 0:
destination = self.grid.get_global_coord_from_node(
self.simple_path[0])
#self.gpg.stop()
self.gps_queue.queue.clear()
self.gps.cancel_movement()
self.gps_queue.put(destination)
def find_path(self, send_message=True):
if len(self.destinations) > 0:
_, self.simple_path = self.grid.find_path(self.rover_position,
self.destinations[0])
# send paths
if send_message:
print("sending paths")
self.send_path()
self.send_simple_path()
def next_gps_point(self):
# if we actually have somewhere to go
if len(self.simple_path) > 0:
print("navigating to next point!")
node = self.simple_path[0]
if node == self.rover_position and len(self.simple_path) > 0:
node = self.simple_path.pop(0)
destination = self.grid.get_global_coord_from_node(node)
self.gps_queue.put(destination)
self.send_simple_path()
#else:
#print("no more points!")
#self.stop_navigation()
def send_message(self, message):
# puts a message in the send queue
self.send_queue.put((" " + message))
def send_status(self):
self.send_message("ST " + str(self.status))
def send_path(self):
if len(self.simple_path) > 0:
self.current_path, _ = self.grid.find_path(self.rover_position,
self.simple_path[0])
for i in range(1, len(self.simple_path)):
temp, _ = self.grid.find_path(self.simple_path[i - 1],
self.simple_path[i])
self.current_path += temp
# sends the full path to the client
if len(self.current_path) > 0:
message = "FP "
for p in self.current_path:
message += p.__str__() + " "
message += "D"
self.send_message(message)
else:
self.send_message("FP D")
def send_simple_path(self):
# sends the simplified path to the client
if len(self.simple_path) > 0:
message = "SP "
for p in self.simple_path:
message += p.__str__() + " "
message += "D"
self.send_message(message)
else:
self.send_message("SP D")
def change_status(self, status):
self.status = status
if status == IDLE:
self.gpg.set_left_eye_color(BLUE)
elif status == TO_POINT:
self.gpg.set_left_eye_color(GREEN)
else: # status == TO_HOME:
self.gpg.set_left_eye_color(YELLOW)
self.gpg.open_left_eye()
self.send_status()
def change_mode(self, mode):
# handle lights
if mode == AUTO:
self.gpg.set_right_eye_color(CYAN)
self.gpg.open_eyes()
elif mode == SIM:
print("starting sim " + str(self.sim_type))
self.gpg.set_right_eye_color(RED)
if self.sim_type == 1:
self.gpg.set_antenna_color(BLUE)
elif self.sim_type == 2:
self.gpg.set_antenna_color(ORANGE)
elif self.sim_type == 3:
self.gpg.set_antenna_color(GREEN)
else: # self.sim_type == 4:
self.gpg.set_antenna_color(YELLOW)
self.gpg.open_all_leds()
else: # mode == MAN:
self.stop_navigation()
self.gpg.set_eye_color(PURPLE)
self.gpg.open_eyes()
self.gpg.close_left_eye()
# handle state change
if self.mode == MAN:
self.gpg.open_left_eye()
elif self.mode == SIM:
self.gpg.close_antenna()
# Send all obstacles
for node in self.grid.all_obstacles:
self.send_message("N " + str(node.gridPos.x) + " " +
str(node.gridPos.y) + " 1")
# Send all destinations
temp = "DU "
for node in self.destinations:
temp += str(node.gridPos.x) + " " + str(node.gridPos.y) + " "
temp += " UD"
self.send_message(temp)
# Send paths
self.send_path()
self.send_simple_path()
# Send position
self.send_message("R " + str(self.rover_position.gridPos.x) + " " +
str(self.rover_position.gridPos.y))
# end any behaviors
self.end_sim_behavior()
self.mode = mode
