일단 파이썬 쓰레드 개념 잡는 것은 여기에 설명이 아주 잘 되어 있다.
https://mechacave.tistory.com/2
라즈베리파이에서 한 쓰레드는 센서 값을 읽어오고, 다른 쓰레드는 그 읽어온 값을 기반으로 다른 센서를 설정하는 일을 쓰레드로 간단히 구현한 일이 있었는데, 이번에는 YDLidar 라이다 센서의 값을 읽어온 좌표값을 기반으로 시각화 작업을 직접 해보고 싶어서 불가불 쓰레드를 활용하게 되었다. 한 쓰레드는 라이다 센서 값을 읽어와서 좌표값을 리스트에 저장하는 일만 하고, 다른 쓰레드는 그 좌표값을 읽어와서 OpenCV로 좌표평면을 만들어 그 위에 붉은 점만 그리는 것이다. 즉, rbiz의 기본 기능을 pyserial과 opencv를 사용해서 직접 구현하는 것이다. 파이썬의 쓰레드나 멀티프로세싱 기능을 사용하지 않고는, 이 문제 해결이 쉽지 않았다. 왜냐하면, 지속적으로 들어오는, 쉴틈없이 들어오는 센서 정보를 읽고 분석해서 좌표값을 만들어내야 하는데, 이 일을 하는 녀석이 잠시 시간내어 opencv로 그림을 그리는, 그리고 그 그림을 cv2.imshow로 계속 갱신하는 일을 할 수는 없는 것이다. 그래서 부득불, 간단히 테스트를 해보니 쓰레드가 멀티프로세싱에 비해 사용하기에 용이하고 그 효과도 나쁘지 않은 듯했다.
먼저, 한 쓰레드는 난수 목록을 만들어 리스트에 저장하고, 다른 쓰레드는 그 난수 목록을 읽어서 출력하는 작업을 간단하게 쓰레드로 구현해봤다.
1라운드. 난수 생성해서 저장하는 쓰레드 VS 그 난수를 읽어와서 출력하는 쓰레드
import threading
import time
import numpy as np
# 첫번째 쓰레드가 랜덤 넘버를 만들어 저장하면, 그것을 불러와서 출력하는 두 번째 쓰레드
# 난수들을 저장할 리스트
list_numbers = []
def make_numbers(start, end, n):
np.random.seed(int(time.time()))
while True:
x = np.random.randint(start, end, size=n)
y = np.random.randint(start, end, size=n)
list_numbers.append([x, y])
print("from thread_1: len_list_numbers = ", len(list_numbers))
print(x, y)
time.sleep(1)
def show_numbers():
while True:
print("from thread_2: len_list_numbers = ", len(list_numbers))
# 리스트에 번호뭉치가 담겨 있으면
if (len(list_numbers) > 0):
xys = list_numbers.pop(0)
# xys = [3107 341 905 235 2434 1956 1049 2212 556 2833] [ 811 2634 3045 444 470 3457 2482 2210 1456 3105]
xs = xys[0] # x: [3107 341 905 235 2434 1956 1049 2212 556 2833]
ys = xys[1] # y: [ 811 2634 3045 444 470 3457 2482 2210 1456 3105]
print("x: ", xs)
print("y: ", ys)
time.sleep(1)
# ------- 쓰레드 생성
# 0에서 4000 사이 난수를 10개 발생하는 일을 맡는 쓰레드 1
thread_1 = threading.Thread(target=make_numbers, args=(0, 4000, 10))
# 그 난수를 읽어오는 일을 맡는 쓰레드
thread_2 = threading.Thread(target=show_numbers, args=())
# ------- 쓰레드 실행
thread_1.start()
thread_2.start()---- 출력 결과
from thread_1: len_list_numbers = 1
[3107 341 905 235 2434 1956 1049 2212 556 2833] [ 811 2634 3045 444 470 3457 2482 2210 1456 3105]
from thread_2: len_list_numbers = 1
x: [3107 341 905 235 2434 1956 1049 2212 556 2833]
y: [ 811 2634 3045 444 470 3457 2482 2210 1456 3105]
from thread_1: len_list_numbers = 1
[ 487 2989 4 70 3059 1604 3367 1840 2233 2353] [ 451 2764 3804 984 1713 334 2542 76 3316 3847]
from thread_2: len_list_numbers = 1
x: [ 487 2989 4 70 3059 1604 3367 1840 2233 2353]
y: [ 451 2764 3804 984 1713 334 2542 76 3316 3847]
잘 작동한다. 두 번째는 이 난수 발생하는 쓰레드는 그대로 두고, 난수 출력하는 녀석을 opencv로 점을 찍는 쓰레드로 바꾸는 것이다.
2라운드: 난수 발생 저장 쓰레드 VS 난수로 좌표에 점을 찍는 쓰레드
import threading
import time
import numpy as np
import cv2
# 첫번째 녀석이 랜덤 넘버를 만들어 저장하면, 그것을 불러와서 좌표에서 출력하는 두 번째 녀석
xys = []
def make_numbers(start, end, n):
np.random.seed(int(time.time()))
while True:
x = np.random.randint(start, end, size=n)
y = np.random.randint(start, end, size=n)
xys.append([x, y])
print("len_list_numbers = ", len(xys))
print(x, y)
def show_numbers():
while True:
print("from thread2: len_list_numbers = ", len(xys))
if (len(xys) > 0):
xy = xys.pop(0)
x = xy[0]
y = xy[1]
print("x: ", x)
print("y: ", y)
time.sleep(1)
#----------------------------------------------------------------
# 시각화 함수: 분석값을 받아와서 좌표평면에 점을 찍는 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_color = (0, 0, 255)
xy_radius = 3
# --------------------------------
# 평면에 xy 점 찍기
# --------------------------------
print("from thread2: len_list_numbers = ", len(xys))
if (len(xys) > 0):
xy = xys.pop(0)
print(xy)
x = xy[0] // 10
y = xy[1] // 10
for i in range(len(x)):
cv2.circle(img, (x[i], y[i]), xy_radius, xy_color, cv2.FILLED, cv2.LINE_AA)
cv2.imshow('img', img)
if cv2.waitKey(1) == ord('q'):
break
# 쓰레드 생성
thread_1 = threading.Thread(target=make_numbers, args=(0, 8000, 100))
thread_2 = threading.Thread(target=my_viz, args=(800, 800, 100))
thread_1.start()
thread_2.start()
--출력 결과
잘 출력된다.
격자선은 1M 간격으로 그어져 있다. 녹색 점이 (0,0) 기준점이다. 곧 라이다 위치점으로 삼을 셈이다. 현재 난수발생 쓰레드가 너무 빠르게 일을 하고 있어서 리스트에 XY 목록이 18만개나 들어가 있다. 스캔 데이터를 읽어 좌표값 생성할 때는 어떻게 될지 궁금하다. 이 수치가 너무 높으면 일정 비율 이 쓰레드를 sleep 시키면 될 것이다. 가령, time.sleep(len_list_numbers / 100000)
좌표평면은 800x800으로, (400, 400)이 중심점이고, 라이다 사방 4M를 측정하여 출력할 생각이다. 최대 1000x1000 (1080p 모니터 기준), 사방 5M까지 생각하고 있다. 실내에서 이 정도면 테스트하는 데 지장없을 것이다. 격자 간격을 1미터(100pixel)로 하는 것은 거리와 좌표 계산에 용이해서이다.
이제 핵심 작업으로 들어가보자.
3라운드: 라이다 센서값을 읽어와서 좌표값을 뽑아내는 쓰레드 VS 그 좌표값으로 opencv 좌표평면에 점을 찍는 쓰레드
시행착오1) 처음에 이렇게 쓰레드를 생성했는데,
thread_1 = threading.Thread(target=scan_values, args=(2500))
args로 들어가는 인자들이 튜플로 인식되어야 하는데 그러려면 (2500,) 하는 식으로 따옴표를 적어놓어야 한다. 그냥 (2500)으로 하니까 튜플이 아니라 int 정수형으로 인식되어서 오류가 났다.
해결책 => thread_1 = threading.Thread(target=scan_values, args=(2500, MIN_LENGTH))
시행착오2) args 의 첫번째 인자는 시리얼에서 읽어오는 데이터 사이즈(read_size)인데 테스트를 해보니, 2500일 때 x,y 좌표 분해 갯수가 720~750개 사이였다. 360도가 0.5도 단위로 쪼개져서 총 720개면 360도를 표현할 수 있는데, 720개 이상이니까 한 바퀴를 표현하기에 적당한 수치였다.
시행착오3) G2가 초당 5~12 회전을 하고, 이를 쉽게 세팅할 수 있는데, 보통값인 8을 감안하면, 초당 8회 회전, 즉 초당 8회 프레임으로 나의 my_viz (rviz 대용물)에서 보여줄 수 있다는 것이다. cv2.imshow에서 24프레임으로 세팅했다면, 3 프레임 중 1개 프레임에서만 xy 좌표값들을 보여줄 수 있다는 말이다. xys 리스트에 xy 좌표값이 들어와 있는지 체크를 하고, 들어와 있다면 그것을 점으로 찍어 보여주는데, 대략 3번 시도 중 한번만 xy 좌표값들이 들어와 있음을 하기 코드 중
230 라인: print("from thread2: len_list_numbers = ", len(xys))
으로 알 수 있었다.
따라서 빠르게 (이동하면서) 스캔 결과를 갱신하려면 라이다의 스핀 속도가 빨라야 한다는 것이다. G2가 초당 12정도 되므로, X2, X4 보다는 약간 나은 것 같다. 실내에서 느린 속도로 갱신하기에는 YD Lidar X2도 나쁘진 않다.
시행착오4) 스레드와 관련된 제일 중요한 문제인데, 이걸 몰라서 30분쯤 헤맸다. 결국 25년전 공유메모리 코딩하던 기억이 흐릿하게 되살아나면서 거기를 탁 쳤다. 결국 쓰레드를 이용하면 기본적으로 메모리를 공유하지 않기 때문에 이에 대한 대처가 필요하다. 그것도 모르고, 메인 스레드에서 USB 장치에 접속해서 serial 객체를 받아와놓고, (자식) 스레드1(데이터 스캔 담당)에게 변수들을 넘기지 않아서 scan_values 함수 내에서 USB에서 데이터를 읽어오는 s.read(read_size)에서 int가 아니라 NoneType 이 들어왔다고 계속 난리를 치는 것이다. 나는 이게 파이썬 3.8(우분투 20.04)의 스레드 관련 버그인줄 알고, 파이썬 3.10(우분투 22.04), 3.12(우분투 24.04/라즈베리파이)에서 실행해봐도 동일했다. 스레드 호출 코드를 다시 들여다보는데, 그때 머릿속을 흐릿하게 스쳐가는 무언가가 있었다. 스레드는 메모리를 공유하지 않는다,라는... 25년 전 리눅스에서 공유메모리 관련 세마포어 코드를 짜면서 느꼈던 문제의식이 희미하게 되살아난 것이다. 영원하라, 기억이여! 그대 찰나를 살고 가는 호모 사피엔스의 덧없는 기억이여! 내가 로봇 속 메모리였다면 백업해서 영원히 기억할 수 있을 텐데, 하필이면 이런 불멸의 살과 피로 태어났으니, 이 덧없음과 가엾음을 어찌할 것인가!
아무튼, 전역 변수 xys 는 자식 스레드1에서 사용할 수 있도록 global 선언을 했고, 대부분, 즉 포트 접속, 초기화 등을 모두 스레드 담당 함수 안에 넣어서 처리하니까 비로소 원활하게 인식되고 돌아갔다. 굳이 멀티프로세싱까지 끌어들이지 않아도, list의 pop() 기능만 사용해도 충분했다.
남은문제) 좌표 평면에서 마우스로 점을 찍어 그 사이의 거리를 구해주는 기능을 간단하게 구현해보고 싶다(rviz 기본 기능). 더 중요하게는, 가령 2500 바이트를 USB 포트에서 읽어오게 되면, 아무래도 마지막 패킷(스캔 데이터가 담겨 있는)이 잘리게 되어 버려지는데, 이것을 살려서 360 스캔 정보를 온전히 살려보고 싶다. 그러려면 포트에서 읽어올 때 1 바이트씩 읽어서 '\xa5\x5a'가 나오는 지점을 기준으로 패킷을 분리해서 리스트에 저장하고(스레드1), 그것을 불러와서 분석해서 좌표값을 저장하고(스레드2), 그 좌표값을 읽어와서 cv2로 출력하는(스레드3) 역할 분담이 필요한 것 같다. 스캔 속도가 예상보다 느리기 때문에 (24 프레임의 1/3 수준... 회전 속도를 초당 12회로 올린다면, 24 프레임의 1/2 수준) 파이썬으로도 충분하고 그 사이에도(스캔 데이터 분석 값이 들어오는 동안에도) 많은 일들을 처리할 수 있다.
# YDLidar G2용
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, G2에서는 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+3*n+2 >= len_data: # 아래에서 발생할 수 있는 인덱싱 에러 방지. 인덱스가 데이터 길이를 벗어나지 않도록 체크
print("check code: out of index")
break
tmp_cs ^= data[10+3*n] # (4)
tmp_cs ^= (data[10+3*n+1] | data[10+3*n+2] << 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
s = serial.Serial('/dev/ttyUSB0', 230400)
#for i in range(6):
# s.write(b'\xa5\x0c') # increase the frequency by 1Hz
s.write(b'\xa5\x60') # start scanning, output point cloud data
while True:
xs = [] # 1회 read_size해서 구한 x 좌표값들을 저장하는 리스트
ys = [] # 1회 read_size해서 구한 y 좌표값들을 저장하는 리스트
read_data = s.read(read_size)
# parse response header
idx = read_data.find(b'\xa5\x5a') # search for starting point
response_mode = read_data[5] >> 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
# 앵글 계산 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+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) # 단위 mm
# 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)
# 좌표 구하기 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.write(b'\xa5\x65') # stop motor
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_AA)
cv2.imshow('img', img)
if cv2.waitKey(1) == ord('q'):
break
time.sleep(0.04) # 초당 24프레임
#----------------------- main ------------------------
#s = serial.Serial('/dev/ttyUSB0', 230400)
#s.write(b'\xa5\x60') # start scanning, output point cloud data
# x, y 좌표값을 담는 전역변수. 쓰레드들이 사용해야 하기 때문에 전역으로 뺐음.
# [[x1_values, y1_values], [x2_values, y2_values], ...]
# 제일 앞에 저장된 원소를 pop()해서 opencv로 그림.
xys = []
#read_size = 6000 # 한번에 읽어오는 데이터 길이
MIN_LENGTH = 13 # 한 패킷의 최소 길이. 헤더 정보 10바이트와 점 한 개 3바이트 해서 총 13바이트에 못 미치면 해당 패킷은 건너뛴다.
#header_size = 7 # response 헤드 bytes
#cloud_header = 10 # 클라우드 데이터 헤드 bytes
#point_size = 3 # 점 하나 데이터 bytes (거리 및 루미넌스 값)
# 쓰레드 생성
thread_1 = threading.Thread(target=scan_values, args=(2500, MIN_LENGTH))
thread_2 = threading.Thread(target=my_viz, args=(800, 800, 100))
thread_1.start()
thread_2.start()
#s.write(b'\xa5\x65') # stop motor
#s.close()