(Python) YDLidar G2, 거리와 intensity 출력하는 코드

#  YDLidar G2용
# 거리와 intensity 출력하는 코드입니다.
# filename: show_distance.py
# 종료 후에도 라이다가 돈다면, 다음의 파이썬 스크립트로 작동을 중지시킬 수 있습니다.
# try: except 코드 짜기가 좀 귀찮아서...ㅎㅎ
'''
#!/bin/python3
import serial

s = serial.Serial('/dev/ttyUSB0', 230400)
s.write(b'\xa5\x65')  # stop motor
s.close()
'''

import serial
import re
import math

s = serial.Serial('/dev/ttyUSB0', 230400)
s.write(b'\xa5\x60')  # start scanning, output point cloud data

MIN_LENGTH = 13 # 한 패킷의 최소 길이. 헤더 정보 10바이트와 점 한 개 3바이트 해서 총 13바이트에 못 미치면 해당 패킷은 건너뛴다.
header_size = 7 # response 헤드 bytes
cloud_header = 10  # 클라우드 데이터 헤드 bytes
point_size = 3  # 점 하나 데이터 bytes (거리 및 루미넌스 값)

while True:
  read_data = s.read(512)

  # parse response header
  idx = read_data.find(b'\xa5\x5a') # search for starting point
  response_mode = read_data[5] >> 6
  #print("response mode(1=continuous): ", response_mode)
  #print("type code : ", hex(read_data[6]))

  # 패킷 헤더를 모두 찾아서 그 인덱스를 리스트로 저장한다.
  indexes = [m.start() for m in re.finditer(b"\xaa\x55", read_data)]
  count = len(indexes)  # 패킷 갯수
  len_read = len(read_data)  # 읽어온 바이트 수
  #print("indexes: ", indexes)
  #print("len(indexes)", count)
  #print("len_read: ", len_read)

  for i in range(count):
    if i == count - 1: # 마지막 패킷이라면, end 인덱스가 없으므로 끝지점을 직접 지정
      data = read_data[indexes[i] : len_read]
      #print("last packet length : ", len(data))
    else :
      data = read_data[indexes[i] : indexes[i+1]] # header부터 다음 header 직전까지 읽음
    len_data = len(data)
    #print("len_data: ", len_data)
    if len_data <= MIN_LENGTH:
      continue

    frequency = (data[2] >> 1) / 10.0  # current frequency
    #print("current frequency: ", frequency)
    packet_type = data[2] & 0b01  # 하위 1비트만 얻음
    #print("current packet type: ", packet_type)
    lsn = data[3] # sample quantity
    #print("-------")
    #print("lsn(sample quantity): ", lsn)
    
    if lsn != 1: # 한 패킷 안의 샘플링 데이터 갯수. 없으면 lsn=1
      for j in range(0, lsn):
        if 10+3*j+2 >= len_data: # 아래에서 발생할 수 있는 인덱싱 에러 방지. 인덱스가 데이터 길이를 벗어나지 않도록 체크
          break

        #---------------- Luminous intensity 계산
        # 첫 번째 바이트를 전부 취하고 (최대 256 표현), 두 번째 바이트의 하위 2개 비트에 256을 곱해서(= 왼쪽으로 8번 비트 쉬프트해서), 
        # 이 둘을 더해줌. 2^8은 256이므로, 해당 비트들을 왼쪽으로 8번 쉬프트하는 것이나, 256을 곱하는 것이나 동일함. 
        # 0b11은 이진수 표현. 16진수로 표현하자면 0x03이 됨.
        # 표현 범위: 0~1023
        intensity = data[10+3*j] + (data[10+3*j+1] & 0b11) * 256 # 두 번째 바이트 하위 2비트 얻어서 256을 곱해서 첫번째 바이트와 더함
        print("Luminous intensity: ", intensity) 
        #--------------- 거리 계산
        # 두 번째 바이트 상위 6비트를 밑으로 내리고, 세번째 바이트 비트 전부를 여섯 비트(계단) 올려서 이 둘을 비트 조합(OR 연산)함.
        distance = ((data[10+3*j+1] >> 2) | (data[10+3*j+2] << 6))
        print("distance: ", distance / 1000.0)  # 단위 mm -> M

s.write(b'\xa5\x65')  # stop motor
s.close()