def mqtt_cmd_CB(client, userdata, message):
'''
This is a cmd_CB from MQTT subscribe topic
'''
logger.info("[MQTT] cmd_CB : " + str(message.payload) + "(Q" +
str(message.qos) + ", R" + str(message.retain) + ")")
# Parse payload and Add task to req_list[]
try:
cmd_dict = json.loads(message.payload.decode())
except:
logger.error("[MQTT_cmd_CM] invalid cmd formet ")
return
# ------ Utility cmd ------#
if cmd_dict['cmd'] == 'open':
Ele_Ser.open(cmd_dict['robot_id'], cmd_dict['tid'])
elif cmd_dict['cmd'] == 'close':
Ele_Ser.close(cmd_dict['robot_id'], cmd_dict['tid'])
elif cmd_dict['cmd'] == 'release_button':
Ele_Ser.release_button(cmd_dict['robot_id'], cmd_dict['tid'])
# ------ Often Used cmd ------#
elif cmd_dict['cmd'] == 'call':
Ele_Ser.call(cmd_dict['robot_id'], cmd_dict['tid'],
cmd_dict['current_floor'], cmd_dict['target_floor'])
elif cmd_dict['cmd'] == 'precall':
Ele_Ser.precall(cmd_dict['robot_id'], cmd_dict['tid'],
cmd_dict['current_floor'],
cmd_dict['target_floor'])
elif cmd_dict['cmd'] == 'reboot':
Ele_Ser.reboot(cmd_dict['robot_id'], cmd_dict['tid'],
cmd_dict['pw'])
elif cmd_dict['cmd'] == 'entering_done':
Ele_Ser.entering_done(cmd_dict['robot_id'], cmd_dict['tid'],
cmd_dict['current_floor'],
cmd_dict['target_floor'])
elif cmd_dict['cmd'] == 'release':
Ele_Ser.release(cmd_dict['robot_id'], cmd_dict['tid'])
#---- Test cmd -----#
elif cmd_dict['cmd'] == 'EVledWrite':
ans = Ele_Ser.EVledWrite(cmd_dict['key'], cmd_dict['d'])
topic_list = message.topic.split("/")
mqtt_obj.publish(topic_list[0] + "/" + topic_list[1] + "/reply",
ans,
qos=1,
retain=False)
elif cmd_dict['cmd'] == 'EVledRead':
ans = Ele_Ser.EVledRead(cmd_dict['key'])
topic_list = message.topic.split("/")
mqtt_obj.publish(topic_list[0] + "/" + topic_list[1] + "/reply",
ans,
qos=1,
retain=False)
elif cmd_dict['cmd'] == 'sudo_release':
Ele_Ser.sudo_release()
elif cmd_dict['cmd'] == 'weixin_test':
Ele_Ser.weixin_test()
else:
logger.error(
"[MQTT_cmd_CM] good cmd forment, but don't know this cmd.")
def EVledWrite(self, key, d, retryTime=3):
if d == "high":
sendBuff = "w " + str(key) + " 1"
elif d == "low":
sendBuff = "w " + str(key) + " 0"
try:
self.mq_send.send(sendBuff, priority=9)
except posix_ipc.BusyError: # queue full
logger.error("[EVledWrite] IPC queue full, can't send.")
return 'G'
def set_notify(self, payload={}, topic="topic", qos=2, retain=False):
'''
payload is dict()
You don't have to pass 'topic', 'qos' ... arg , when you switch IS_USING_MQTT to False
'''
if self.state == "stand_by": # Accepted
self.payload = payload
# Input
if IS_USING_HTTP:
self.http_payload = payload.copy()
del self.http_payload['cmd']
self.http_req = AMR_URI + payload['cmd']
self.http_try_times = 0
self.http_thread = None
if IS_USING_MQTT:
self.mqtt_payload = payload
self.topic = topic
self.qos = qos
self.retain = retain
self.mqtt_rc = None
if IS_USING_XBEE:
self.xbee_payload = payload
self.xbee_rc = None
# State
self.state = "trying" # "completed" "abort" "trying"
self.notify_t_start = time.time() # Counting time
#---- MQTT publish (non-blocking) ------#
if IS_USING_MQTT:
if mqtt_obj.available == "online":
self.mqtt_rc = mqtt_obj.publish(
self.topic, json.dumps(self.mqtt_payload), self.qos,
self.retain)
else: #
logger.warning(
"[set_notify] One of the client is not online")
#---- XBEE publish (non-blocking) ------#
if IS_USING_XBEE:
self.xbee_rc = xbee_obj.send(
payload=json.dumps(self.xbee_payload))
#----- HTTP publish (blocking) ---------#
if IS_USING_HTTP:
self.http_thread = threading.Thread(target=self.http_notify)
self.http_thread.start()
else:
logger.error('[notify_agent] Rejected to notify AMR' +
str(http_payload['cmd']) +
', because previous task is not done yet.')
return False
def entering_done(self,
robot_id=0,
tid=0,
current_floor=0,
target_floor=0):
try:
if TM.req_list[0].type != 'call':
logger.error(
"[entering_done] REJECT entering_done. No matched cmd.")
elif TM.req_list[0].robot_id != robot_id:
logger.error(
"[entering_done] REJECT entering_done. robot_id not matched."
)
TM.req_list[0].total_logger.append(
("cmd_entering_done_reject", time.time()))
elif TM.req_list[0].tid != tid:
logger.error(
"[entering_done] REJECT entering_done. tid not matched.")
TM.req_list[0].total_logger.append(
("cmd_entering_done_reject", time.time()))
else:
logger.info(
"[entering_done] Accpeted entering_done cmd. Match tid: " +
str(tid))
TM.req_list[0].total_logger.append(
("cmd_entering_done_accpet", time.time()))
TM.req_list[0].is_entering_done = True
except:
logger.error(
"[entering_done] REJECT entering_done. No matched cmd.")
return str(tid)
def xbee_cmd_CB(msg):
# logger.info("Get msg from main : " + msg)
'''
This is a cmd_CB from MQTT subscribe topic
'''
logger.info("[XBEE] xbee_cmd_CB : " + str(msg))
# Parse payload and Add task to req_list[]
cmd_dict = json.loads(msg)
# ------ Utility cmd ------#
if cmd_dict['cmd'] == 'open':
Ele_Ser.open(cmd_dict['robot_id'], cmd_dict['tid'])
elif cmd_dict['cmd'] == 'close':
Ele_Ser.close(cmd_dict['robot_id'], cmd_dict['tid'])
elif cmd_dict['cmd'] == 'release_button':
Ele_Ser.release_button(cmd_dict['robot_id'], cmd_dict['tid'])
# ------ Often Used cmd ------#
elif cmd_dict['cmd'] == 'call':
Ele_Ser.call(cmd_dict['robot_id'], cmd_dict['tid'],
cmd_dict['current_floor'], cmd_dict['target_floor'])
elif cmd_dict['cmd'] == 'precall':
Ele_Ser.precall(cmd_dict['robot_id'], cmd_dict['tid'],
cmd_dict['current_floor'],
cmd_dict['target_floor'])
elif cmd_dict['cmd'] == 'reboot':
Ele_Ser.reboot(cmd_dict['robot_id'], cmd_dict['tid'],
cmd_dict['pw'])
elif cmd_dict['cmd'] == 'entering_done':
Ele_Ser.entering_done(cmd_dict['robot_id'], cmd_dict['tid'],
cmd_dict['current_floor'],
cmd_dict['target_floor'])
elif cmd_dict['cmd'] == 'release':
Ele_Ser.release(cmd_dict['robot_id'], cmd_dict['tid'])
#---- Test cmd -----# TODO TODO
'''
elif cmd_dict['cmd'] == 'EVledWrite':
ans = Ele_Ser.EVledWrite(cmd_dict['key'], cmd_dict['d'])
topic_list = message.topic.split("/")
mqtt_obj.publish(topic_list[0]+"/"+topic_list[1]+"/reply", ans, qos = 1, retain = False)
elif cmd_dict['cmd'] == 'EVledRead':
ans = Ele_Ser.EVledRead(cmd_dict['key'])
topic_list = message.topic.split("/")
mqtt_obj.publish(topic_list[0]+"/"+topic_list[1]+"/reply", ans, qos = 1, retain = False)
elif cmd_dict['cmd'] == 'sudo_release':
Ele_Ser.sudo_release()
elif cmd_dict['cmd'] == 'weixin_test':
Ele_Ser.weixin_test()
'''
else:
logger.error("[MQTT_cmd_CM] unknow cmd ")
def precall(self, floor=0):
'''
Push floor button when EV is not using.
Input:
floor: Which floor you want to call (cmd.target_floor)
'''
if floor in table:
if not self.is_human_override([]):
self.single_hit(floor) #hit with 0.2sec
logger.info("[precall] Successfully push "+ str(floor) + " floor button.")
else:
logger.info("[precall] Fail to push "+ str(floor) + "floor button, Ev is not vacanccy.")
return
else:
logger.error("[precall] "+str(floor)+" floor is not available!")
def EVledRead(self, key, retryTime=3):
'''
Send IO read cmd to ST_board, And wait for ST answer.
Input:
key: Connector ID
retryTime: How many times to resend command when ST
Output:
Return 1 : ev_led is on
Return 0 : ev_led is off
Return -1 : Error
'''
board = table[key][0]
device = self.device_table[board]
ID = table[key][1] # Connector ID
tStart = time.time()
for i in range(retryTime):
tid = self.getTid()
sendBuff = "[r," + str(ID) + "," + tid + "]"
try:
device.write(sendBuff) #blocking by default
except:
logger.error("[EVledWrite] write fail!")
continue
# Test time spend
respond = self.waitAnswer(device,
tid) # Could be only 'H', 'L', 'E'
if respond == 'H' or respond == 'L':
break
else: # "'E', -1 (timeout) mean error", Need to resend
logger.warning(
"[EVledRead] GOT an ERROR from ST_board, retry" +
str(i + 1) + "th time.")
tEnd = time.time()
# Testing msg
if IS_VERBOSE: # TODO super verbose
logger.debug("[GPIO read ]" + " Board:" + board + " ID:" + ID +
"send:" + sendBuff + " H/L:" + respond +
"deltaT(sec)" + str(tEnd - tStart))
##### Return msg ### NOTE THAT, H/L are inverted.
if respond == "H":
return 0 # LED is off, But GPIO is high
elif respond == 'L':
return 1 # LED is on , But GPIO is low.
else:
return -1 # although retry many time, still can get proper answer.
def main():
global EC, TM
if TM.is_sudo_release:
logger.error("[cherrpy_expose] SUDO RELEASE ACCPECT!!")
EC.release_button()
TM.is_sudo_release = False
TM.req_list = []
# Routine check DOOR OPEN TIMEOUT
if EC.door_release_checker(): # TIMEOUT
EC.release_button()
if len(TM.req_list) != 0 and TM.req_list[0].type == 'call':
TM.req_list[0].is_timeout_release = True
if len(TM.req_list) == 0: # Nothing to do
pass
else: # Keep doing cmd
if TM.req_list[0].type == 'open':
EC.open()
TM.delTask(0)
elif TM.req_list[0].type == 'close':
EC.close()
TM.delTask(0)
elif TM.req_list[0].type == 'release_button':
EC.release_button()
TM.delTask(0)
elif TM.req_list[0].type == 'call':
EC.call_iterateOnce(TM.req_list[0])
# Check Finish
if TM.req_list[0].state == "finish":
logger.info("[Main] END CALL(tid: " + str(TM.req_list[0].tid) +
")")
TM.req_list[0].total_logger.append(("end", time.time()))
TM.req_list[0].total_logging()
EC.release_button()
TM.delTask(0)
elif TM.req_list[0].type == 'precall':
EC.precall(TM.req_list[0].target_floor)
TM.delTask(0)
elif TM.req_list[0].type == 'reboot':
EC.reboot()
TM.delTask(0)
else:
pass
def EVledRead(self, key, retryTime=3):
try:
self.mq_send.send("r " + str(key), priority=9)
except posix_ipc.BusyError: # queue full
logger.error("[EVledRead] IPC queue full!.")
return -1 # Error queue full
else:
# Wait for answer for 1 sec.
t_start_wait = time.time()
while time.time() - t_start_wait < 1: # 1 sec
try:
r = self.mq_recv.receive()[0].decode()
# logger.info("[EVledRead]" + r)
except posix_ipc.BusyError:
pass # queue empty, keep waiting
else: # Get msg
return int(r)
logger.error("[EVledRead] TIMEOUT!.")
return -1 # Error , timeout
def abort_handle(self):
#----- Print Out log and rss -------#
if IS_USING_XBEE:
logger.error('[notify_agent] XBEE wait for send AWK for ' +
str(time.time() - self.notify_t_start) +
' sec, abort.')
if is_using_rss:
alarm.sent('[notify_agent] XBEE wait for send AWK for ' +
str(time.time() - self.notify_t_start) +
' sec, abort.')
if IS_USING_MQTT:
logger.error('[notify_agent] MQTT wait for publish callback for ' +
str(time.time() - self.notify_t_start) +
' sec, abort.')
if is_using_rss:
alarm.sent(
'[notify_agent] MQTT wait for publish callback for ' +
str(time.time() - self.notify_t_start) + ' sec, abort.')
if IS_USING_HTTP:
logger.error('[notify_agent] retried ' + str(self.http_try_times) +
' times, but still failed to notify AMR ' +
str(self.http_req) + ', give it up.')
if is_using_rss:
alarm.sent('[notify_agent] retried ' +
str(self.http_try_times) +
' times, but still failed to notify AMR ' +
str(self.http_req) + ', give it up.')
self.state = "abort"
def release(self, robot_id=0, tid=0, current_floor=0, target_floor=0):
'''
release current mission
'''
try:
if TM.req_list[0].type != 'call':
logger.error("[release] REJECT release. No matched cmd.")
elif TM.req_list[0].robot_id != robot_id:
logger.error("[release] REJECT release. robot_id not matched.")
TM.req_list[0].total_logger.append(
("cmd_release_reject", time.time()))
elif TM.req_list[0].tid != tid:
logger.error("[release] REJECT release. tid not matched.")
TM.req_list[0].total_logger.append(
("cmd_release_reject", time.time()))
else:
logger.info("[release] Accpeted release cmd. Match tid: " +
str(tid))
TM.req_list[0].total_logger.append(
("cmd_release_accpet", time.time()))
TM.req_list[0].is_release = True
except:
logger.error("[release] REJECT release. No matched cmd.")
return str(tid)
def call_iterateOnce(self, cmd):
'''
This is a non-blocking function
'''
if not cmd.is_release:
#######################################
######### Loop routine #########
#######################################
###### Door monitor, door_state could be 'closed', 'opened', 'closing', 'opening' ,'unknown'
if ENABLE_VERIFY_DOOR_STATUS:
open_key_state = self.pb.EVledRead('open')
close_key_state = self.pb.EVledRead('close')
if open_key_state == 0 and close_key_state == 0: # opened or closed # Exit for this function !!
if cmd.door_state == 'opening':
cmd.door_state = 'opened'
logger.info("[Door State] |<-- -->| Fully Open.")
if cmd.door_state == 'closing':
cmd.door_state = 'closed'
logger.info("[Door State] -->||<-- Fully Closed.")
elif open_key_state == 1 and close_key_state == 0:
if not cmd.door_state == 'opening':
logger.info("[Door State] <<<-- -->>> Opening.")
cmd.door_state = 'opening'
elif open_key_state == 0 and close_key_state == 1:
if not cmd.door_state == "closing":
logger.info("[Door State] -->>> <<<-- Closing.")
cmd.door_state = 'closing'
elif open_key_state == 1 and close_key_state == 1:
logger.error("[Door State] Both open and close are high, can't decide door state.")
else: #ST ERROR
logger.warning("[Door State] ST Read ERROR, can't confirm door status")
else: # Not able to verify door status, so we could only close eye and wait for door open, pity...
pass
#######################################
######### AMR Notify #########
#######################################
'''
TODO : This block should be written as a state
'''
if cmd.na.state == "stand_by": # Nothing to notify
pass
else:
cmd.na.notify_iterateOnce()
if cmd.na.state == "completed":
if cmd.na.payload['cmd'] == "cancel":
cmd.total_logger.append(("notify_cancel_success", time.time()))
cmd.state = "finish"
elif cmd.na.payload['cmd'] == "reached":
cmd.total_logger.append(("notify_reached_success", time.time()))
if cmd.pre_state == "moving2current":
cmd.switch_state("enteringEV") # switch state will auto clear notify_agent
elif cmd.pre_state == "moving2target":
cmd.switch_state("alightingEV")# switch state will auto clear notify_agent
elif cmd.na.state == "abort":
cmd.total_logger.append(("netowork_abort", time.time()))
cmd.state = "finish"
elif cmd.na.state == "trying":
pass
else:
pass
return # When something to notify, only doing notify
# First Init
if cmd.state == "":
cmd.switch_state("wait_vacancy")
#######################################
######### Wait vacancy ##########
#######################################
if cmd.state == "wait_vacancy":
if time.time() - cmd.t_into_state > WAIT_REACH_LIMIT_TIME:
logger.warning("[call:"+cmd.state +"] wait vacancy for " + str(WAIT_REACH_LIMIT_TIME) +"sec , timeout!")
# Notify
cmd.na.set_notify({'cmd' : "cancel", 'robot_id' : cmd.robot_id}, topic = cmd.robot_id+"/"+CLIENT_NAME+"/reply" , qos = 2, retain = False )
if not self.is_human_override([]): # no human is using EV. # OR opend OR waiting for door open
if cmd.slience_counter >= SLIENCE_MIN_COUNTER:
cmd.switch_state("moving2current")
# t_into_state = time.time() # avoid a endless loop # TODO Human override bug
else:
cmd.slience_counter += 1
logger.debug("[call:"+cmd.state +"] Slience, " + str(cmd.slience_counter) + " / " + str(SLIENCE_MIN_COUNTER))
else: # HUMAN
logger.debug("[call:"+cmd.state +"] Human detected, reset slience counter")
cmd.slience_counter = 0 # Keep waiting
#####################################################
######### moving2current and moving2target #####
#####################################################
elif cmd.state == "moving2current" or cmd.state == "moving2target":
if cmd.state == "moving2current":
push_floor = cmd.current_floor
if self.is_human_override([cmd.current_floor, 'open', 'close']): # Go back to wait_vacancy
logger.info("[call:"+cmd.state +"] Human detected, go back to wait vacanccy")
# t_into_state = time.time() # avoid endless loop # TODO Human override bug
cmd.switch_state("wait_vacancy")
elif cmd.state == "moving2target":
push_floor = cmd.target_floor
if time.time() - cmd.t_into_state > WAIT_REACH_LIMIT_TIME:
logger.warning("[call:"+cmd.state +"] EV not reach exceed "+ str(WAIT_REACH_LIMIT_TIME) + " sec, timeout")
if is_using_rss:
alarm.sent("EV Moving for " + str(WAIT_REACH_LIMIT_TIME) + "sec, but still can't arrived floor, MISSON ABORT !")
cmd.na.set_notify({'cmd' : "cancel", 'robot_id' : cmd.robot_id}, topic = cmd.robot_id+"/"+CLIENT_NAME+"/reply" , qos = 2, retain = False )
GPIO_state = self.pb.EVledRead(push_floor, retryTime=3)
if GPIO_state == 0: # led off
if cmd.t_start_hit_floor == 0: # First push button.
self.single_hit(push_floor, 0.2)
cmd.t_start_hit_floor = time.time()
logger.info("[call:"+cmd.state +"] Pushed " + str(push_floor) + " F button.")
else:
# logger.info("[call:"+cmd.state +"] Takes " + str(time.time() - cmd.t_start_hit_floor) + " sec to extinguish floor button LED.")
cmd.floor_led_record.append( round(time.time() - cmd.t_start_hit_floor, 2)) # 10Hz while loop, it's enough
logger.info("[call:"+cmd.state +"] Led_record: " + str(cmd.floor_led_record))
if not self.floor_confirm(cmd.floor_led_record): # Repush button
time.sleep(0.2)
self.single_hit(push_floor, 0.2)
cmd.t_start_hit_floor = time.time()
logger.info("[call:"+cmd.state +"] "+ str(push_floor) + "F push.")
else: # confirm Arried
logger.info("[call:"+cmd.state +"] Arrived "+ str(push_floor) + " F, takes "+str(round(time.time() - cmd.t_into_state,2))+" sec. (" + str(WAIT_REACH_LIMIT_TIME) + ")")
self.open()
cmd.switch_state("waitDoorOpen")
elif GPIO_state == 1: # led on , Keep waiting
logger.debug("[call:"+cmd.state +"]"+ str(push_floor) + "F LED is still on.( "+str(time.time()-cmd.t_into_state)+"/"+ str(WAIT_REACH_LIMIT_TIME)+")")
else: # "Err"
logger.warning("[call:"+cmd.state +"] Get error from ST read.")
#####################################
######### waitDoorOpen ##########
#####################################
elif cmd.state == "waitDoorOpen":
dT = time.time() - cmd.t_into_state
if dT >= WAIT_DOOR_LIMIT: # Can't wait timeout
cmd.switch_state(cmd.pre_state)
if not ENABLE_VERIFY_DOOR_STATUS:
if dT >= CLOSE_EYE_WAIT_DOOR_SEC:
cmd.door_state = 'opened'
if not cmd.door_state == 'opened':
logger.debug("[call:"+cmd.state +"] wait for door opened.")
else: # cmd.door_state == 'opened'
if not ENABLE_VERIFY_DOOR_STATUS: # if is close-eyes wait for door, need to switch flag back.
cmd.door_state = 'closed'
if cmd.pre_state == "moving2current":
cmd.na.set_notify({'cmd' : "reached", 'robot_id' : cmd.robot_id, 'floor' : cmd.current_floor}, topic = cmd.robot_id+"/"+CLIENT_NAME+"/reply" , qos = 2, retain = False )
elif cmd.pre_state == "moving2target":
cmd.na.set_notify({'cmd' : "reached", 'robot_id' : cmd.robot_id, 'floor' : cmd.target_floor }, topic = cmd.robot_id+"/"+CLIENT_NAME+"/reply" , qos = 2, retain = False )
#####################################
######### enteringEV ##########
#####################################
elif cmd.state == "enteringEV":
if cmd.is_timeout_release:
cmd.na.set_notify({'cmd' : "cancel", 'robot_id' : cmd.robot_id}, topic = cmd.robot_id+"/"+CLIENT_NAME+"/reply" , qos = 2, retain = False )
if cmd.is_entering_done:
logger.info("[call:"+cmd.state +"] get entering_done."+"("+ str(round(time.time()-cmd.t_into_state,2))+"/"+str(DOOR_OPEN_LIMIT_TIME)+" sec)")
self.close()
cmd.is_entering_done = False
cmd.switch_state("moving2target")
else:
logger.debug("[call:"+cmd.state +"] Wait for entering done. takes " + str(time.time()-cmd.t_into_state) + " sec.")
#####################################
######### alightingEV #########
#####################################
elif cmd.state == "alightingEV":
if cmd.is_timeout_release:
cmd.na.set_notify({'cmd' : "cancel", 'robot_id' : cmd.robot_id}, topic = cmd.robot_id+"/"+CLIENT_NAME+"/reply" , qos = 2, retain = False )
else:
logger.debug("[call:"+cmd.state +"] Wait for release. ( " + str(round(time.time()-cmd.t_into_state,2)) + "/" + str(DOOR_OPEN_LIMIT_TIME)+" )")
else:
cmd.state = "finish"
return
"tid": tid
})
# ------ Often Used cmd ------#
elif user_type_list[0] == 'call':
try:
current_floor = user_type_list[1]
target_floor = user_type_list[2]
paylaod_dict = json.dumps({
"cmd": user_type_list[0],
"robot_id": robot_id,
"tid": tid,
"current_floor": current_floor,
"target_floor": target_floor
})
except:
logger.error("[MQTT_cmd_CM] unknow cmd ")
elif user_type_list[0] == 'precall':
try:
target_floor = user_type_list[1]
paylaod_dict = json.dumps({
"cmd": user_type_list[0],
"robot_id": robot_id,
"tid": tid,
"current_floor": current_floor,
"target_floor": target_floor
})
except:
logger.error("[MQTT_cmd_CM] unknow cmd ")
elif user_type_list[0] == 'enter':
paylaod_dict = json.dumps({
"cmd": "entering_done",
def waitAnswer(self, device, tid):
'''
Wait answer from ST_board(tid must match) and parse it, can't block exceed REC_TIMEOUT
Input:
tid : which tid you are expecting for response.
Output:
return -1 : timeout
return ans(str) from ST_board : H , L , G, E
Note: this function is called by EVLedRead() and EvledWrite()
'''
# receiving package for GPIORead
rec_state = waiting
recbuf = ""
rec_timeout_counter = 0
tStartWait = time.time()
while time.time() - tStartWait < REC_TIMEOUT: # ~= 0.1 sec
is_completed = False
is_valid = False
if device.inWaiting(
) != 0: # Wait for ST_board answer, Should be '[H]' or '[L]'
try: #
rec = device.read(
) # Read one byte # blocking for 0.5sec(define by configuration)
except:
logger.error("[EVwaitAnswer] read fail")
continue
#print "rec = ", rec
#receive ST_board answer
if rec_state == waiting:
if rec == START_CHAR:
rec_state = receiving
elif rec_state == receiving:
if rec == END_CHAR: # is_completed
rec_state = waiting
is_completed = True
elif rec == START_CHAR:
recbuf = ""
else:
recbuf += rec
else:
pass
if is_completed:
#print "recbuf:", recbuf
##################### Parse Data
recbufArr = list()
for i in range(NUM_OF_DATA):
interval_idx = recbuf.find(INTERVAL_CHAR)
if interval_idx != -1: # found a interval char
recbufArr.append(recbuf[:interval_idx])
recbuf = recbuf[interval_idx + 1:]
else: # interval not found, Last data?
if recbuf == "":
pass
else:
recbufArr.append(recbuf)
##################### Valid check
if len(recbufArr) == 2:
if self.is_ans(recbufArr[0]) and recbufArr[
1] == tid: # if tid is not match keep while.
is_valid = True
##################### return respond
if __debug__:
#print "recbufArr: ", recbufArr
pass
if is_valid:
return recbufArr[0]
# End of is_completed
rec_timeout_counter += 1
time.sleep(0.001) #TODO test#sleep 1ms, check device for every 1ms
# End of while
return -1
