카테고리 없음
(YDLidar G2 시각화) 스캔한 좌표를 Plot으로 그리기
미친토끼
2025. 3. 22. 09:51
(참고한 자료들: G2 개발자 메뉴얼,
https://robonote.tistory.com/9
https://xandroid.tistory.com/entry/YDLIDAR-G2-360%C2%B0-Laser-Scanner )
YDLidar G2로 좌표값을 얻어서 Plot으로 그려보았다. 상하가 반전되어 나와서(vertical flip) y 좌표값에 -1을 곱했다.
왜 y 좌표값이 반전되어 나오는지 확인해봐야겠다. 원래 그런 건지, 내가 코딩에서 간과하고 넘어간 게 있는지...
방 안이 아주 스캔이 잘 되어 있다. 장애물이 많아서 부분부분 끊어져 있다. 두 번째 결과는 통 안에 넣었는데, 라이다의 전선과 커넥터 있는 부분까지 잘 처리되어 있다.
# YDLidar G2용
import serial
import re
import math
from matplotlib import pyplot as plt
# 맨처음으로, 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(): 라디안으로 변환
def scan_values(data_size):
read_data = s.read(data_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)
lx = []
ly = []
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
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))
# 어떤 이유에선지는 모르겠지만 y 좌표값이 상하반전, 그러니까 +, -가 뒤바뀌어 나타남. (-1)을 곱해서 미리 상하반전(vertical flip)을 시킴.
pos_y = (-1) * distance * math.sin(math.radians(angle_correct_))
print(f"(x, y) : ({pos_x}, {pos_y})")
# 리스트에 추가하기
lx.append(pos_x)
ly.append(pos_y)
return lx, ly
def plot_data(x, y):
plt.cla()
plt.ylim(-4000, 4000)
plt.xlim(-4000, 4000)
plt.scatter(x, y, s=1**2)
plt.show()
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 (거리 및 루미넌스 값)
for i in range(30):
# 한번 스캔에 회수하는 데이터가 적으면 한바퀴(대략 0.5*360=720개 점) 정보가 들어오지 않기 때문에
# scan_values() 안에 들아가는 수치를 조금 키울 필요가 있다.
lx_, ly_ = scan_values(6000)
s.write(b'\xa5\x65') # stop motor
plot_data(lx_, ly_)
s.close()