def response(self): """ Parse the response from the serial :return: ack:bool, type_data: int, data:dict """ print_msg(self.name, "In response(), waiting for ack") type_data, data = self.read() if type_data == None: return None, None, None # sensor data if type_data==SENSOR: return False, type_data, data data_dict = json.loads(data) # Patched for bulk messaging if data_dict["function"] in self.non_waiting_commands: # skip concurrent function_code self.credits += 1 print_msg(self.name, "Received one credits: %d/%d"%(self.credits, MAX_CREDITS)) return None, None, None # silence the actual ack data_parsed = self._parse_function_status(data_dict) if data_parsed.get(self.outstanding_command_pair[0], None)==200: # use the function_code to get the status self.outstanding_command_pair = None return True, type_data, data return None, None, None
def command_pop_n_exe(self): """ Peeks the command queue, if not empty, try to execute the command If the command is non_waiting and have sufficient credits, execute it without set the ack; otherwise waits for credits If the command is normal, executes it and waits for ack :return: :except: index error when the commands queue is empty """ command_pair = self.commands_outgoing.queue[0] # peek # Patched for bulk messaging if command_pair[0] in self.non_waiting_commands: if self.credits<=0: return None self.ack = True self.outstanding_command_pair = None self.credits -= 1 else: self.ack = False self.outstanding_command_pair = command_pair command_pair = self.commands_outgoing.get() self.write(command_pair[0], command_pair[1]) print_msg(self.name, "Executing command"+str(command_pair)) return command_pair
def read(self): """ Reads from the serial Line by line Blocking method to get json response :return: parsed json """ receive_data = "" while True: # keep fetching until found json data = self.ser.readline() # waits for the arduino to send a serial and will not continue unless it fetches a serial if "{" in data and "}" in data: # only check for starting "{" # naive type checking receive_data = data[data.find("{"):] break else: # if no json, wait time.sleep(0.05) continue print_msg(self.name, "received serial data: %s"%receive_data) try: receive_data_dict = json.loads(receive_data) if "sensors" in receive_data_dict: return SENSOR, receive_data # return raw data elif "function" in receive_data_dict: return FUNCTION, receive_data # return raw data else: return None, None except ValueError: print_msg(self.name, "json mal-formatted") return None, None
def disconnect(self, *args): """ Waiting for the Progress Terminate signal to close the port Not used yet :param args: """ print_msg(self.name, "Closing serial") if self.ser.isOpen(): self.ser.close()
def communicate_with_pc(self): """ Main Flow """ if self._is_connected(): msg_dict = self.__read() if "map" in msg_dict: if self.android_api != None: print_msg(self.name, "Updating Android's map") self.android_api.map_put(msg_dict["map"], msg_dict["location"]) if "function" in msg_dict: # executing function with parameter function_code = int(msg_dict["function"]) parameter = int(msg_dict["parameter"]) print_msg(self.name, "Executing robot command") self.serial_api.command_put( function_code, parameter) # passing information to Robot if not function_code in self.serial_api.non_waiting_commands: # ack for commands except for 0 1 2 command # waiting for ack while True: lst = self.serial_api.response_pop( ) # send acknowledgement to PC if lst == None: time.sleep(0.05) # 50 ms continue else: print_msg(self.name, "Received acknowledgement") ack, type_data, data = lst[0], lst[1], lst[2] print_msg( self.name, "Acknowledgement: " + str(ack) + str(type_data) + str(data)) try: if json.loads( data )["function"] in self.serial_api.non_waiting_commands: # avoid 0 1 2 continue except KeyError as e: pass sending_msg = data self.__response_to_pc(sending_msg) if ack: break else: sending_msg = json.dumps({ "function": function_code, "status": 200 }) # manual 0 1 2 self.__response_to_pc(sending_msg) else: print_msg(self.name, "Not connected")
def __response_to_pc(self, msg): # taking function code from Robot """ Write response to PC :param msg: String """ msg += "\0" # talking to C thus adding \0 if self._is_connected(): # try: print_msg(self.name, "Writing to PC: %s" % msg) # passing function code to PC self.conn.sendto(msg, self.pc_addr) time.sleep(0.0001) # adjusts thread scheduling and allows the socket I/O to finish FLUSH
def is_ready(self): """ Ready when receiving {"function": 99, "status": 200} :return: """ indicator, dic = self.read() if indicator!=FUNCTION: return if dic.get(99, None)==200: print_msg(self.name, "robot ready") self.ready = True
def explore_run_signal(self): if not self.__explore_sent: if self.android_api.explore_start: print_msg(self.name, 'Sending "explore" signal') self.__response_to_pc("explore") self.__explore_sent = True if not self.__run_sent: if self.android_api.run_start: print_msg(self.name, 'Sending "start" signal') self.__response_to_pc("start") self.__run_sent = True
def explore_run_signal(self): if not self.__explore_sent: if self.android_api.explore_start: print_msg(self.name, "Sending \"explore\" signal") self.__response_to_pc("explore") self.__explore_sent = True if not self.__run_sent: if self.android_api.run_start: print_msg(self.name, "Sending \"start\" signal") self.__response_to_pc("start") self.__run_sent = True
def __response_to_pc(self, msg): # taking function code from Robot """ Write response to PC :param msg: String """ msg += "\0" # talking to C thus adding \0 if self._is_connected(): #try: print_msg(self.name, "Writing to PC: %s" % msg) # passing function code to PC self.conn.sendto(msg, self.pc_addr) time.sleep( 0.0001 ) # adjusts thread scheduling and allows the socket I/O to finish FLUSH
def connect(self): """ TCP connection """ self.conn = socket.socket(socket.AF_INET, socket.SOCK_STREAM) # Create a TCP/IP socket # make port reusable self.conn.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) # To reuse local socket in Time-Wait state self.conn.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1) # Sending broadcast message self.conn.bind((self.tcp_ip, self.port)) self.conn.listen(1) # listening for incoming connection to the IP/Port you got with bind client, addr = self.conn.accept() print_msg(self.name, "Connected! Connection address: " + str(addr)) self.conn = client self.pc_addr = addr self.is_connect = True
def communicate_with_pc(self): """ Main Flow """ if self._is_connected(): msg_dict = self.__read() if "map" in msg_dict: if self.android_api != None: print_msg(self.name, "Updating Android's map") self.android_api.map_put(msg_dict["map"], msg_dict["location"]) if "function" in msg_dict: # executing function with parameter function_code = int(msg_dict["function"]) parameter = int(msg_dict["parameter"]) print_msg(self.name, "Executing robot command") self.serial_api.command_put(function_code, parameter) # passing information to Robot if ( not function_code in self.serial_api.non_waiting_commands ): # ack for commands except for 0 1 2 command # waiting for ack while True: lst = self.serial_api.response_pop() # send acknowledgement to PC if lst == None: time.sleep(0.05) # 50 ms continue else: print_msg(self.name, "Received acknowledgement") ack, type_data, data = lst[0], lst[1], lst[2] print_msg(self.name, "Acknowledgement: " + str(ack) + str(type_data) + str(data)) try: if json.loads(data)["function"] in self.serial_api.non_waiting_commands: # avoid 0 1 2 continue except KeyError as e: pass sending_msg = data self.__response_to_pc(sending_msg) if ack: break else: sending_msg = json.dumps({"function": function_code, "status": 200}) # manual 0 1 2 self.__response_to_pc(sending_msg) else: print_msg(self.name, "Not connected")
def __read(self): """ Read from the socket (json needed) :return: dictionary """ msg = self.conn.recvfrom(1024) msg = msg[0] print_msg(self.name, "Message received: %s" % msg) msg = msg[msg.find("{"):] # cleaning data msg = msg.replace("}\n{", ",") # in case of combined socket try: msg_dict = json.loads(msg) except ValueError as e: print_msg(self.name, "Duplicated Key: " + str(e.message)) msg_dict = msg[msg.find("{"):msg.find("}") + 1] return msg_dict
def __read(self): """ Read from the socket (json needed) :return: dictionary """ msg = self.conn.recvfrom(1024) msg = msg[0] print_msg(self.name, "Message received: %s" % msg) msg = msg[msg.find("{") :] # cleaning data msg = msg.replace("}\n{", ",") # in case of combined socket try: msg_dict = json.loads(msg) except ValueError as e: print_msg(self.name, "Duplicated Key: " + str(e.message)) msg_dict = msg[msg.find("{") : msg.find("}") + 1] return msg_dict
def run(self): self.print_msg("Starting") while True: if self.serial_api.is_command_empty(): # self.print_msg("Waiting for enqueuing command") if self.production: # waiting for request_command to be called time.sleep(0.05) continue else: function_code = int(raw_input("function code: ")) parameter = float(raw_input("parameter: ")) self.serial_api.command_put(function_code, parameter) else: command_pair = self.serial_api.command_pop_n_exe() if command_pair==None: self.print_msg("Waiting for refilling credits") while True: ack, type_data, data = self.serial_api.response() if self.serial_api.credits>=MAX_CREDITS: if self.serial_api.credits>MAX_CREDITS: print_msg(self.name, "ERROR - Received extra credit") self.serial_api.credits=MAX_CREDITS break time.sleep(0.05) elif not command_pair[0] in self.serial_api.non_waiting_commands: # Patched for bulk messaging # self.serial_api.credits = MAX_CREDITS # restore credits # naturally restored in response() self.print_msg("Waiting for normal ack") while True: # stop-n-wait for other non non-waiting commands ack, type_data, data = self.serial_api.response() if ack!=None: self.serial_api.response_put(ack, type_data, data) if ack: break time.sleep(0.05) self.print_msg("Exiting")
def connect(self): """ TCP connection """ self.conn = socket.socket(socket.AF_INET, socket.SOCK_STREAM) # Create a TCP/IP socket #make port reusable self.conn.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) # To reuse local socket in Time-Wait state self.conn.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1) # Sending broadcast message self.conn.bind((self.tcp_ip, self.port)) self.conn.listen( 1 ) # listening for incoming connection to the IP/Port you got with bind client, addr = self.conn.accept() print_msg(self.name, "Connected! Connection address: " + str(addr)) self.conn = client self.pc_addr = addr self.is_connect = True
def _init_serial(self, port, data_rate): """ if the port is specified as None, it automatically tries to find a serial port if the port is specified (usually through command line argument), it open the serial at that port :param port: path to the serial port :param data_rate: int :return: void """ # Serial port: /dev/ttyACM0 # The Raspberry Pi may not provide enough power to drive an Arduino, so you might need external power. self.ser = serial.Serial() self.ser.baudrate = data_rate self.ser.timeout = 1 if port!=None: self.ser.port = port else: port = "/dev/ttyACM0" print_msg(self.name, "Automatically find serial port") for i in range(20): try: port_auto_find = port[:-1] + str(i) self.ser.port = port_auto_find print_msg(self.name, "Finding port at %s"%port_auto_find) self.ser.open() return except serial.SerialException as e: print e.message continue print_msg(self.name, "Automatically find port fails. Try to reboot the linux OS") sys.exit(-1)