일단, 잃어버린 패킷 이어붙이기를 하지 않아도 데이터가 괜찮길래 그부분을 빼고 원래대로 했습니다.
* 기능:
1. 스캔 데이터를 읽어서 check code를 하고, 각도, 거리, intensity, 좌표 등을 출력해줌.
2. 그 결과를 opencv로 점을 찍어 rviz처럼 보여줌.
* 버그 잡기
1. 사이 각도(inter_angle) 함수에서 idx를 -1하는 바람에 첫 부분에 중복 각도가 출현하고, 마지막 부분에 1개 각도가 비는 현상이 있었음.
2. 각도 보정 함수(angle_corrent)에서 보정 각도가 음각일 경우, 양각으로 변환하는 부분 추가했음.
실행 결과:
텍스트 출력:
-------------------len(x) : 336
indexes: [112, 242, 372, 502, 632, 762, 892, 1022, 1152]
len(indexes) 9
len_read: 1200
len_data: 130
lsn(sample quantity): 40
cs = 0x9e07
xor result = 0x9e07
데이터에 결함이 없습니다.
start_angle: 252.421875
end_angle: 286.203125
diff_angle: 33.78125
step angle: 0.8661858974358975
angle: 0.0 distance: 0 intensity: 19 (x, y): ( 0, 0 )
angle: 249.6 distance: 286 intensity: 44 (x, y): ( -99, -268 )
angle: 0.0 distance: 0 intensity: 0 (x, y): ( 0, 0 )
angle: 0.0 distance: 0 intensity: 0 (x, y): ( 0, 0 )
angle: 252.1 distance: 295 intensity: 21 (x, y): ( -90, -280 )
angle: 252.9 distance: 297 intensity: 24 (x, y): ( -87, -283 )
angle: 253.7 distance: 301 intensity: 31 (x, y): ( -84, -288 )
설명:
1) -------------------len(x) : 336
opencv로 점을 찍는 2번 쓰레드가 출력한 메시지로, 찍을 점이 336개 있다는 이야기입니다. 제가 1200바이트를 읽어오라고 명령을 내렸군요.
2) indexes: [112, 242, 372, 502, 632, 762, 892, 1022, 1152]
읽어온 1200 바이트 속에 패킷이 9개 들어 있네요. 그 위치입니다.
3) len(indexes) 9 : 그 갯수
4) len_read: 1200 : 읽어온 바이트 수
----- 여기서 부터는 개별 패킷 데이터임
5) len_data: 130 : 패킷 바이트 수 (여기서는 첫번째 패킷임)
lsn(sample quantity): 40 : points 갯수 (40개 점에 대한 정보가 있다는 이야기)
6) check code 부분
cs = 0x9e07
xor result = 0x9e07
데이터에 결함이 없습니다.
(데이터에 결함이 있을 때도 있는데 그럴 경우, out of index 에러가 났음, 무슨 의미인지는 체크 요함)
7) 다음은 해당 패킷 내에서의 시작 각도와 끝 각도입니다.
start_angle: 252.421875 : 보정 전 시작 각도
end_angle: 286.203125 : 보정 전 끝 각도
diff_angle: 33.78125: 시작 각도와 끝 각도의 차이
step angle: 0.8661858974358975: 그 차이를 (점 갯수 - 1)로 나눈 것. (lsn - 1). 사이 각도.
(*주의: G2의 초기 스캔 프리퀀스(scan frequency) 설정으로는 사이 각도가 0.51도 정도였는데, 지금은 최고 속도인 회당 12.4 회전을 시킨 상태라, 사이 각도가 0.86으로 올라가 있습니다. 회전 속도가 빨라지면 빠르게 스캔을 하는데, 그만큼 점 사이의 간격이 듬성듬성해집니다. X2는 12 Hz대에서 무려 사이각이 1.2까지 벌어지더군요. 장애물을 빠르게 감지하려면 프리퀀시값을 올려주면 되고, 여기 코드에서는 1번 쓰레드를 생성할 때 args 첫번째 값을 좀 낮춰주면 됩니다. 한 바퀴 다 스캔하고 그 정보를 우리에게 줘서 우리가 분석하는 것보다는 반바퀴라도 스캔한 정보를 먼저 우리에게 준다면 그 반바퀴 안에 공격자가 나타났을 때 훨씬 빠르게 대응할 수 있을 겁니다. 야외에서 로봇 반대자가 몽둥이를 들고 로봇에게 빠르게 달려온다고 했을 때, 이를 피하려면 읽어오는 주기를 짧게 하고, 라이다의 회전 속도를 빠르게(스캔 프리퀀시를 높게) 하면 좀더 일찍 감지를 할 수 있겠지요.)
8) angle: 252.1 distance: 295 intensity: 21 (x, y): ( -90, -280 )
이 각도는 보정 후 각도이기 때문에, 보정 전 시작 각도와 끝 각도와는 몇 도쯤 차이가 있습니다. intensity는 0~1023 범위고, 레이저를 쏘았을 때 되돌아온 밝기를 뜻합니다. 검정 물체를 갖다대면 값이 아주 낮아집니다.
G2의 좌표 기준은 전선이 나온 부분이 0도이고, 시계방향으로 90, 180... 올라갑니다.
# YDLidar G2: 스캔 데이터 시각화 : 최종판
# filename: scan_viz_G2.py
# 기능: 스캔 데이터를 읽어서 check code를 하고, 각도, 거리, intensity, 좌표 등을 출력해줌.
# 버그 잡은 것:
# 1. 사이 각도(inter_angle) 함수에서 idx를 -1하는 바람에 첫 부분에 중복 각도가 출현하고, 마지막 부분에 1개 각도가 비는 현상이 있었음.
# 2. 각도 보정 함수(angle_corrent)에서 보정 각도가 음각일 경우, 양각으로 변환하는 부분 추가했음.
import serial
import re
import math
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") # 점들 갯수가 lsn보다 적다는 이야기. 패킷이 잘렸거나 데이터 오류?
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 + 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
my_angle = angle + math.degrees(math.atan2(21.8 * (155.3 - distance), 155.3 * distance))
if my_angle < 0:
my_angle = 360 + my_angle
return my_angle
# 거리와 각도로부터 점의 좌표 구해서 math.atan2에 집어넣기 : 각도를 degree단위로 바꿀 필요성
# math.degrees(): degree로 변환, math.radians(): 라디안으로 변환
#----------------------------------------------------------------
# 패킷을 분석해서 xy 좌표값, 거리를 구하는 핵심 함수
# ---------------------------------------------------------------
def scan_values(read_size, MIN_LENGTH):
global xys
s = serial.Serial('/dev/ttyUSB0', 230400)
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("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: # 아래에서 발생할 수 있는 인덱싱 에러 방지. 인덱스가 데이터 길이를 벗어나지 않도록 체크
print("(S1, S2, S3): out of index")
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을 곱해서 첫번째 바이트와 더함
# -------- 거리 계산
# 두 번째 바이트 상위 6비트를 밑으로 내리고, 세번째 바이트 비트 전부를 여섯 비트(계단) 올려서 이 둘을 비트 조합(OR 연산)함.
distance = ((data[10+3*j+1] >> 2) | (data[10+3*j+2] << 6))
# 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("angle: {0:>6} distance: {1:>4} intensity: {2:>4} (x, y): ( {3:>5}, {4:>5} )".format(\
round(angle_correct_, 1), distance, intensity, int(pos_x), int(pos_y)))
# 리스트에 추가하기
xs.append(pos_x)
ys.append(pos_y)
xys.append([xs, ys])
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 ------------------------
# x, y 좌표값을 담는 전역변수. 쓰레드들이 사용해야 하기 때문에 전역으로 뺐음.
# [[x1_values, y1_values], [x2_values, y2_values], ...]
# 제일 앞에 저장된 원소를 pop()해서 opencv로 그림.
xys = []
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()
------------------------------------------ X2 용 소스 ---------------------------------------------
# YDLidar X2: 스캔 데이터 시각화 : 최종판
# filename: scan_viz_X2.py
# 기능: 스캔 데이터를 읽어서 check code를 하고, 각도, 거리, 좌표 등을 출력해줌.
# 버그 잡은 것:
# 1. 사이 각도(inter_angle) 함수에서 idx를 -1하는 바람에 첫 부분에 중복 각도가 출현하고, 마지막 부분에 1개 각도가 비는 현상이 있었음.
# 2. 각도 보정 함수(angle_corrent)에서 보정 각도가 음각일 경우, 양각으로 변환하는 부분 추가했음.
import serial
import re
import math
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+2*n+1 >= len_data: # 아래에서 발생할 수 있는 인덱싱 에러 방지. 인덱스가 데이터 길이를 벗어나지 않도록 체크
print("check code: out of index")
# lsn과 점개수가 맞지 않다는 이야기. 즉, 데이터가 lsn에 지정된 것보다 작다는 이야기. 데이터가 잘려나갔거나 오류가 생겼다는 건가?
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 + 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
my_angle = angle + math.degrees(math.atan2(21.8 * (155.3 - distance), 155.3 * distance))
if my_angle < 0:
my_angle = 360 + my_angle
return my_angle
# 거리와 각도로부터 점의 좌표 구해서 math.atan2에 집어넣기 : 각도를 degree단위로 바꿀 필요성
# math.degrees(): degree로 변환, math.radians(): 라디안으로 변환
#----------------------------------------------------------------
# 패킷을 분석해서 xy 좌표값, 거리를 구하는 핵심 함수
# ---------------------------------------------------------------
def scan_values(read_size, MIN_LENGTH):
global xys
s = serial.Serial('/dev/ttyUSB0', 115200)
while True:
xs = [] # read_size해서 구한 x 좌표값들을 저장하는 리스트
ys = [] # 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("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: # 아래에서 발생할 수 있는 인덱싱 에러 방지. 인덱스가 데이터 길이를 벗어나지 않도록 체크
print("out of range: S1, S2 다루는 중")
break
# -------- 거리 계산
# 두 번째 바이트 상위 6비트를 밑으로 내리고, 세번째 바이트 비트 전부를 여섯 비트(계단) 올려서 이 둘을 비트 조합(OR 연산)함.
distance = ((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)
# 좌표 구하기 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("angle: {0:>6} distance: {1:>4} (x, y): ( {2:>5}, {3:>5} )".format(\
round(angle_correct_, 1), int(distance), int(pos_x), int(pos_y)))
# 리스트에 추가하기
xs.append(pos_x)
ys.append(pos_y)
xys.append([xs, ys])
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 ------------------------
# x, y 좌표값을 담는 전역변수. 쓰레드들이 사용해야 하기 때문에 전역으로 뺐음.
# 제일 앞에 저장된 원소를 pop()해서 opencv로 그림.
xys = []
MIN_LENGTH = 12 # 한 패킷의 최소 길이. 헤더 정보 10바이트와 점 한 개 3바이트 해서 총 13바이트에 못 미치면 해당 패킷은 건너뛴다.
#header_size = 7 # response 헤드 bytes
#cloud_header = 10 # 클라우드 데이터 헤드 bytes
#point_size = 2 # 점 하나 데이터 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()
출력 결과:
-------------------len(x) : 482
from thread2: len_list_numbers = 0 (대기 중인 2번 thread가 출력한 것임)
from thread2: len_list_numbers = 0
from thread2: len_list_numbers = 0
indexes: [44, 78, 90, 180, 270, 360, 450, 540, 630, 664, 676, 766, 856, 946, 1036, 1126]
len(indexes) 16
len_read: 1200
len_data: 34
lsn(sample quantity): 12
cs = 0x4f39
xor result = 0x4f39
데이터에 결함이 없습니다.
start_angle: 321.828125
end_angle: 337.53125
diff_angle: 15.703125
step angle: 1.4275568181818181
angle: 0.0 distance: 0 (x, y): ( 0, 0 )
angle: 317.5 distance: 558 (x, y): ( 411, -377 )
angle: 318.8 distance: 573 (x, y): ( 431, -377 )
angle: 0.0 distance: 0 (x, y): ( 0, 0 )
angle: 320.8 distance: 970 (x, y): ( 752, -613 )
angle: 322.2 distance: 991 (x, y): ( 783, -607 )
angle: 323.6 distance: 1012 (x, y): ( 814, -600 )
angle: 325.0 distance: 1036 (x, y): ( 848, -593 )
angle: 326.4 distance: 1061 (x, y): ( 883, -586 )
angle: 327.8 distance: 1088 (x, y): ( 920, -579 )
(매 각도 사이가 1.4도 정도로 넓어진 것을 알수 있음. 회전 속도(scan frequency)가 12Hz 정도임.
읽어온 바이트가 G2와 동일하게 1200바이트라고 해도, 점(거리 정보) 하나가 2바이트를 차지하므로 좌표 갯수가 G2 때보다 더 많아졌음.)