def _load_map_file(self, filename): #TODO: so far this is hardcoded, only read 15*20 map with open(filename, "rb") as f: content = f.read() arr = bitarray() arr.frombytes(content) debug("Map loaded: {}".format(self.to_hex(arr.tobytes())), DEBUG_COMMON) explored = arr[2:302] # explored or unknown cleared = arr[304:] # cleared or obstacle # load into a 2d array ls = [[None for _ in range(15)] for __ in range(20)] cleared_cur_index = 0 for i in range(len(explored)): x = i % 15 y = i / 15 if (explored[i] == self.bit_unknown): ls[y][x] = MapSetting.UNKNOWN else: # explored if (cleared[cleared_cur_index] == self.bit_clear): ls[y][x] = MapSetting.CLEAR else: #obstacle ls[y][x] = MapSetting.OBSTACLE cleared_cur_index += 1 if (any([ True if ls[y][x] == None else False for y in range(20) for x in range(15) ])): raise Exception("The map data is not complete") if (not self._is_top_down): ls = list(reversed(ls)) return ls
def write_to_interface(from_queue, interface): """thread task for writing to android,pc,arduino""" while True: if (not from_queue.empty()) and interface.is_ready(): val = from_queue.get_nowait() debug("get {} from queue to write".format(val), DEBUG_IO_QUEUE) interface.write(val)
def set_next_state(self, state): debug("Next state set to {}".format(str(state)), DEBUG_STATES) for q in self._data_out_qs: self._enqueue_list( q=q, list=[PMessage(type=PMessage.T_STATE_CHANGE, msg=str(state))]) self._state = state
def move_along_wall(self): # check whether the robot has gone into a cycle if (self._CHECK_HISTORY_FOR_LOOP): if ((self._robot.get_position(), self._robot.get_orientation()) in self._history_ls): self.action_precedence = [ PMessage.M_TURN_RIGHT, PMessage.M_MOVE_FORWARD, PMessage.M_TURN_LEFT ] debug("Loop detected, go back", DEBUG_ALGO) self._history_ls = [] return PMessage.M_TURN_BACK else: debug( "History: pos({}), orientation({})".format( self._robot.get_position(), self._robot.get_orientation()), DEBUG_ALGO) self._history_ls.append((self._robot.get_position(), self._robot.get_orientation())) for action in self.action_precedence: ori = self.get_ori_to_check(desired_action=action) status = self.check_status(ori) if (status != self.CANNOT_ACCESS): self.action_precedence = self.PRECEDENCE_UPDATE_DICT[action][ status] return action return PMessage.M_TURN_BACK
def connect(self): debug("waiting to connect to {}".format(self._name), DEBUG_INTERFACE) self._connection = self._connect(server_ip=self._server_ip, server_port=self._server_port) self.set_ready() debug( "{} connected to {}:{}".format(self._name, self._server_ip, self._server_port), DEBUG_INTERFACE)
def write(self, msg): try: msg = msg.render_msg() self.client_sock.send(msg) time.sleep(self._write_delay) debug("BT--Write to Android: %s" % str(msg), DEBUG_INTERFACE) except Exception, e: debug("BT--write exception: %s" % str(e), DEBUG_INTERFACE)
def disconnect(self): try: self.client_sock.close() self.server_sock.close() self.set_not_ready() debug("BT--Disconnected to Android!", DEBUG_INTERFACE) except Exception, e: debug("BT--disconnection exception: %s" % str(e), DEBUG_INTERFACE)
def post_process(self,label,msg): cmd_ls,data_ls = super(ExplorationFirstRoundStateWithTimer,self).post_process(label,msg) if (cmd_ls): # get new command, stop the previous timer and start a new timer self._timer.shutdown() debug("Get new command, try to start timer",DEBUG_STATES) self._timer.start() return cmd_ls,data_ls
def read_from_interface(to_queue, interface, label): """thread task for reading from android, arduino""" while True: if (interface.is_ready()): val = interface.read() if val: debug("get {} from interface to enqueue".format(val), DEBUG_IO_QUEUE) to_queue.put_nowait((label, val))
def runA3(): argparser = argparse.ArgumentParser(description="Assignment 3") argparser.add_argument('--no-extract', '-e', dest="no_extract", action='store_true', help='skip extraction of zip file') argparser.add_argument('--no-compile', '-c', dest="no_compile", action='store_true', help='skip compilation stage') argparser.add_argument('--only-extract', '-E', dest="only_extract", action='store_true', help='only extract D2L zip file') argparser.add_argument('mainZip', type=str, help='D2L zip file') argparser.add_argument('rootDir', type=str, help='output folder') argparser.add_argument('student', type=str, nargs='?', help='student name') args = argparser.parse_args(sys.argv[1:]) mainZip = Path(args.mainZip) rootDir = Path(args.rootDir) if (not rootDir.exists() or not mainZip.exists()): print("Invalid rootDir or main zip file") exit(-1) scratchDir = (rootDir / "scratch").resolve() scratchDir.mkdir(exist_ok=True) markingDir = (rootDir / "marking").resolve() markingDir.mkdir(exist_ok=True) testCasesDir: Path = (rootDir / "test_cases").resolve() extraSourceDir: Path = (rootDir / "java_files").resolve() runConfig = RunConfig(args.no_extract, args.no_compile, args.only_extract, args.student) workspace = Workspace(rootDir, scratchDir, markingDir, mainZip, testCasesDir, extraSourceDir) debug("Running with \n config=%s \n in %s" % (str(runConfig), str(workspace))) skipExtract = runConfig.noExtract or runConfig.student extractMain(workspace, clean=not skipExtract) for submission in allZipFilesIn(workspace.scratchDir)[:]: studentWorkspace: StudentWorkspace = createStudentWorkspace(workspace, submission) # run for everyone or only the student specified if not runConfig.student or runConfig.student == studentWorkspace.studentDirName: print("Running %s" % studentWorkspace.studentDirName) if not skipExtract: extractStudent(studentWorkspace) if not runConfig.onlyExtract: runTarget = getRunTarget(studentWorkspace) if runTarget: extraClasses = copyExtraSource(workspace.extraSourceDir, runTarget) classNames = ["RunSystem"] + extraClasses if not runConfig.noCompile: compileRunTarget(studentWorkspace, runTarget, classNames) runA3Tests(workspace.testCasesDir, studentWorkspace, runTarget, override=True) validateA3(workspace.testCasesDir, studentWorkspace, override=True) print("\n")
def execute_command(self, command): if (command == PMessage.M_TURN_RIGHT): self.turn_right() elif (command == PMessage.M_TURN_LEFT): self.turn_left() elif (command == PMessage.M_MOVE_FORWARD): self.move_forward() elif (command == PMessage.M_TURN_BACK): self.turn_back() elif (command.find(PMessage.M_MOVE_FORWARD) != -1): _, grid = command.split("*") self.move_forward(num_grids=int(grid)) else: debug( "Command {} is not a valid command for robot".format(command), DEBUG_COMMON)
def process(self, label, pmsg): """ :param label: ANDROID_LABEL or ARDUINO_LABEL or PC_LABEL :param pmsg: PMessage object :return: cmd_list,data_list """ if (not label in VALID_LABELS): debug("label '{}' is not valid".format(label), DEBUG_MIDDLEWARE) return [], [] elif (not isinstance(pmsg, PMessage)): debug("object {} is not a PMessage object".format(pmsg), DEBUG_MIDDLEWARE) return [], [] return self.process_input(label, pmsg)
def _connect(self, server_ip, server_port): sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) server_address = (server_ip, server_port) connected = False while (not connected): try: sock.connect(server_address) connected = True except Exception as e: debug( "Socket server failed to connect to {}, message: {}". format(server_ip, e.message), DEBUG_INTERFACE) debug("Retrying...", DEBUG_INTERFACE) time.sleep(1) return sock
def connect(self): if self.ser is not None: self.ser.close() time.sleep(2) try: self.port_no = self.port_no ^ 1 self.ser = serial.Serial(SER_PORT + str(self.port_no), SER_BAUD) time.sleep(2) if self.ser is not None: self.set_ready() self.status = True debug("SER--Connected to Arduino!", DEBUG_INTERFACE) except Exception, e: debug("SER--connection exception: %s" % str(e), DEBUG_INTERFACE) self.reconnect()
def write(self, msg): """msg is a Message object""" if (not self._connection): raise Exception("connectio not ready, cannot write") data = msg.render_msg() debug("{} Writing data: {}".format(self._name, data), DEBUG_INTERFACE) try: self._connection.sendall(data) if (self._write_delay): time.sleep(self._write_delay) except Exception as e: debug("Write exception: {}".format(e), DEBUG_INTERFACE) if (hasattr(e, 'errno') and getattr(e, 'errno') == 10054): self.reconnect() return None
def process_input(self, label, msg): "check whether the input is the expected ack message" if (not self._ack_q.is_empty()): ack_item = self._ack_q.peek() if (ack_item['label'] == label and msg.equals(ack_item['msg'])): call_back = ack_item['call_back'] call_back_args = ack_item['args'] self._ack_q.dequeue() try: cmd_list, data_list = call_back( *call_back_args) if call_back_args else call_back() return cmd_list, data_list except Exception as e: debug( "ack call back function doesn't return two lists: {}". format(e), DEBUG_MIDDLEWARE) return [], [] return [], []
def runA1(): if len(sys.argv) != 3: print(""" Usage: ./scripts/markall.py <zipfile> <root folder> """) exit(0) mainZip = Path(sys.argv[1]) rootDir = Path(sys.argv[2]) if (not rootDir.exists() or not mainZip.exists()): print("Invalid rootDir or main zip file") exit(-1) scratchDir = (rootDir / "scratch").resolve() scratchDir.mkdir(exist_ok=True) markingDir = (rootDir / "marking").resolve() markingDir.mkdir(exist_ok=True) testCasesDir: Path = (rootDir / "test_cases").resolve() workspace = Workspace(rootDir, scratchDir, markingDir, mainZip, testCasesDir) debug("Running with %s" % str(workspace)) extractMain(workspace, clean=True) skipExtract = False for submission in allZipFilesIn(workspace.scratchDir)[:]: studentWorkspace: StudentWorkspace = createStudentWorkspace( workspace, submission) print("Running %s" % studentWorkspace.studentDirName) if not skipExtract: extractStudent(studentWorkspace) runTarget = getRunTarget(studentWorkspace) if runTarget: compileRunTarget(studentWorkspace, runTarget) runA1Tests(studentWorkspace, runTarget, override=True) validateA1(workspace.testCasesDir, studentWorkspace, override=True) print("\n")
def process_input(self,label,msg): "process using middleware" cmd_ls,data_ls = [],[] for mid_ware in self._middlewares: try: cmds,datas = mid_ware.process(label,msg) except: continue cmd_ls = cmd_ls + cmds data_ls = data_ls + datas # stop processing when this mid does return something and it wants to consume the input if (mid_ware.is_consume_input() and (cmds or datas)): return cmd_ls,data_ls # call final processing function try: cmds,datas = self.post_process(label,msg) return cmd_ls+cmds,data_ls+datas except Exception as e: debug("Post process error: {}".format(e),DEBUG_STATES) return cmd_ls,data_ls
def get_callibration_msgs(self,sides): "return a list of PMessage" if (len(sides)==1 and sides[0]==RIGHT and self._robot_ref.has_continuous_straight_moves(3)): # if right side fully blocked, send callibration if there's at least 3 straight moves debug("more than 3 straight moves in a row",DEBUG_STATES) self._robot_ref.clear_history() return [PMessage(type=PMessage.T_CALLIBRATE,msg=PMessage.M_CALLIBRATE_RIGHT)] elif(len(sides)==1 and sides[0]==FRONT): # if front side fully blocked, callibrate return [PMessage(type=PMessage.T_CALLIBRATE,msg=PMessage.M_CALLIBRATE_FRONT)] elif (len(sides)>1): # if at corner, callibrate ORI_TO_MSG = { FRONT:PMessage.M_CALLIBRATE_FRONT, LEFT:PMessage.M_CALLIBRATE_LEFT, RIGHT: PMessage.M_CALLIBRATE_RIGHT } return [PMessage(type=PMessage.T_CALLIBRATE,msg=ORI_TO_MSG[s]) for s in sides] else: return [],[]
def write(self, msg): try: msg = msg.get_msg() debug( str(current_milli_time()) + "SER--Write to Arduino b4: %s" % str(msg), DEBUG_INTERFACE) realmsg = "" if msg in TO_SER: realmsg = TO_SER.get(msg) if realmsg == "0": realmsg = "0a" elif len(msg.split('*')) == 2: msg = msg.split('*') tmp = int(msg[1]) + 96 realmsg = TO_SER.get(msg[0]) + chr(tmp) if realmsg: debug( str(current_milli_time()) + "SER--Write to Arduino: %s" % str(realmsg), DEBUG_INTERFACE) self.ser.write(realmsg) if realmsg == 'i' or realmsg == 'o': time.sleep(self._calib_delay - self._write_delay) time.sleep(self._write_delay) except Exception, e: debug( str(current_milli_time()) + "SER--write exception: %s" % str(e), DEBUG_INTERFACE) self.reconnect()
def move_along_wall_efficient(self): "under dev" #TODO: change this code and test if ((self._robot.get_position(), self._robot.get_orientation()) in self._history_ls): debug("Loop detected, go back", DEBUG_ALGO) self.action_precedence = [ PMessage.M_TURN_RIGHT, PMessage.M_MOVE_FORWARD, PMessage.M_TURN_LEFT ] return PMessage.M_TURN_BACK else: debug( "History: pos({}), orientation({})".format( self._robot.get_position(), self._robot.get_orientation()), DEBUG_ALGO) self._history_ls.append( (self._robot.get_position(), self._robot.get_orientation())) candidate_actions = [] # list of (action,utility) for action in self.action_precedence: ori = self.get_ori_to_check(desired_action=action) status = self.check_status(ori) if (status != self.CANNOT_ACCESS): utility = self._robot.get_action_utility_points( action=action, map_ref=self._map_ref) if (not candidate_actions): candidate_actions.append((action, utility)) else: if (utility == candidate_actions[0][1]): candidate_actions.append((action, utility)) elif (utility > candidate_actions[0][1]): candidate_actions = [(action, utility)] if (candidate_actions): #pick the action with highest utility return candidate_actions[random.randint(0, len(candidate_actions) - 1)][0] else: # if no action can be done, then try going back return PMessage.M_TURN_BACK
def read(self): """return a Message object, None if invalid message is received""" if (not self._msg_buffer.is_empty()): return self._msg_buffer.dequeue() if (not self._connection): raise Exception("connection not ready, cannot read") data = None try: data = self._connection.recv(self._recv_size) except Exception as e: debug("Read exception: {}".format(e), DEBUG_INTERFACE) if (hasattr(e, 'errno') and getattr(e, 'errno') == 10054): self.reconnect() return None if (data): debug("Received data from {} : {}".format(self._name, data), DEBUG_INTERFACE) try: pmsgs = PMessage.load_messages_from_json(json_str=data) if (pmsgs): for msg in pmsgs: self._msg_buffer.enqueue(msg) return self._msg_buffer.dequeue() except ValidationException as e: debug("Validation exception: {}".format(e.message), DEBUG_VALIDATION) return None
def connect(self): connected = False while (not connected): try: self.server_sock = BluetoothSocket(RFCOMM) self.server_sock.bind(("", BT_PORT)) self.server_sock.listen(2) port = self.server_sock.getsockname()[1] advertise_service( self.server_sock, "SampleServer", service_id=BT_UUID, service_classes=[BT_UUID, SERIAL_PORT_CLASS], profiles=[SERIAL_PORT_PROFILE], ) self.client_sock, client_info = self.server_sock.accept() debug( "BT--Connected to %s on channel %s" % (str(client_info), str(port)), DEBUG_INTERFACE) self.set_ready() return True except Exception, e: debug("BT--connection exception: %s" % str(e), DEBUG_INTERFACE)
def _save_map_file(self, filename, td_array): if (not self._is_top_down): td_array = list(reversed(td_array)) explored_ls = [] # record explored or unknown cleared_ls = [] # record clear or obstacle explored_ls.extend([1, 1]) # prefix for y in range(len(td_array)): for x in range(len(td_array[0])): if td_array[y][x] == MapSetting.UNKNOWN: explored_ls.append(self.bit_unknown) else: # explored explored_ls.append(self.bit_explored) if (td_array[y][x] == MapSetting.OBSTACLE): cleared_ls.append(self.bit_obstacle) else: cleared_ls.append(self.bit_clear) explored_ls.extend([1, 1]) # postfix # concatenate the two lists result_ls = self._pad_zero_right(explored_ls + cleared_ls) arr = bitarray([result_ls[i] == 1 for i in range(len(result_ls))]) debug("Map saved: {}".format(self.to_hex(arr.tobytes())), DEBUG_COMMON) # write the binary string to file with open(filename, 'wb') as f: f.write(arr.tobytes())
def run(self): debug("{} timer starts".format(self._name), DEBUG_TIMER) self._remaining_time = self._time_limit self._is_timing = True while (self._remaining_time > 0 and self._is_timing): debug( "{} timer remaining time: {}".format(self._name, self._remaining_time), DEBUG_TIMER) if (self._interval_callback): self._interval_callback(self._remaining_time) time.sleep(self._interval) self._remaining_time -= 1 if (self._end_callback and self._remaining_time == 0): debug("{} timer times up!".format(self._name), DEBUG_TIMER) self._end_callback() else: debug( "{} timer forced to stop! remaining time: {}".format( self._name, self._remaining_time), DEBUG_TIMER) self._is_timing = False
def read(self): if (not self._msg_buffer.is_empty()): return self._msg_buffer.dequeue() try: msg = self.client_sock.recv(2048) debug("BT--Read from Android: %s" % str(msg), DEBUG_INTERFACE) pmsgs = PMessage.load_messages_from_json(json_str=msg) if (pmsgs): for msg in pmsgs: self._msg_buffer.enqueue(msg) return self._msg_buffer.dequeue() except ValidationException as e: debug("Validation exception: {}".format(e.message), DEBUG_VALIDATION) except Exception, e: debug("BT--read exception: %s" % str(e), DEBUG_INTERFACE)
def ack_move_to_android(self,move): self._robot_ref.execute_command(move) self._map_ref.set_fixed_cells(self._robot_ref.get_occupied_postions(),MapSetting.CLEAR) debug("Current robot position:{}".format(self._robot_ref.get_position()),DEBUG_STATES) debug("Current map coverage: {}".format(100-self._map_ref.get_unknown_percentage()),DEBUG_STATES) if(self._robot_ref.get_position()==self._map_ref.get_start_zone_center_pos() and 100-self._map_ref.get_unknown_percentage()>self._end_coverage_threshold): debug("Ending Exploration",DEBUG_STATES) self.trigger_end_exploration() # if (self._USE_ROBOT_STATUS_UPDATE): # data_ls = [PMessage(type=PMessage.T_UPDATE_ROBOT_STATUS,msg="{},{},{}".format( # self._robot_ref.get_position()[0], # self._robot_ref.get_position()[1], # self._robot_ref.get_orientation().get_value() # ))] # else: # data_ls = [PMessage(type=PMessage.T_ROBOT_MOVE,msg=move)] # # return [],data_ls return [],[]
def read(self): try: msg = self.ser.readline().rstrip() if msg != "": debug( str(current_milli_time()) + "SER--Read from Arduino: %s" % str(msg), DEBUG_INTERFACE) if msg[0] != 'T' and len(msg.split(',')) == 6: realmsg = PMessage(type=PMessage.T_MAP_UPDATE, msg=msg) debug( str(current_milli_time()) + "SER--Read from Arduino after1: %s" % str(realmsg), DEBUG_INTERFACE) return realmsg elif msg[0] != 'T' and msg in FROM_SER: if msg[0] <= '8': realmsg = PMessage(type=PMessage.T_ROBOT_MOVE, msg=FROM_SER.get(msg[0])) debug( str(current_milli_time()) + "SER--Read from Arduino after2: %s" % str(realmsg), DEBUG_INTERFACE) return realmsg elif msg[0] != 'T' and len(msg) > 1: tmp = ord(msg[1]) - 96 if tmp > 1: msg = FROM_SER.get(msg[0]) + "*" + str(tmp) else: msg = FROM_SER.get(msg[0]) realmsg = PMessage(type=PMessage.T_ROBOT_MOVE, msg=msg) debug( str(current_milli_time()) + "SER--Read from Arduino after3: %s" % str(realmsg), DEBUG_INTERFACE) return realmsg except ValidationException as e: debug( str(current_milli_time()) + "validation exception: {}".format(e.message), DEBUG_VALIDATION) except Exception, e: debug( str(current_milli_time()) + "SER--read exception: %s" % str(e), DEBUG_INTERFACE) self.reconnect()
def set_next_state(self,st): self._state = st debug("state changed to {}".format(st),DEBUG_STATES)
def javaCompile(markingDir: Path, outDir: Path, cpDir: Path, file: str): command = "javac -d \"%s\" -cp \"%s\" \"%s\"/%s" % (str(outDir), str(cpDir), str(cpDir), file) debug(command) os.system("%s >> \"%s\"/compile.out 2>&1" % (command, str(markingDir)))