def start(): # 启动GPS GPS_thread = GPS() GPS_thread.start() # 启动提示预警 remind_thread = remind.remind_thread() remind_thread.start()
class Monitor(): def __init__(self): #super(Monitor,self).__init__() ecuWorkerPipe = PipeCont() # ECU <-> Worker ecuDataPipe = PipeCont() # ECU <-> Collector ecuControlPipe = PipeCont() # ECU <-> Application workerDataPipe = PipeCont() # Worker <-> Collector workerControlPipe = PipeCont() # Worker <-> Application collectorControlPipe = PipeCont() # Collector <-> Application loggerControlPipe = PipeCont() # Logger <-> Application loggerDataPipe = PipeCont() # Logger <-> Collector loggerWorkerPipe = PipeCont() # Logger <-> Worker gpsControlPipe = PipeCont() # GPS <-> Application self.__events = {} workQue = Queue() resultQue = Queue() self.__pipes = {} self.__pipes['ECU'] = PipeWatcher(self, ecuControlPipe.s, 'ECU->APP') self.__pipes['WORKER'] = PipeWatcher(self, workerControlPipe.s, 'WORKER->APP') self.__pipes['COLLECTOR'] = PipeWatcher(self, collectorControlPipe.s, 'COLLECTOR->APP') self.__pipes['LOG'] = PipeWatcher(self, loggerControlPipe.s, 'LOG->APP') self.__pipes['GPS'] = PipeWatcher(self, gpsControlPipe.s, 'GPS->APP') self.__event = {} self.__event['ISCONNECTED'] = Event() self.__event['GETQUEUES'] = Event() self.__event['GETCOMMANDS'] = Event() self.__event['SUPPORTED_COMMANDS'] = Event() self.__event['GETSTATUS'] = Event() self.__event['SUM'] = Event() self.__event['AVG'] = Event() self.__event['MIN'] = Event() self.__event['MAX'] = Event() self.__event['VAL'] = Event() self.__event['DATALINE'] = Event() self.__event['LOGNAME'] = Event() self.__event['SNAPSHOT'] = Event() self.__event['SUMMARY'] = Event() self.__ecu = ECU( workQue, ecuWorkerPipe.s, # ECU <-> Worker ecuControlPipe.r, # ECU <-> Application ecuDataPipe.s) # ECU <-> Collector self.__worker = Worker( workQue, resultQue, ecuWorkerPipe.r, # Worker <-> ECU workerControlPipe.r, # Worker <-> Application workerDataPipe.s, # Worker <-> Collector loggerWorkerPipe.s) # Worker <-> Logger self.__collector = Collector( ecuDataPipe.r, # Collector <-> ECU collectorControlPipe.r, # Collector <-> Application loggerDataPipe.r, # Collector <-> Logger workerDataPipe.r, # Collector <-> Worker resultQue) self.__logger = DataLogger( loggerControlPipe.r, # Logger <-> Application loggerDataPipe.s, # Logger <-> Collector loggerWorkerPipe.r) # Logger <-> Worker self.__gps = GPS(resultQue, gpsControlPipe.r) # GPS <-> Application self.gpsEnable = config.getboolean('Application', 'GPS Enabled') self.__ecu.start() self.__collector.start() self.__worker.start() self.__logger.start() self.__gps.start() for p in self.__pipes: self.__pipes[p].start() # monitor needs to listen for a connection event triggered by WORKER def connection(self): if self.isConnected: self.resume() else: self.pause() def addQue(self, que, frequency): self.__pipes['ECU'].send( Message('ADDQUE', QUE=que, FREQUENCY=frequency)) def addCommand(self, que, command, override=False): self.__pipes['ECU'].send( Message('ADDCOMMAND', QUE=que, COMMAND=command, OVERRIDE=override)) def setQueFrequency(self, que, frequency): self.__pipes['ECU'].send( Message('SETFREQUENCY', QUE=que, FREQUENCY=frequency)) def deleteAfterPoll(self, que, flag): self.__pipes['ECU'].send(Message('DELETEAFTERPOLL', QUE=que, FLAG=flag)) def stop(self): self.__pipes['ECU'].send(Message('STOP')) self.__pipes['COLLECTOR'].send(Message('STOP')) self.__pipes['WORKER'].send(Message('STOP')) self.__pipes['LOG'].send(Message('STOP')) self.__pipes['GPS'].send(Message('STOP')) logging.info('Joining processes') self.__collector.join() logging.info('Collector joined') self.__worker.join() logging.info('Worker joined') self.__logger.join() logging.info('Logger joined') self.__gps.join() logging.info('GPS Joined') self.__ecu.join() logging.info('ECU Joined') def pause(self): self.__pipes['ECU'].send(Message('PAUSE')) self.__pipes['LOG'].send(Message('PAUSE')) self.__pipes['GPS'].send(Message('PAUSE')) def resume(self): logger.info('Resuming co-processes') self.__pipes['ECU'].send(Message('RESUME')) self.__pipes['LOG'].send(Message('RESUME')) self.__pipes['GPS'].send(Message('RESUME')) def reset(self): self.__pipes['COLLECTOR'].send(Message('RESET')) def save(self): self.__pipes['LOG'].send(Message('SAVE')) def discard(self): self.__pipes['LOG'].send(Message('DISCARD')) def logPath(self, path): self.__pipes['LOG'].send(Message('LOGPATH', PATH=path)) def logFrequency(self, frequency): self.__pipes['LOG'].send(Message('FREQUENCY', FREQUENCY=frequency)) def logHeadings(self, headings): self.__pipes['LOG'].send( Message('ADD_HEADINGS', HEADINGS=headings, INSERT=True)) @property def isConnected(self): self.__connected_return = None # Stores the response from the callback self.__pipes['WORKER'].send( Message('CONNECTED')) # Send message for incomming request self.__event['ISCONNECTED'].wait() self.__event['ISCONNECTED'].clear() return self.__connected_return[ 'STATUS'] # Return the response from the callback to the caller # Callback function must be lower case of the message it is to respond to def connected(self, p): # Callback function for IsConnected self.__connected_return = p # Store the response returned for the caller to find self.__event['ISCONNECTED'].set() @property def queues(self): self.__queues_return = None # Stores the response from the callback self.__pipes['ECU'].send( Message('GETQUEUES')) # Send message for incomming request self.__event['GETQUEUES'].wait() self.__event['GETQUEUES'].clear() return self.__queues_return[ 'QUEUES'] # Return the response from the callback to the caller # Callback function must be lower case of the message it is to respond to def getqueues(self, p): # Callback function for IsConnected self.__queues_return = p # Store the response returned for the caller to find self.__event['GETQUEUES'].set() @property def commands(self): self.__commands_return = None # Stores the response from the callback self.__pipes['WORKER'].send( Message('GETCOMMANDS')) # Send message for incomming request self.__event['GETCOMMANDS'].wait() self.__event['GETCOMMANDS'].clear() return self.__commands_return[ 'COMMANDS'] # Return the response from the callback to the caller # Callback function must be lower case of the message it is to respond to def getcommands(self, p): # Callback function for IsConnected self.__commands_return = p # Store the response returned for the caller to find self.__event['GETCOMMANDS'].set() @property def supportedCommands(self): self.__s_commands_return = None # Stores the response from the callback self.__pipes['WORKER'].send(Message( 'SUPPORTED_COMMANDS')) # Send message for incomming request self.__event['SUPPORTED_COMMANDS'].wait() self.__event['SUPPORTED_COMMANDS'].clear() return self.__s_commands_return[ 'SUPPORTED_COMMANDS'] # Return the response from the callback to the caller # Callback function must be lower case of the message it is to respond to def supported_commands(self, p): # Callback function for IsConnected self.__s_commands_return = p # Store the response returned for the caller to find self.__event['SUPPORTED_COMMANDS'].set() @property def status(self): s = {} self.__s_ecu_return = None # Stores the response from the callback self.__s_data_return = None # Stores the response from the callback self.__s_worker_return = None # Stores the response from the callback self.__s_log_return = None # Stores the response from the callback self.__s_gps_return = None # Stores the response from the callback self.__pipes['WORKER'].send( Message('GETSTATUS')) # Send message for incomming request self.__event['GETSTATUS'].wait() self.__event['GETSTATUS'].clear() s['WORKER'] = self.__s_worker_return['STATUS'] self.__pipes['ECU'].send( Message('GETSTATUS')) # Send message for incomming request self.__event['GETSTATUS'].wait() self.__event['GETSTATUS'].clear() s['ECU'] = self.__s_ecu_return['STATUS'] self.__pipes['COLLECTOR'].send( Message('GETSTATUS')) # Send message for incomming request self.__event['GETSTATUS'].wait() self.__event['GETSTATUS'].clear() s['COLLECTOR'] = self.__s_data_return['STATUS'] self.__pipes['LOG'].send( Message('GETSTATUS')) # Send message for incomming request self.__event['GETSTATUS'].wait() self.__event['GETSTATUS'].clear() s['LOG'] = self.__s_log_return['STATUS'] self.__pipes['GPS'].send( Message('GETSTATUS')) # Send message for incomming request self.__event['GETSTATUS'].wait() self.__event['GETSTATUS'].clear() s['GPS'] = self.__s_gps_return['STATUS'] return s # Return the response from the callback to the caller # Callback function must be lower case of the message it is to respond to def workerstatus(self, p): # Callback function for IsConnected self.__s_worker_return = p # Store the response returned for the caller to find self.__event['GETSTATUS'].set() # Callback function must be lower case of the message it is to respond to def ecustatus(self, p): # Callback function for IsConnected self.__s_ecu_return = p # Store the response returned for the caller to find self.__event['GETSTATUS'].set() # Callback function must be lower case of the message it is to respond to def datastatus(self, p): # Callback function for IsConnected self.__s_data_return = p # Store the response returned for the caller to find self.__event['GETSTATUS'].set() # Callback function must be lower case of the message it is to respond to def logstatus(self, p): # Callback function for IsConnected self.__s_log_return = p # Store the response returned for the caller to find self.__event['GETSTATUS'].set() # Callback function must be lower case of the message it is to respond to def gpsstatus(self, p): # Callback function for IsConnected self.__s_gps_return = p # Store the response returned for the caller to find self.__event['GETSTATUS'].set() def sum(self, name): self.__sum_return = None # Stores the response from the callback self.__pipes['COLLECTOR'].send(Message( 'SUM', NAME=name)) # Send message for incomming request self.__event['SUM'].wait() self.__event['SUM'].clear() return self.__sum_return[ 'SUM'] # Return the response from the callback to the caller # Callback function must be lower case of the message it is to respond to def getsum(self, p): # Callback function for IsConnected self.__sum_return = p # Store the response returned for the caller to find self.__event['SUM'].set() def avg(self, name): self.__avg_return = None # Stores the response from the callback self.__pipes['COLLECTOR'].send(Message( 'AVG', NAME=name)) # Send message for incomming request self.__event['AVG'].wait() self.__event['AVG'].clear() return self.__avg_return[ 'AVG'] # Return the response from the callback to the caller # Callback function must be lower case of the message it is to respond to def getavg(self, p): # Callback function for IsConnected self.__avg_return = p # Store the response returned for the caller to find self.__event['AVG'].set() def min(self, name): self.__min_return = None # Stores the response from the callback self.__pipes['COLLECTOR'].send(Message( 'MIN', NAME=name)) # Send message for incomming request self.__event['MIN'].wait() self.__event['MIN'].clear() return self.__MIN_return[ 'MIN'] # Return the response from the callback to the caller # Callback function must be lower case of the message it is to respond to def getmin(self, p): # Callback function for IsConnected self.__min_return = p # Store the response returned for the caller to find self.__event['MIN'].set() def max(self, name): self.__max_return = None # Stores the response from the callback self.__pipes['COLLECTOR'].send(Message( 'MAX', NAME=name)) # Send message for incomming request self.__event['MAX'].wait() self.__event['MAX'].clear() return self.__max_return[ 'MAX'] # Return the response from the callback to the caller # Callback function must be lower case of the message it is to respond to def getmax(self, p): # Callback function for IsConnected self.__max_return = p # Store the response returned for the caller to find self.__event['MAX'].set() def val(self, name): self.__val_return = None # Stores the response from the callback self.__pipes['COLLECTOR'].send(Message( 'VAL', NAME=name)) # Send message for incomming request self.__event['VAL'].wait() self.__event['VAL'].clear() return self.__val_return[ 'VAL'] # Return the response from the callback to the caller # Callback function must be lower case of the message it is to respond to def getval(self, p): # Callback function for IsConnected self.__val_return = p # Store the response returned for the caller to find self.__event['VAL'].set() def dataLine(self, name): self.__dataline_return = None # Stores the response from the callback self.__pipes['COLLECTOR'].send(Message( 'DATALINE', NAME=name)) # Send message for incomming request self.__event['DATALINE'].wait() self.__event['DATALINE'].clear() return self.__dataline_return[ 'LINE'] # Return the response from the callback to the caller # Callback function must be lower case of the message it is to respond to def data_line(self, p): # Callback function for IsConnected self.__dataline_return = p # Store the response returned for the caller to find self.__event['DATALINE'].set() @property def gpsEnable(self): return self.__gpsEnabled @gpsEnable.setter def gpsEnable(self, v): self.__gpsEnabled = v if getBlockPath(config.get('Application', 'GPS Vendor ID'), config.get('Application', 'GPS Product ID')) is None: logger.warning('GPS Device not found. Disabling GPS.') self.__gpsEnabled = False if self.__gpsEnabled: #self.__pipes['GPS'].send(Message('RESUME')) self.__pipes['LOG'].send( Message('ADD_HEADINGS', HEADINGS=[ 'LATITUDE', 'LONGITUDE', 'ALTITUDE', 'GPS_SPD', 'HEADING' ])) else: self.__pipes['GPS'].send(Message('PAUSE')) self.__pipes['LOG'].send( Message('REMOVE_HEADINGS', HEADINGS=[ 'LATITUDE', 'LONGITUDE', 'ALTITUDE', 'GPS_SPD', 'HEADING' ])) @property def snapshot(self): self.__snapshot_return = None # Stores the response from the callback self.__pipes['COLLECTOR'].send( Message('SNAPSHOT')) # Send message for incomming request self.__event['SNAPSHOT'].wait() self.__event['SNAPSHOT'].clear() return self.__snapshot_return[ 'SNAPSHOT'] # Return the response from the callback to the caller # Callback function must be lower case of the message it is to respond to def snap_shot(self, p): # Callback function for IsConnected self.__snapshot_return = p # Store the response returned for the caller to find self.__event['SNAPSHOT'].set() @property def logName(self): self.__logname_return = None # Stores the response from the callback self.__pipes['LOG'].send( Message('LOGNAME')) # Send message for incomming request self.__event['LOGNAME'].wait() self.__event['LOGNAME'].clear() return self.__logname_return[ 'NAME'] # Return the response from the callback to the caller # Callback function must be lower case of the message it is to respond to def log_name(self, p): # Callback function for IsConnected self.__logname_return = p # Store the response returned for the caller to find self.__event['LOGNAME'].set() @property def summary(self): self.__summary_return = None # Stores the response from the callback self.__pipes['COLLECTOR'].send( Message('SUMMARY')) # Send message for incomming request self.__event['SUMMARY'].wait() self.__event['SUMMARY'].clear() return self.__summary_return[ 'SUMMARY'] # Return the response from the callback to the caller # Callback function must be lower case of the message it is to respond to def getsummary(self, p): # Callback function for IsConnected self.__summary_return = p # Store the response returned for the caller to find self.__event['SUMMARY'].set()
from gps import GPS from remind import remind_thread if __name__ == '__main__': # 启动GPS GPS_thread = GPS() GPS_thread.start() # 启动提示预警 remind_thread = remind_thread() remind_thread.start() # 启动RGB灯带
from gps import GPS from rfcomm import RFCOMM import time import subprocess print("STARTING...") rf = RFCOMM() gps = GPS() subprocess.run(["hciconfig", "hci0", "piscan"]) while True: if rf.client_connected: data = gps.update() if data is not None: print("DATA: " + data) rf.send_data(data + "\r\n") time.sleep(0.1) else: gps.stop() print("WAIT FOR CLIENT...") rf.wait_for_client() print("CLIENT ACCEPTED") gps.start() print("ENABLE GPS")
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