X2는 속도가 약간 느린 편이고, USB에 꽂으면 항상 느린 속도로 돌아가고 있어서 신경이 쓰인다. USB 포트에 꽂힌 상태에서는 라이다를 멈춰 세울 방법이 없다. 최소 X4나 X4 Pro가 되어야 그나마 약간 속도 향상이 있고, 정지 모드가 있어서 괜찮을 것 같다. G2와 비교를 해봤는데 속도 차이가 엄청 나더라. 아래는 스캔 데이터 시각화 G2 버전을 약간 수정한 것이다. 주로 baudrate, scan 명령, stop 명령, cs를 check code하는 부분, 3바이트 대신 2바이트 데이터를 읽어오는 인덱싱 부분 정도다.
쓰레드 1번을 만들 때 args의 첫번째 수치를 G2와 동일하게 2500으로 하니까 너무 느리고 지연이 심하더라. 그 절반 정도로 낮추니까 그나마 화면을 갱신하는 속도가 나아졌다.
thread_1 = threading.Thread(target=scan_values, args=(1200, MIN_LENGTH))
X2:G2가 baudrate에서 115200: 230400, sample rate에서 3:5 차이던데, X4(Pro):G2는 baudrate에서 128000:230400, sample rate에서는 5:5로 동일해서 그럭저럭 쓸만할 것으로 보인다.
* 흥미로운 사실 하나. G2, X2, X4, X4 Pro 네 기종의 개발자 튜토리얼을 보니까,
- 명령 내려서 작동하는 기종: G2, X4 Pro
- 자동으로 스캔하고 자동으로 멈추는 기종: X2, X4
* 데이터 바이트인 Si가 G2는 3바이트, 나머지는 2바이트
* 데이터 바이트 첫번째 S1의 하위 2비트의 역할이 무엇인지 X4 Pro에서 처음으로 밝힘. Interference Filtering으로 하위 2비트 할당. 아마도 이전 버전들은 이 2비트가 reserved 였고, 설명하기 귀찮아서 밝히지 않은 듯. 듕국애들 영어 메뉴얼 작성한다고 고생했다만, 꼭 나 같은 작자가 번역한 듯 문법도 구석구석 엉터리고, 귀찮은 건 설명을 건너뛰고...쯔쯔쯧... picamera2 튜토리얼 본 분들은 그 꼼꼼한 설명에 감동하신 분들도 있으리라...
* G2의 Si 3바이트에는 intensity와 distance가 섞여 있기 때문에 다음과 같이 뽑아낸다.
intensity = S1 + (S2 & 0b11) * 256
distance = S2 >> 2 | S3 << 6
* X2 및 X4와 X4 Pro는 설명을 약간 달리 하고 있는데, 공통적으로 OR 연산자인 "'|"를 '+'로 설명하는 큰 오류를 범하고 있다. 수정해서 말하면,
X4 Pro: distance = S1 >> 2 | S2 << 6
X2: X4: distance = (S1 | S2 << 8) >> 2
결국 같은 결과를 낳는데,
distance = S1 >> 2 | S2 << 6
이 방식이 좀더 직관적이다. 왜 *64나 /4 같이 비직관적으로 썼는지 모르겠다. 비트 이동은 C에서 표준적으로 사용되었고, 파이썬에서도 지원하는 방식인데...
# YDLidar X2 시각화 코드 v2.
# G2를 위한 시각화 코드를 X2에도 사용할 수 있도록 수정
# serial.read()로 읽어왔을 때 끝에 잘리는 패킷 짜투리를,
# 뒤에 오는 패킷 짜투리와 이어 붙여서 활용하는 코드 추가
import serial
import re
import math
from matplotlib import pyplot as plt
import numpy as np
import cv2
import time
import threading
#----------------------------------------------------
# check code로 데이터 무결성 검토
# 입력값: 해당 패킷, 출력값: False(결점 있음), True(결점 없음)
#----------------------------------------------------
def check_code(data):
len_data = len(data)
lsn = data[3]
fsa1 = data[4] # LSB : start angle
fsa2 = data[5] # MSB : start angle
lsa1 = data[6] # LSB : end angle
lsa2 = data[7] # MSB : end angle
cs = data[8] | (data[9] << 8)
ph = data[0] | (data[1] << 8) # Packet header: 0x55AA
# XOR 연산 시작
tmp_cs = ph ^ (data[2] | (data[3] << 8)) # (1) ct(f&c) 와 lsn
tmp_cs = tmp_cs ^ (fsa1 | (fsa2 << 8)) # (2)
tmp_cs = tmp_cs ^ (lsa1 | (lsa2 << 8)) # (3)
# 거리 및 밝기 정보와 XOR 연산
for n in range(0, lsn):
if 10+2*n+1 >= len_data: # 아래에서 발생할 수 있는 인덱싱 에러 방지. 인덱스가 데이터 길이를 벗어나지 않도록 체크
print("check code: out of index")
break
tmp_cs ^= (data[10+2*n] | data[10+2*n+1] << 8) # (5)
print("cs = ", hex(cs))
print("xor result = ", hex(tmp_cs))
if cs == tmp_cs:
#print("데이터에 결함이 없습니다.")
return True
else:
#print("데이터에 결함이 있습니다.")
return False
#----------------------------------------------------------------
# 각도 구하는 함수들
# ---------------------------------------------------------------
# 맨처음으로, start_angle과 end_angle을 구한다. 이 둘을 구하는 공식은 같다.
def first_level_angle(lsb, msb):
return ((lsb | (msb << 8)) >> 1) / 64.0
# end_angle 과 start_angle의 차이를 구한다.
# end_angle 이 360도를 넘어가서 1부터 시작하면 360을 더해서, 거기서 start_angle을 빼서 차이를 구한다.
def diff_angle(end_angle, start_angle):
if end_angle < start_angle: # end_angle이 360도를 넘어 1도로 되돌아간 상황
end_angle += 360
return end_angle - start_angle
# start_angle 과 end_angle 사이의 angle들을 구한다.
# diff_angle 을 lsn-1로 나누면 하나의 간격(약 0.5도) angle이 나온다. 이 angle*idx를 start_angle에 더한다.
# 360도를 넘어가면 360을 뺀다.
def inter_angle(idx, diff_angle, lsn, start_angle):
ret = (diff_angle / (lsn - 1)) * (idx - 1) + start_angle
if ret >= 360:
ret -= 360
return ret
# 개발자 가이드 나온대로 따라한 수정 앵글 value
def angle_correct(angle, distance):
if distance == 0:
return 0.0
#return angle + (math.atan(21.8 * ((155.3 - distance)/(155.3*distance))) * 180/math.pi) # OK
#return angle + (math.atan2(21.8 * (155.3 - distance), 155.3 * distance) * 180/math.pi) # OK
return angle + math.degrees(math.atan2(21.8 * (155.3 - distance), 155.3 * distance))
# 거리와 각도로부터 점의 좌표 구해서 math.atan2에 집어넣기 : 각도를 degree단위로 바꿀 필요성
# math.degrees(): degree로 변환, math.radians(): 라디안으로 변환
#----------------------------------------------------------------
# 패킷을 분석해서 xy 좌표값, 거리를 구하는 핵심 함수
# ---------------------------------------------------------------
def scan_values(read_size, MIN_LENGTH):
global xys
global remained
s = serial.Serial('/dev/ttyUSB0', 115200)
while True:
xs = [] # 1회 read_size해서 구한 x 좌표값들을 저장하는 리스트
ys = [] # 1회 read_size해서 구한 y 좌표값들을 저장하는 리스트
o_read_data = s.read(read_size) # original
indexes = [m.start() for m in re.finditer(b"\xaa\x55", o_read_data)]
count = len(indexes)
len_read = len(o_read_data)
print("indexes: ", indexes)
print("len(indexes)", count)
print("len_read: ", len_read)
read_data = remained + o_read_data[indexes[0]:indexes[count-1]]
remained = o_read_data[indexes[count-1]:] # 마지막의 잘린 패킷
# 데이터가 구조가 달라졌으니 다시 세팅
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)
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
# 앵글 계산 end_angle , start_angle
fsa1 = data[4] # LSB : start angle
fsa2 = data[5] # MSB : start angle
lsa1 = data[6] # LSB : end angle
lsa2 = data[7] # MSB : end angle
#check code 해서 무결성 검사
if check_code(data) == True:
print("데이터에 결함이 없습니다.")
else:
print("데이터에 결함이 있습니다.")
start_angle = first_level_angle(fsa1, fsa2)
end_angle = first_level_angle(lsa1, lsa2)
diff_angle_ = diff_angle(end_angle, start_angle)
print("start_angle: ", start_angle)
print("end_angle: ", end_angle)
print("diff_angle: ", diff_angle_)
print("step angle: ", diff_angle_/(lsn-1))
for j in range(0, lsn):
if 10+2*j+1 >= len_data: # 아래에서 발생할 수 있는 인덱싱 에러 방지. 인덱스가 데이터 길이를 벗어나지 않도록 체크
break
# -------- 거리 계산
distance = int((data[10+2*j] | (data[10+2*j+1] << 8)) / 4)
# Intermediate angle solution formula (중간 각도 구하기). 코멘트는 메뉴얼에 나온 공식대로. 메뉴얼 설명이 부실함.
if j > 0 and j < lsn-1: # start_angle 과 end_angle 가급적 건드리지 않기
inter_angle_ = inter_angle(j, diff_angle_, lsn, start_angle)
elif j == 0:
inter_angle_ = start_angle
elif j == lsn - 1:
inter_angle_ = end_angle
angle_correct_ = angle_correct(inter_angle_, distance)
print("angle: {0:>6} distance: {1:>4}".format(\
round(angle_correct_, 1), distance))
# 좌표 구하기 pos_x, pos_y
pos_x = distance * math.cos(math.radians(angle_correct_))
#pos_x = distance * math.cos(inter_angle_*(math.pi/180))
# plot으로 그릴 때는 아래 값에 -1을 곱해야 했었는데
# opencv로 출력할 때는 곱하지 않아도 vertical_flip(상하반전) 일어나지 않음.
pos_y = distance * math.sin(math.radians(angle_correct_))
print(f"(x, y) : ({pos_x}, {pos_y})")
# 리스트에 추가하기
xs.append(pos_x)
ys.append(pos_y)
xys.append([xs, ys])
print("xs: ", xs[:10])
print("ys: ", ys[:10])
print("len_xys", len(xys))
#return xys
s.close()
#----------------------------------------------------------------
# 시각화 함수: 분석값을 받아와서 좌표평면에 점을 찍는 rviz 비슷하게.
# ---------------------------------------------------------------
def my_viz(WIDTH=800, HEIGHT=800, INTER=100):
# 좌표 평면의 너비와 폭
#WIDTH = 800
#HEIGHT = 800
# 배경색, 격자 간격, 격자의 색깔
background_color = 70
#background_color = (90, 79, 74) # 어두운 회색
grid_color = (100, 100, 100)
grid_thickness = 1
#INTER = 100 # 격자 간격
row_numbers = int(HEIGHT / 100)
column_numbers = int(WIDTH / 100)
while True:
# 좌표 평면 생성
img = np.full((HEIGHT, WIDTH, 3), background_color, dtype=np.uint8)
# 세로선 그리기
for i in range(1, column_numbers):
cv2.line(img, (i * INTER, 0), (i * INTER, HEIGHT), grid_color, grid_thickness, cv2.LINE_AA)
# 가로선 그리기
for i in range(1, row_numbers):
cv2.line(img, (0, i * INTER), (WIDTH, i * INTER), grid_color, grid_thickness, cv2.LINE_AA)
# 좌표 중심(0, 0) 잡기. (x, y) 좌표값들의 분포를 구해야 함. 일단, 정가운데로 잡기. 가령 (400, 400)
center = (WIDTH // 2, HEIGHT // 2)
# 중심점에 빨간색 원 그리기
center_color = (0, 255, 0)
center_radius = 5
cv2.circle(img, center, center_radius, center_color, cv2.FILLED, cv2.LINE_AA)
# --------------------------------
# 평면에 xy 점 찍기
# --------------------------------
# mm로 표현된 xys 값을 좌표값 단위(pixel = cm 단위)로 바꿈 => 나누기(//) 10 (cv2 출력에서는 소수점 안 됨)
# (-400, -400)(400, 400) 스케일을 (0, 0)(800, 800) 스케일로 바꿈 => 좌표값에 400을 더함
xy_color = (0, 0, 255)
xy_radius = 1
print("from thread2: len_list_numbers = ", len(xys))
if (len(xys) > 0):
xy = xys.pop(0) # list type
# 리스트에는 연산 연력이 없으므로 넘파이 배열로 바꿔서 연산함
x = np.array(xy[0], dtype=np.int16) // 10 + 400
y = np.array(xy[1], dtype=np.int16) // 10 + 400
print("-------------------len(x) : ", len(x))
for i in range(len(x)):
cv2.circle(img, (x[i], y[i]), xy_radius, xy_color, cv2.FILLED, cv2.LINE_4)
cv2.imshow('img', img)
if cv2.waitKey(1) == ord('q'):
break
time.sleep(0.08) # 초당 24프레임
'''
def plot_data(x, y):
plt.cla()
plt.ylim(-4000, 4000)
plt.xlim(-4000, 4000)
plt.scatter(x, y, s=1**2)
plt.axis('square')
plt.show()
'''
#----------------------- main ------------------------
# x, y 좌표값을 담는 전역변수. 쓰레드들이 사용해야 하기 때문에 전역으로 뺐음.
# [[x1_values, y1_values], [x2_values, y2_values], ...]
# 제일 앞에 저장된 원소를 pop()해서 opencv로 그림.
xys = []
MIN_LENGTH = 12
#header_size = 7 # response 헤드 bytes
#cloud_header = 10 # 클라우드 데이터 헤드 bytes
#point_size = 2 # 점 하나 데이터 bytes (거리 및 루미넌스 값)
remained = bytes() # 패킷의 자투리를 보관하는 바이트 변수.
# 쓰레드 생성
thread_1 = threading.Thread(target=scan_values, args=(1200, MIN_LENGTH))
thread_2 = threading.Thread(target=my_viz, args=(800, 800, 100))
thread_1.start()
thread_2.start()
출력 결과:
