ydlidar python code

두번째 버전 수정했다 나름 괜찮게 나오는거 같고?

대충 형상에 맞게 나온다.

import serial
import serial.tools.list_ports as sp
import math
import numpy as np
import struct
from matplotlib import pyplot as plt
import time

class LCcali:
    lx=[]
    ly=[]

    def connect(comport,baudrate):
        open_com=serial.Serial(comport,baudrate)
        open_com.isOpen()
        return open_com
        
    def scan(serial):
        open_com=serial
        value=bytearray([int('a5',16),int('60',16)])
        open_com.write(value)
        return open_com
    def receive(serial):
        read_data = serial.read(6000)
        n=read_data.count(b"\xaa\x55")        
        while(n>0):
            #print(n)
            a=read_data.find(b"\xaa\x55")
            read_data=read_data[a:]
            lsn = read_data[3]
            n=n-1
            #print(n)
            if(lsn!=1):
                #print('lsn:',lsn)
                fsa1= read_data[4]
                fsa2= read_data[5]
                lsa1= read_data[6]
                lsa2= read_data[7]
                start_angle=(fsa2*64.0 + fsa1/4.0)/32.0
                end_angle=(lsa2*64.0 + lsa1/4.0)/32.0
                #print(start_angle,end_angle)
                
                
                for i in range(1,lsn):
                    if(end_angle - start_angle<0):
                        lsn_angle=((end_angle+360.0-start_angle)/(lsn-1))*i+start_angle
                        
                        if(lsn_angle > 360.0):
                            lsn_angle=lsn_angle-360.0
                    else:
                        lsn_angle=((end_angle-start_angle)/(lsn-1))*i+start_angle

                    lsn_distance = (read_data[8+2*i+1]*64.0 + read_data[8+2*i]/4.0)
                    if(int(lsn_distance)==0):
                        lsn_angle = lsn_angle
                    else:
                        lsn_angle=lsn_angle+math.atan2(21.8*((155.3-lsn_distance)),155.3*lsn_distance)*180/math.pi
                        #print(math.atan(21.8*((155.3-lsn_distance)/155.3*lsn_distance))*180/math.pi)
                        #print(lsn_angle)
                    lidar_x=lsn_distance*math.cos(lsn_angle*(math.pi/180))
                    lidar_y=lsn_distance*math.sin(lsn_angle*(math.pi/180))
                    LCcali.lx.append(lidar_x)
                    LCcali.ly.append(lidar_y)
        return LCcali.lx, LCcali.ly

    def plot(lidar_x,lidar_y):
        plt.cla()
        plt.ylim(-9000,9000)
        plt.xlim(-9000,9000)
        plt.scatter(lidar_x,lidar_y)
        plt.show()
open_com=LCcali.connect('/dev/ttyUSB1',128000)
open_com=LCcali.scan(open_com)
for i in range(50):
    lx,ly=LCcali.receive(open_com)
LCcali.plot(ly,lx)

import serial
import serial.tools.list_ports as sp
import math
import numpy as np
import struct
from matplotlib import pyplot as plt
import time
list = sp.comports()
connected =[]

for i in list:
    connected.append(i.device)
print("Connected COM PORTS: "+str(connected))

open_com=serial.Serial('/dev/ttyUSB0',128000)
open_com.isOpen()
value=bytearray([int('a5',16),int('60',16)])
open_com.write(value)
#open_com.write(bytes(bytearray([0xA5, 0x60])))



lx=[]
ly=[]
for a in range(50):
    rx=[]
    buf_idx=0
    n=1024
    parsing_count=[]
    time.sleep(0.001)
    while(n>0):
        
        read_data = open_com.read()
        #rx[buf_idx]=read_data.split(b'')
        #print(read_data)
        
        read_data=struct.unpack('b',read_data)
        #print(read_data)
        #if(read_data==(85,)):
            #print('sccc')
        rx.append(read_data)
        #print(read_data)
        if(rx[buf_idx]==(85,) and rx[buf_idx-1]==(-86,)):
            parsing_count.append(1024 - n+1)    
        #print('parsing:',parsing_count)
        #if(read_data.find("x") != -1 ):
        #    rx.append(int(read_data.split('x')[1].split("'")[0],16))
        n-=1
        buf_idx+=1
        #print(rx)
        #rx_packet = rx.decode('ascii')[:len(open_com)-1]
    #print(int(rx,16))
    #print(int(rx))
    

    lsn = rx[parsing_count[-2]+1][0]
    if(lsn==40):
        #print('lsn:',lsn)
        fsa1= rx[parsing_count[-2]+2]
        fsa2= rx[parsing_count[-2]+3]
        lsa1= rx[parsing_count[-2]+4]
        lsa2= rx[parsing_count[-2]+5]
        start_angle=(fsa2[0]*64.0 + fsa1[0]/4.0)/32.0
        end_angle=(lsa2[0]*64.0 + lsa1[0]/4.0)/32.0
        #print(start_angle,end_angle)
        for i in range(lsn):
            if(end_angle - start_angle<0):
                lsn_angle=((end_angle+360.0-start_angle)/(lsn-1))*i+start_angle
                if(lsn_angle > 360.0):
                    lsn_angle=lsn_angle-360.0
            else:
                lsn_angle=((end_angle-start_angle)/(lsn-1))*i+start_angle
            #print('lsn-',i,parsing_count[-2]+8+2*i+1,'parsing: ',rx[parsing_count[-2]+8+2*i+1][0])
            lsn_distance = (rx[parsing_count[-2]+8+2*i+1][0]*64.0 + rx[parsing_count[-2]+8+2*i][0]/4.0)
            #print(lsn_distance)

            #angle Correction
            if(lsn_distance<0.1):
                lsn_angle = lsn_angle
            else:
                lsn_angle=lsn_angle+math.atan2(21.8*(155.3-lsn_distance),155.3*lsn_distance)
            lidar_x=lsn_distance*math.cos(lsn_angle*(math.pi/180))
            lidar_y=lsn_distance*math.sin(lsn_angle*(math.pi/180))
            lx.append(lidar_x)
            ly.append(lidar_y)
    
            #print('x:',lidar_x,'y:',lidar_y)
plt.scatter(lx,ly)
plt.show()
open_com.close()