BachelorThesis/BachelorThesis/Software/PyPrecisionGUI/live_demo.py

import matplotlib.pyplot as plt
import numpy as np
from matplotlib.patches import Ellipse
from matplotlib.patches import Circle
import matplotlib.pyplot as plt
from mpl_toolkits.axes_grid1.anchored_artists import AnchoredDrawingArea
import matplotlib.animation as anm
import random
import time
import queue
from threading import Thread
import socket

FRAME_RATE = 0.05

def generate_results_thread():
    UDP_IP = "localhost"
    UDP_PORT = 5678

    sock = socket.socket(socket.AF_INET,  # Internet
                         socket.SOCK_DGRAM)  # UDP
    #sock.bind((UDP_IP, UDP_PORT))
    startTime = time.time()
    originalTime = time.time()
    while True:

        if time.time() - startTime > FRAME_RATE:
            x = random.uniform(0, 1.5)
            y = random.uniform(0, 1.20)
            z = random.uniform(-0.2, 0.2)
            #np.save('result', np.array([x, y, theta]))
            #outputQueue.put(([x,y,z]))
            sock.sendto(("%f; %f; %f" %(x, y, z)).encode('utf-8'), ('localhost', 5678))
            startTime = time.time()
def recieving_thread(outputQueue):
    UDP_IP = "localhost"
    UDP_PORT = 4589

    sock = socket.socket(socket.AF_INET,  # Internet
                         socket.SOCK_DGRAM)  # UDP
    sock.bind((UDP_IP, UDP_PORT))
    originalTime = time.time()
    counter = 0
    while True:
        counter += 1
        if time.time() - originalTime > 1:
            print('recivingThread counter = ' +str(counter))
            counter = 0
            originalTime = time.time()
        data, addr = sock.recvfrom(1024)  # buffer size is 1024 bytes
        data = data.decode().split(';')
        data = [float(data[0]), float(data[1]), float(data[2])]#, float(data[3]), float(data[4]), float(data[5])]

        outputQueue.put(data)

class LiveDemo:

    def __init__(self, setupDimensions=[0.65,0.65], antennaLength=0.2, antennaWidth=0.2,
                 figureTitle='test', showAntennas=False, showCompartments=False):


        self.antennas_pos = [[0, 0.59, 0], [0, 1.28, 0],[0, 1.97, 0]
        ,[0.7, 2.57, 0],[1.5, 2.57, 0],[2.3, 2.57, 0], [3.1, 2.57, 0]

        , [3.9, 2.57, 0],[4.7, 2.57, 0], [5.4, 1.97, 0], [5.4, 1.28, 0],[5.4, 0.59, 0]

        , [4.7, 0, 0], [3.9, 0, 0],[3.1, 0, 0],[2.3, 0, 0]

        ,[1.5, 0, 0],[0.7, 0, 0]]

        self.setupDimensions = setupDimensions
        self.antennaLength = antennaLength
        self.antennaWidth =  antennaWidth
        self.figureTitle = figureTitle
        self.showAntennas = showAntennas
        self.showCompartments = showCompartments
        xyTitle = 16
        xyTicksSize = 12
        legendSize = 14
        self.titleFontSize = 40
        self.figure = plt.figure(num=1, figsize=(16,9), dpi=70, facecolor='w', edgecolor='k')
        self.figure.suptitle(self.figureTitle, size=self.titleFontSize)
        # fig, (ax1, ax2) = plt.subplots(1, 2)
        self.ax = plt.subplot2grid((1, 3), (0, 0), colspan=3)
        #self.ax1 = plt.subplot2grid((1, 3), (0, 2), colspan=1, sharey=self.ax)

        self.x1 = 0.5
        self.x2 = 0.56
        self.x3 = 0.9
        self.x4 = 1
        self.y1 = 0.4
        self.y2 = 0.5
        # Dimensions of the measurements setup [Width, hight] in meters
        # Setup borders dashed line:
        x = [0, 0, self.setupDimensions[0], self.setupDimensions[0], 0]
        y = [0, self.setupDimensions[1], self.setupDimensions[1], 0, 0]
        self.ax.plot(x, y, linestyle='dashed', color='grey', label='Shelf borders', linewidth=6)
        if self.showCompartments:
            ### Compartment1 #######
            x = [0, 0, self.x1, self.x1, 0]
            y = [self.y2, self.setupDimensions[1], self.setupDimensions[1], self.y2, self.y2]
            self.ax.plot(x, y, color='blue', label='Comps. 1-6', linewidth=4)

            ### Compartment2 #######
            x = [self.x2, self.x2, self.x3, self.x3, self.x2]
            y = [self.y2, self.setupDimensions[1], self.setupDimensions[1], self.y2, self.y2]
            self.ax.plot(x, y, color='blue', linewidth=4)

            ### Compartment3 #######
            x = [self.x4, self.x4, self.setupDimensions[0], self.setupDimensions[0], self.x4]
            y = [self.y2, self.setupDimensions[1], self.setupDimensions[1], self.y2, self.y2]
            self.ax.plot(x, y, color='blue', linewidth=4)

            ### Compartment4 #######
            x = [0, 0, self.x1, self.x1, 0]
            y = [0.0, self.y1, self.y1, 0.0, 0.0]
            self.ax.plot(x, y, color='blue', linewidth=4)

            ### Compartment5 #######
            x = [self.x2, self.x2, self.x3, self.x3, self.x2]
            y = [0.0, self.y1, self.y1, 0.0, 0.0]
            self.ax.plot(x, y, color='blue', linewidth=4)

            ### Compartment6 #######
            x = [self.x4, self.x4, self.setupDimensions[0], self.setupDimensions[0], self.x4]
            y = [0.0, self.y1, self.y1, 0.0, 0.0]
            self.ax.plot(x, y, color='blue', linewidth=4)

        if self.showAntennas:
            for pos in self.antennas_pos:
                x = [pos[0]-(self.antennaLength/2), pos[0]+(self.antennaLength/2),
                     pos[0] + (self.antennaLength / 2), pos[0]-(self.antennaLength/2), pos[0]-(self.antennaLength/2)]
                y = [pos[1]-(self.antennaLength/2), pos[1]-(self.antennaLength/2),
                     pos[1] + (self.antennaLength / 2), pos[1]+(self.antennaLength/2), pos[1]-(self.antennaLength/2)]
                self.ax.plot(x, y, color='blue', linewidth=4)



        self.ax.set_xlim(-0.2, self.setupDimensions[0] + 0.2)
        self.ax.set_ylim(-0.2, self.setupDimensions[1] + 0.2)
        self.ax.set_xlabel('X (m)', fontsize=xyTitle+6)
        self.ax.set_ylabel('Y (m)', fontsize=xyTitle+6)
        self.ax.tick_params(labelsize=xyTicksSize+10)
        z = [-0.2, -0.2, 0.2, 0.2, -0.2]
        y1 = [0, self.setupDimensions[1], self.setupDimensions[1], 0, 0]
        #self.ax1.plot(z, y1, linestyle='dashed', color='grey', linewidth=2)
        #self.ax1.axvline(linewidth=1, color='r')

        # self.plot_points()

        #self.ax1.set_xlim(-0.4, 0.4)
        #self.ax1.set_ylim(-0.5, self.setupDimensions[1] + 0.5)
        #self.ax1.set_xlabel('Z (m)', fontsize=xyTitle)
        #self.ax1.tick_params(labelsize=xyTicksSize)
        #self.ax1.grid()
        self.ax.grid()
        self.ax.axvline(x=0, linestyle='-', color='black', linewidth=8, label='z = 0 cm')
        # self.ax1.legend(prop={'size': legendSize - 3})
        self.scatterArtist = self.ax.scatter(0, 0, c='y', s=500, marker='o',
                                             label='object (x,y)')
        self.scatterArtist1 = self.ax.scatter(0, 0, c='y', s=500, marker='o',
                                             label='object (z,y)')

        self.scatterArtist2 = self.ax.scatter(0, 0, c='b', s=500, marker='o',
                                             label='object (x,y)')
        self.scatterArtist3 = self.ax.scatter(0, 0, c='b', s=500, marker='o',
                                              label='object (z,y)')
        #self.scatterArtist1 = self.ax1.scatter(0, 0, c='r', s=200, marker='o',
        #                                       label='Estimated position')
        self.result = [2.5, 2.5, 45]
        self.outputQueue = queue.Queue()
        self.ax.legend(prop={'size': legendSize - 3})
        #self.create_setup()
        self.plottedSamples = 0
        self.startTime = time.time()

    def create_setup(self):
        self.scatterArtist.set_offsets(np.array([-1, -1]))
        self.scatterArtist1.set_offsets(np.array([-1, -1]))
        self.scatterArtist2.set_offsets(np.array([-1, -1]))
        self.scatterArtist3.set_offsets(np.array([-1, -1]))
#        self.scatterArtist1.set_offsets(np.array([0, 0]))
        return self.scatterArtist, self.scatterArtist1,


    def plot_results(self, i):
        try:
            if not self.outputQueue.empty():
                self.result = self.outputQueue.get()
                if self.result[0] != -1:
                    self.scatterArtist.set_offsets(np.array([self.result[0], self.result[1]]))
                    self.scatterArtist1.set_offsets(np.array([self.result[2], self.result[1]]))
                # if self.result[3] != -1:
                #     self.scatterArtist2.set_offsets(np.array([self.result[3], self.result[4]]))
                #     self.scatterArtist3.set_offsets(np.array([-self.result[5], self.result[4]]))
                self.plottedSamples += 1
                self.outputQueue.queue.clear()
                #self.plottedSamples += 1
        except:
            pass
        if time.time() - self.startTime > 1:
            print('Frame rate = ' + str(self.plottedSamples) + ' Frames/sec')
            self.plottedSamples = 0
            self.startTime = time.time()


        return self.scatterArtist, self.scatterArtist1,self.scatterArtist2, self.scatterArtist3,


if __name__ == "__main__":
    l = LiveDemo(showAntennas=False, figureTitle='Loocalization')
    resultsThread = Thread(target=recieving_thread, args=(l.outputQueue,)).start()
    ani = anm.FuncAnimation(l.figure, l.plot_results, init_func=l.create_setup, frames=60, interval=10, blit=True)
    #ani = anm.FuncAnimation(l.figure, l.plot_results, init_func=l.create_setup, frames=,interval=10, blit=True)
    plt.show()