Exemplo n.º 1
0
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)
Exemplo n.º 2
0
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"
Exemplo n.º 3
0
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