# YDLidar G2용
# 거리, intensity, 각도, X/Y 좌표, check code 결과 출력
# filename: scan_data.py
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 (거리 및 루미넌스 값)
# 맨처음으로, 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(): 라디안으로 변환
while True:
read_data = s.read(1024)
# 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
# 앵글 계산 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 = ((fsa1 | (fsa2 << 8)) >> 1) / 64.0
#end_angle = ((lsa1 | (lsa2 << 8)) >> 1) / 64.0
#--------------------------------
# check code로 데이터 무결성 검토
# cs_xor_sequence 함수로 뺄 듯? : G2 Deveopment Manual <FIG 8 XOR SEQUENCE> 참조
#--------------------------------
# cs (check code) 구하기
cs = data[8] | (data[9] << 8)
ph = data[0] | (data[1] << 8) # Packet header, G2에서는 0x55AA임
print("ph = ", hex(ph))
# 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("인덱싱 범위 벗어남")
break
tmp_cs ^= data[10+3*n] # (4)
tmp_cs ^= (data[10+3*n+1] | data[10+3*n+2] << 8) # (5)
print("check code = ", hex(cs))
print("tmp_cs = ", hex(tmp_cs))
if cs == tmp_cs:
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(inter_angle_))
#pos_x = distance * math.cos(inter_angle_*(math.pi/180))
pos_y = distance * math.sin(math.radians(inter_angle_))
print(f"(x, y) : ({pos_x}, {pos_y})")
# 리스트에 추가하기
s.write(b'\xa5\x65') # stop motor
s.close()
# 모터 중지 코드를 넣어야 하는데, 종료 뒤에도 모터가 돈다면, 아래 파이썬 스크립트를 실행하면 됨.
'''
#!/bin/python3
# filename: stop
# chmod +x stop 해서 $PATH 걸린 곳에 복사할 것.
import serial
s = serial.Serial('/dev/ttyUSB0', 230400)
s.write(b'\xa5\x65') # stop motor
s.close()
'''카테고리 없음