def sendUIUpdate(stat, vcl, spot):
print ("sending " + stat + " message from vehicle " + str(vcl) + " to UI")
msg = list_ui()
msg.status = stat
msg.vcl_id = vcl
msg.spot_id = spot[0]
pub1.publish(msg)
def callback():
print "entering callback"
global liq, vcl_dict, liq_id, isUI, vcl_spot, hello_dict, vcl_id, vcl_motion, hellocount
while True:
while ser.inWaiting()>0:
print "received message"
rec_msg = getmsg()
if rec_msg is not None:
print "original dict"
print vcl_dict
# INTRO XBEE
if rec_msg.message_type == 'INTRO':
print "I received INTRO from ", rec_msg.vcl_id
# if somehow two vehicles got the same ID OR
# if intro vcl has id greater than that in local dict
# then bump up person ID, replace dict and send update
if rec_msg.vcl_id == vcl_id or rec_msg.vcl_id > liq_id:
print 'rec_msg.data'
print rec_msg.data
for key, val in rec_msg.data.iteritems():
vcl_dict[key] = val
liq_id = max(vcl_dict.keys())
vcl_id = liq_id + 1
vcl_dict[vcl_id] = [vcl_spot, vcl_motion]
liq_id = vcl_id
print "I'm the liq"
print "Sending update because erroneous INTRO received"
msg = send_XBee_msg(vcl_id, 'UPDATE', {vcl_id:vcl_dict[vcl_id]})
ser.write(msg)
if vcl_id == 0:
vcl_id = max(rec_msg.data.keys())+1
vcl_dict = rec_msg.data
vcl_dict[vcl_id] = [vcl_spot, vcl_motion]
liq_id = vcl_id
print "I'm the liq"
print "Sending update because I have arrived"
msg = send_XBee_msg(vcl_id, 'UPDATE', {vcl_id:vcl_dict[vcl_id]})
ser.write(msg)
#Tell the UI that I, and all my friends, exist.
if isUI:
for key, val in vcl_dict.iteritems():
msg = list_ui()
if val[0] == 0:
sendUIUpdate('in_queue', key, val)
elif val[0] == 25 and val[1] == 1:
sendUIUpdate('returning', key, val)
elif val[0] == 25 and val[1] == 0:
sendUIUpdate('returned', key, val)
elif val[1] == 1: #moving and spot != 0 and spot != 25
sendUIUpdate('parking', key, val)
elif val[1] == 0:
sendUIUpdate('parked', key, val)
else:
print ("ERROR: unknown status in dict: " + str(key) + " " + str(val))
# HELLO XBEE
if rec_msg.message_type == 'HELLO':
print "I received HELLO from ", rec_msg.vcl_id
# add liq_id+1 to hellos or increment val if necessary
if liq_id+1 in hello_dict:
hello_dict[liq_id+1] = hello_dict[liq_id+1]+1
else:
hello_dict[liq_id+1] = 1
# find second to liq_id. if len(dict) < 2, stliq_id is the liq_id
stliq_id = vcl_dict.keys()
if len(vcl_dict) >= 2:
stliq_id.remove(max(stliq_id))
stliq_id = max(stliq_id)
else:
stliq_id = liq_id
# if i am liq, send intro
print 'liq'
print liq_id
print 'vcl_id'
print vcl_id
if liq_id == vcl_id:
msg = send_XBee_msg(vcl_id, 'INTRO', vcl_dict)
ser.write(msg)
# if this is the second hello from new vehicle AND i am second-to-liq, send intro, update dict, update liq
elif hello_dict[liq_id+1]>= 2 and vcl_id == stliq_id:
vcl_dict[liq_id] = [0,0]
msg = send_XBee_msg(vcl_id, 'INTRO', vcl_dict)
ser.write(msg)
# UPDATE XBEE
if rec_msg.message_type == 'UPDATE':
print "I received UPDATE from ", rec_msg.vcl_id
vcl_dict[rec_msg.vcl_id] = rec_msg.data[rec_msg.vcl_id]
liq_id = max(vcl_dict.keys())
if isUI:
if vcl_dict[rec_msg.vcl_id][0] == 0:
sendUIUpdate('in_queue', rec_msg.vcl_id, vcl_dict[rec_msg.vcl_id])
elif vcl_dict[rec_msg.vcl_id][0] < 25 and vcl_dict[rec_msg.vcl_id][1] == 1:
sendUIUpdate('parking', rec_msg.vcl_id, vcl_dict[rec_msg.vcl_id])
elif vcl_dict[rec_msg.vcl_id][0] < 25 and vcl_dict[rec_msg.vcl_id][1] == 0:
sendUIUpdate('parked', rec_msg.vcl_id, vcl_dict[rec_msg.vcl_id])
elif vcl_dict[rec_msg.vcl_id][0] == 25 and vcl_dict[rec_msg.vcl_id][1] == 1:
sendUIUpdate('returning', rec_msg.vcl_id, vcl_dict[rec_msg.vcl_id])
elif vcl_dict[rec_msg.vcl_id][0] == 25 and vcl_dict[rec_msg.vcl_id][1] == 0:
sendUIUpdate('returned', rec_msg.vcl_id, vcl_dict[rec_msg.vcl_id])
else:
print ("ERROR: unknown status received: " + str(rec_msg.vcl_id) + " " + str(vcl_dict[rec_msg.vcl_id]))
# PARKED XBEE
if rec_msg.message_type == 'PARKED':
print "I received PARKED from ", rec_msg.vcl_id
vcl_dict[rec_msg.vcl_id] = rec_msg.data[rec_msg.vcl_id]
if isUI:
sendUIUpdate('parked', rec_msg.vcl_id, vcl_dict[rec_msg.vcl_id])
# GOODBYE XBEE
if rec_msg.message_type == 'GOODBYE':
print "I received GOODBYE from ", rec_msg.vcl_id
if rec_msg.vcl_id in vcl_dict:
vcl_dict.pop(rec_msg.vcl_id)
if len(vcl_dict.keys()) > 0:
liq_id = max(vcl_dict.keys())
if isUI:
sendUIUpdate('returned', rec_msg.vcl_id, [0, 0])
print "updated dict"
print vcl_dict
else:
print "ERROR RECEIVED BAD DATA"
impath = os.path.join(os.path.dirname(__file__), 'demo_template.jpg')
img = cv2.imread(impath)
a = img
spot_id = {1: (175, 260), 2: (241, 260), 3: (307, 260), 4: (373, 260),
5: (439, 260), 6: (505, 260), 7: (175, 326), 8: (241, 326),
9: (307, 326), 10: (373, 326), 11: (439, 326), 12: (505, 326),
13: (175, 524), 14: (241, 524), 15: (307, 524), 16: (373, 524),
17: (439, 524), 18: (505, 524), 19: (175, 590), 20: (241, 590),
21: (307, 590), 22: (373, 590), 23: (439, 590), 24: (505, 590),
25: (0,0)}
queue = 0
returning = 0
rospy.init_node("GUI")
pub1 = rospy.Publisher("ui_update",list_ui, queue_size = 10)
pub2 = rospy.Publisher("vv_update", String, queue_size = 10)
msg = list_ui()
current_state = {}
virtualcount = 0
max_virtual_vehicles = 99
time.sleep(3)
# remove all virtual vehicles from parking lot when GUI is closed from terminal
def signal_handler(signal, frame):
global virtualcount, max_virtual_vehicles
for key in current_state.keys():
if key <= max_virtual_vehicles:
sendToXBee(key,25,0)
time.sleep(1)
msg.status = "returned"
msg.vcl_id = key
