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)
