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