visualize_sick.py

import numpy as np
import matplotlib.pyplot as plt
from matplotlib.animation import TimedAnimation
from matplotlib.animation import FuncAnimation

import math
import serial

import threading
 

import matplotlib.animation as animation

xdata = []
ydata = []

def renderData(bytes):
    distanceBytes = bytes[14:-2]

    parsedDistance = []
    xdata.clear()
    ydata.clear()
    for i in range(761):
        currentBytes = distanceBytes[i*2:i*2+2]
        currentDistance = int.from_bytes(currentBytes, 'little')&0x1fff
        parsedDistance.append(currentDistance)

        angle = -5 + (i * 0.25)
        rad = math.radians(angle)
        #xdata.append(-1 * currentDistance * math.cos(rad))
        #ydata.append(-1 * currentDistance * math.sin(rad))
        xdata.append(currentDistance * math.cos(rad))
        ydata.append(currentDistance * math.sin(rad))

    #print('Distances', parsedDistance)


def readData():
    ser = serial.Serial('/dev/ttyUSB0', 38400)

    while (1 == 1):
        # Find header
        header = [0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0x04, 0xff, 0x07]
        headerIndex = 0
        while (headerIndex < 10):
            byte = ser.read()
            number = int.from_bytes(byte, byteorder='big')

            if (number == header[headerIndex]):
                headerIndex += 1
            else:
                headerIndex = 0

        bytes = ser.read(1538)
        #print(bytes)
        #print('bytes read')
        renderData(bytes)


def update_line(num, datax, datay, line):
    #print('updating plot')
    #line.set_data(data[..., :num])
    line.set_data(datax, datay)
    return line,
fig1 = plt.figure()

thread = threading.Thread(target=readData, args=())
thread.start()

l, = plt.plot([], [], 'b-')
plt.plot(0,0, 'ro')
plt.xlim(-2000, 2000)
plt.ylim(-200, 2000)
plt.xlabel('x')
plt.title('test')
line_ani = animation.FuncAnimation(fig1, update_line, 25, fargs=(xdata, ydata, l),
                                   interval=500, blit=True)

plt.show()