def post_process(self,label,msg):
type,msg = label,msg
if (type in CMD_SOURCES and msg.get_msg()==PMessage.M_START_FASTRUN):
# get the fast run commands
self.transit_state(FastRunState)
return [PMessage(type=PMessage.T_COMMAND,msg=PMessage.M_START_FASTRUN)],[]
return [],[]
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 set_start(self):
self.started = True
self.cmd_buffer = self.get_commands_for_fastrun()
move = self.cmd_buffer[0]
self.cmd_buffer = self.cmd_buffer[1:]
self._machine.send_command(move)
self.add_expected_ack(label=ARDUINO_LABEL,msg=PMessage(type=PMessage.T_ROBOT_MOVE,msg=move),call_back=self.continue_sending_command,args=[move])
def process_input(self, label, msg):
if (label in CMD_SOURCES and msg.get_type() == PMessage.T_COMMAND
and msg.get_msg() == PMessage.M_RESET):
self._state.reset_machine()
return [PMessage(type=PMessage.T_COMMAND,
msg=PMessage.M_RESET)], []
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 post_process(self,label,msg):
# get next move
if (label==ARDUINO_LABEL and msg.is_map_update() and (not self._explore_end)):
command = self._explore_algo.get_next_move()
self.add_robot_move_to_be_ack(command)
return [PMessage(type=PMessage.T_COMMAND,msg=command)],\
[]
else:
return [],[]
def set_start(self):
self.started = True
# if the robot is not in correct orientation (cannot face WEST), correct it
if (self._robot_ref.get_orientation()==WEST):
correction_command = PMessage.M_TURN_LEFT
self._machine.send_command(correction_command)
self.add_expected_ack(label=ARDUINO_LABEL,msg=PMessage(type=PMessage.T_ROBOT_MOVE,msg=correction_command),call_back=self.continue_sending_command,args=[correction_command])
else:
self.continue_sending_command()
def process_input(self, label, msg):
# process ack
cmd_ls, data_ls = super(MoveCommandMiddleware,
self).process_input(label, msg)
if (cmd_ls or data_ls):
return cmd_ls, data_ls
# process command
elif (msg.get_type() == PMessage.T_COMMAND
and msg.get_msg() in PMessage.get_valid_move_commands()
and label in CMD_SOURCES):
# send command to arduino, wait till ack and then send update to android
self.add_expected_ack(label=ARDUINO_LABEL,
msg=PMessage(type=PMessage.T_ROBOT_MOVE,
msg=msg.get_msg()),
call_back=self.move_ack_call_back,
args=[msg.get_msg()])
return [msg], []
return [], []
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 continue_sending_command(self,move=None):
if (hasattr(self,"_explore_algo") and move):
# receive ack
self._robot_ref.execute_command(move)
# update android
self.send_robot_update(move)
# if still have commands, send
if (self._robot_ref.get_position()!=self._map_ref.get_end_zone_center_pos()):
new_move = self._explore_algo.get_next_move()
if (self._SEND_CALLIBRATION):
self.send_callibration_msg()
self._machine.send_command(new_move)
self.add_expected_ack(label=ARDUINO_LABEL,msg=PMessage(type=PMessage.T_ROBOT_MOVE,msg=new_move),call_back=self.continue_sending_command,args=[new_move])
else:# end of fast run
self.transit_state(EndState(machine=self._machine))
else:
# init algo
self._explore_algo = MazeExploreAlgo(robot=self._robot_ref,map_ref=self._map_ref)
new_move = self._explore_algo.get_next_move()
self._machine.send_command(new_move)
self.add_expected_ack(label=ARDUINO_LABEL,msg=PMessage(type=PMessage.T_ROBOT_MOVE,msg=new_move),call_back=self.continue_sending_command,args=[new_move])
def post_process(self,label,msg):
"only listen for explore, fast run and move commands"
type,msg = label,msg
# read android command
if (msg.get_type()==PMessage.T_COMMAND):
if (msg.get_msg()==PMessage.M_START_EXPLORE):
self.transit_state(ExplorationState)
return [msg],[PMessage(type=PMessage.T_STATE_CHANGE,msg=msg.get_msg())]
elif(msg.get_msg()==PMessage.M_END_EXPLORE):
self.transit_state(ExplorationDoneState)
#TODO: load map from file, for simulation only
elif(msg.get_type()==PMessage.T_LOAD_MAP):
path = msg.get_msg()
self._map_ref.load_map_from_file(path)
return [],[]
elif(msg.get_type()==PMessage.T_SET_ROBOT_POS):
x,y=msg.get_msg().split(",")
self._robot_ref.set_position((int(x),int(y)))
return [PMessage(type=PMessage.T_SET_ROBOT_POS,msg=msg.get_msg())],[]
return [],[]
def post_process(self,label,msg):
if (self.is_going_back_finished()):
self._machine.end_exploration()
return [],[]
else:
if (not self.is_on_going_back()):
self.started_go_back = True
self._cmd_buffer = self.get_go_back_cmd_list()
move = self.dequeue_buffer()
self._robot_ref.execute_command(move)
self._machine.add_robot_move_to_be_ack(move)
return [PMessage(type=PMessage.T_COMMAND,msg=move)],[]#[PMessage(type=PMessage.T_ROBOT_MOVE,msg=move)]
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 continue_sending_command(self,move):
# receive ack
self._robot_ref.execute_command(move)
# update android
self.send_robot_update(move)
# determine whether need to send callibration msg
if (self._SEND_CALLIBRATION_MSG):
self.send_callibration_msg(always_callibrate_right=True)
# if still have commands, send
if (self.cmd_buffer):
new_move = self.cmd_buffer[0]
self.cmd_buffer = self.cmd_buffer[1:]
self._machine.send_command(new_move)
self.add_expected_ack(label=ARDUINO_LABEL,msg=PMessage(type=PMessage.T_ROBOT_MOVE,msg=new_move),call_back=self.continue_sending_command,args=[new_move])
else:# end of fast run
self.transit_state(EndState(machine=self._machine))
def serve_connection(self, conn):
"get instructions from Rpi and execute"
while True:
msg_obj = conn.read()
if (msg_obj):
self.show_status("received data: " + str(msg_obj))
if (msg_obj.get_type() == PMessage.T_SET_ROBOT_POS):
x, y = msg_obj.get_msg().split(",")
self._map_ref.refresh()
self._robot.set_position((int(x), int(y)))
self.show_status("Robot position set to {},{}".format(
x, y))
continue
elif (msg_obj.get_type() == PMessage.T_COMMAND
and msg_obj.get_msg() == PMessage.M_GET_SENSOR):
self.send_sensor_data()
continue
elif (msg_obj.get_msg() == PMessage.M_START_FASTRUN):
self.send_data(type=PMessage.T_ROBOT_MOVE,
data=PMessage.M_START_FASTRUN)
continue
elif (msg_obj.get_type() == PMessage.T_CALLIBRATE):
self.do_callibration(msg_obj.get_msg())
continue
instruction = self.decode_instruction(msg_obj)
if (instruction):
self.execute_instruction(instruction)
if (self._sending_move_ack
and instruction in PMessage.get_valid_cmd_msgs()):
self.send_data(type=PMessage.T_ROBOT_MOVE,
data=instruction)
if (self._sending_sensor_data):
self.send_sensor_data()
else:
self.show_status("Instruction cannot be decoded!")
def __init__(self,*args,**kwargs):
super(FastRunState,self).__init__(*args,**kwargs)
self.started = False
self.cmd_buffer = []
self.add_expected_ack(label=ARDUINO_LABEL,msg=PMessage(type=PMessage.T_ROBOT_MOVE,msg=PMessage.M_START_FASTRUN),call_back=self.set_start)
def send_data(self, type, data):
msg = PMessage(type=type, msg=data)
self._client.write(msg)
def add_robot_move_to_be_ack(self,move):
self.add_expected_ack(label=ARDUINO_LABEL,msg=PMessage(type=PMessage.T_ROBOT_MOVE,msg=move),call_back=self.ack_move_to_android,args=[move])
