making Lidar - step 06 : Finally, getting data

드디어 데이타를 읽고 화면에 내보내기 시작했습니다.     

아직 거리 배율값이 정확히 맞지 않습니다.    형태와 방향은 정확한 듯 합니다.

이젠 그 문제만 해결하면 됩니다.

 

영상 :

# -*- coding: utf-8 -*-
"""
From main library's example, ScatterPlot.py, 
I have taken out one of chart to make it for lidar data display.
The lidar related codes were taken from working code of Vidicon@DISCORD.
Last updated : Aug. 26, 2021 
"""
from pyqtgraph.Qt import QtGui, QtCore
import pyqtgraph as pg
import numpy as np
from collections import namedtuple
from itertools import chain
import sys

import serial   # For this one, you must install pyserial, not serial
from enum import Enum
import time
import math

#SERIAL_PORT = "/dev/ttyS5"   # for Orange Pi Zero 2's serial port
SERIAL_PORT = "/dev/ttyUSB0"  # for Other PC's USB to Serial module

class State(Enum):
   START1 = 0
   START2 = 1
   HEADER = 2
   DATA = 3

def readbytes(file, count):
   data = ser.read(count)
   #data = f.read(count)
   if len(data) != count:
      print("End of file")
      return False
   return data
   
step = (math.pi*2)   
anglePlus = math.pi / 2
fullround = 1300     # max dots in 1 round, it is larger than the real max dots in slowest mode
pos = np.zeros(shape=(2, fullround))
spots = [{'pos': pos[:,i], 'data': 1} for i in range(fullround)] + [{'pos': [0,0], 'data': 1}]
   
file_name = "RAW_DATA.LOG"
try:
   #f = open(file_name, "rb")
   ser = serial.Serial(SERIAL_PORT, 153600, timeout=0.1) 
   time.sleep(1)
except:
   print("could not connect to device")
   exit()

app = QtGui.QApplication([])
mw = QtGui.QMainWindow()
mw.resize(530,500)
view = pg.GraphicsLayoutWidget()  ## GraphicsView with GraphicsLayout inserted by default
mw.setCentralWidget(view)
mw.show()
mw.setWindowTitle('Lidar Test, unit in mm')

## create areas to add plots
w1 = view.addPlot()
w1.setAspectLocked()
  
###### Refresh Screen   
def RefreshScreen():
   global spots   # using globla spots array will ensure that it stores & clears data in same spot
   # Add polar grid lines
   w1.clear()   # clear screen and start drawing basic lines
   w1.addLine(x=0, pen=0.3)  # draw vertical center line
   w1.addLine(y=0, pen=0.3)  # draw horizontal center line
   for radius in range(200, 2000, 200):   # Draw 9 circles 200 ~ 2000 step 200
      # Adding circle (x, y, width, height)
      circleWidth = radius * 2
      circleHeight = radius * 2
      circle = pg.QtGui.QGraphicsEllipseItem(-radius, -radius, circleWidth, circleHeight)
      circle.setPen(pg.mkPen(0.3))
      w1.addItem(circle)  # addItem means draw or plot.  Here, draw circle
   # clear all data in the global spots array, make sure there will be no residue dots from previous round
   emptyone = np.zeros(shape=(2, fullround))   
   spots = [{'pos': emptyone[:,i], 'data': 1} for i in range(fullround)] + [{'pos': [0,0], 'data': 1}]   
      
###### Get Full Circle of Data 
def GetDataFromOneFullCycle():
   counter = 0
   ThisRoundCount = 0 # counts within one round
   maxThisRound = 0   # Number of good numbers for this cycle
   global pos         # using globla pos array will ensure we as storing data in same spot
   global spots       # using globla spots array will ensure we as storing data in same spot
   run = True
   try:
      state = State.START1
      while run:
         if state == State.START1:
            data = ser.read(1)
            #data = readbytes(f, 1)
            if data[0] == 0xAA:
               state = State.START2
            continue
         elif state == State.START2:
            data = ser.read(1)
            #data = readbytes(f, 1)
            if data[0] == 0x55:
               state = State.HEADER
            else:
               state = State.START1           
            continue
         elif state == State.HEADER:
            data = ser.read(8)
            #data = readbytes(f, 8)
            pack_type = data[0]
            data_lenght = int(data[1])
            start_angle = int(data[3] << 8) + int(data[2])
            stop_angle = int(data[5] << 8) + int(data[4])
            #unknown = int(data[7] << 8) + int(data[6])

            diff = stop_angle - start_angle
            if stop_angle < start_angle:
               diff =  0xB400 - start_angle + stop_angle
             
            angle_per_sample = 0
            if diff > 1 and (data_lenght-1) > 0:
               angle_per_sample = diff / (data_lenght-1)      
				
            #print("[{}]\ttype:{},\tlenght {},\tstart: {},\tstop: {}, \tdiff: {} \tdiff: {}"
            #   .format(counter, pack_type, data_lenght, start_angle, stop_angle, diff, angle_per_sample), end="\n")
				
            counter += 1
            #if pack_type != 40:
            #   counter = 0

            state = State.DATA
            continue
            
         elif state == State.DATA:
            state = State.START1
            #read data
            data = ser.read(data_lenght * 3)
            #data = readbytes(f, data_lenght * 3) 
            if data == False:
               break

            for i in range(0, data_lenght):

               data0 = int(data[i*3 + 0]) 
               data1 = int(data[i*3 + 1])
               data2 = int(data[i*3 + 2]) 
               distance = (data2  << 8) + data1

               angle = (start_angle + angle_per_sample * i)
               anglef = step * (angle / 0xB400) 
               #print("[{}]\tangle:{},\tanglef {},\tdist: {}".format(i, data0, (anglef + anglePlus), (distance/1000)), end="\n")
               distanceDivided = distance / 1000   # div to convert mm to meter
               #if (data0 != 1) & (distanceDivided < 3) :
               if (distanceDivided < 60) :
                  distanceDivided = (distance/5)   # Adjust distance ratio.  It is too large
          
                  x = distanceDivided * np.cos(anglef)
                  y = distanceDivided * np.sin(anglef)   
                  pos[0][ThisRoundCount] = y
                  pos[1][ThisRoundCount] = x
                  #print("[{}]\tDistance:{},\tanglef {},\tx:y: {}{}".format(ThisRoundCount, distanceDivided, anglef, x, y), end="\n")
                  ThisRoundCount += 1

            if pack_type != 40:  # After 1 full round
               spots = [{'pos': pos[:,i], 'data': 1} for i in range(ThisRoundCount)] + [{'pos': [0,0], 'data': 1}]
               ThisRoundCount = 0   
               ser.reset_input_buffer()    # This will clear serial line buffer, make update almost realtime.
               run = False   # Completed the mission of filling data in spots, now exit to draw step.
         else:
            print("error")

   except KeyboardInterrupt:
      run = False
      exit()               
   
   
######  I have to focus on putting data here.   
def _update():
   RefreshScreen()  # Draw basic chart with no data dots
   
   GetDataFromOneFullCycle()  # Get Full cycle of data from either File or Serial, prepare "spots"
   
   s1 = pg.ScatterPlotItem(size=5, pen=pg.mkPen(None), brush=pg.mkBrush(127, 255, 127, 120))
   s1.addPoints(spots)
   # addItem means draw or plot.  Here, plot all points
   w1.addItem(s1, ignoreBounds = True)  # ignoreBounds will prevent annoying rescaling

timer = QtCore.QTimer(interval=1)
timer.timeout.connect(_update)
#timer.start(0.1)  # duration number in millisecond
timer.start()  # A.S.A.P.

if __name__ == '__main__':
    import sys
    if (sys.flags.interactive != 1) or not hasattr(QtCore, 'PYQT_VERSION'):
        QtGui.QApplication.instance().exec_()