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)))
