def run_lidar():
try:
lidar = RPLidar(None,'/dev/ttyUSB0') #check usb port with dmesg
while True:
for scan in lidar.iter_scans():
for(quality, angle, distance) in scan:
if( angle >= 240 and angle <=300): #center on 267.3, corrected on filter end
print(",,,,,{:7.2f}, {:7.2f}".format(angle, distance))
except KeyboardInterrupt:
lidar.stop()
lidar.clear_input()
lidar.disconnect()
finally:
lidar.stop()
lidar.disconnect()
class RPLidar(object):
'''
https://github.com/adafruit/Adafruit_CircuitPython_RPLIDAR
'''
def __init__(self, lower_limit = 0, upper_limit = 360, debug=False):
from adafruit_rplidar import RPLidar
# Setup the RPLidar
PORT_NAME = "/dev/ttyUSB0"
import glob
port_found = False
self.lower_limit = lower_limit
self.upper_limit = upper_limit
temp_list = glob.glob ('/dev/ttyUSB*')
result = []
for a_port in temp_list:
try:
s = serial.Serial(a_port)
s.close()
result.append(a_port)
port_found = True
except serial.SerialException:
pass
if port_found:
self.port = result[0]
self.distances = [] #a list of distance measurements
self.angles = [] # a list of angles corresponding to dist meas above
self.lidar = RPLidar(None, self.port, timeout=3)
self.lidar.clear_input()
time.sleep(1)
self.on = True
#print(self.lidar.get_info())
#print(self.lidar.get_health())
else:
print("No Lidar found")
def update(self):
scans = self.lidar.iter_scans(550)
while self.on:
try:
for scan in scans:
self.distances = [item[2] for item in scan]
self.angles = [item[1] for item in scan]
except serial.serialutil.SerialException:
print('serial.serialutil.SerialException from Lidar. common when shutting down.')
def run_threaded(self):
sorted_distances = []
if (self.angles != []) and (self.distances != []):
angs = np.copy(self.angles)
dists = np.copy(self.distances)
filter_angs = angs[(angs > self.lower_limit) & (angs < self.upper_limit)]
filter_dist = dists[(angs > self.lower_limit) & (angs < self.upper_limit)] #sorts distances based on angle values
angles_ind = np.argsort(filter_angs) # returns the indexes that sorts filter_angs
if angles_ind != []:
sorted_distances = np.argsort(filter_dist) # sorts distances based on angle indexes
return sorted_distances
def shutdown(self):
self.on = False
time.sleep(2)
self.lidar.stop()
self.lidar.stop_motor()
self.lidar.disconnect()
class proc_lidar():
def __init__(self):
# Setup the RPLidar, 라이다 센서 셋업
PORT_NAME = '/dev/ttyUSB0' # 포트네임 설정
self.lidar = RPLidar(
None,
PORT_NAME) # 해당 포트 네임으로 라이다 객체 생성, None -> motor pin, 모터 작동 시작
time.sleep(2)
def get_data(self):
self.scan_data = [0] * 360 # 360개의 버퍼 생성
scan = next(self.lidar.iter_scans())
for (_, angle, distance) in scan: # 레이저 강도, 각도, 거리 순
self.scan_data[min(
[359, floor(angle)]
)] = distance # floor->내림값 구하는 함수, 내림값 구한후 359보다 작은 값인지 검사후 저장, 각도가 인덱스값이 됨
return self.scan_data
def process_data(self): # 데이터 처리함수 정의(FOV내 데이터 좌표로 변환)
global max_distance
self.data = []
self.coords = []
angle = int(sta_ang) # 처음 인덱스 시야각의 시작 각도로 설정
while (angle != int(end_ang)): # 인덱스 값이 종료 각 도달하기 전까지 실행
if angle == 360: # 인덱스 각도가 360도가 되면 다시 0부터 시작하기 위한 설정
angle = 0
distance = self.scan_data[angle] # 해당 각도에 대한 거리값 가져오기
if distance > 0 and distance < max_distance: # 측정되지 않은 거리값은 버림
print("{0}도 에서 {1} cm: ".format(angle, distance * 0.1))
# max_distance = max([min([5000, distance]), max_distance]) # 최대 5000으로 거리값 제한,
# radians = (angle-90) * pi / 180.0 # 각도의 라디안값 구하기, mask
radians = (angle) * pi / 180.0 # 각도의 라디안값 구하기, view
x = distance * cos(radians) # x축 좌표 계산
y = distance * sin(radians) # y축 좌표 계산
self.coords.append([
int(distance * 0.1 * sin((angle) * pi / 180.0)),
int(distance * 0.1 * cos((angle) * pi / 180.0))
])
self.data.append([
int(640 + x / max_distance * 639),
int(720 + y / max_distance * 639)
]) # 640*640에 맞게 좌표 계산
angle = angle + 1
return self.data, self.coords
def proc_coords(self): # 좌표 리스트에서 물체 탐지
self.obj_coords = []
sta_x = 0
sta_y = 0
max_dist = 10 # 두 점 사이 최대 거리
i = 0
while (i < len(self.coords)): # 좌표 리스트 길이만큼 반복
x, y = self.coords[i] # 좌표 불러와 저장
if sta_x == 0 and sta_y == 0: # 처음일 경우
sta_x = x
sta_y = y
i = i + 1
continue
pre_x, pre_y = coords[i - 1] # 이전 좌표 저장
dist = sqrt((x - pre_x)**2 + (y - pre_y)**2) # 이전 좌표와 현재 좌표 거리 저장
if dist > max_dist: # 거리가 최대 길이보다 클 경우(다른 객체)
tmp_list = [] # 임시 리스트 생성
tmp_list += (sta_x, sta_y, pre_x, pre_y) # 시작점 끝점 저장
sta_x = x # 현재 x좌표를 새로운 객체 시작점으로 저장
sta_y = y # 현재 y좌표를 새로운 객체 시작점으로 저장
self.obj_coords.append(tmp_list) # 객체 리스트에 탐지한 객체 저장
i = i + 1
continue
if (i == len(coords) - 1) and (
dist < max_dist): # 리스트의 마지막 요소 이고, 이전 점과 연결된 객체인 경우
tmp_list = [] # 임시 리스트 생성
tmp_list += (sta_x, sta_y, x, y) # 객체 좌표 저장
self.obj_coords.append(tmp_list) # 객체 리스트에 정보 저장
i = i + 1
return self.obj_coords # 최종 결과 리턴
def get_view_coords(self): # 좌표 영상좌표로 변환
self.view_coords = [] # 변환된 좌표 저장할 리스트 초기화
for x1, y1, x2, y2 in self.obj_coords: # 물체 좌표 불러오기
if y1 > 50 or y2 > 50: # 거리 이용하여 물체 제한(y 좌표)
continue
print("물체 좌표(탑-뷰 좌표): {0}, {1}, {2}, {3}".format(x1, y1, x2, y2))
ang1 = atan(x1 / y1) * (180 / pi) # 시작점 각도 추출
ang2 = atan(x2 / y2) * (180 / pi) # 끝점 각도 추출
dist1 = sqrt(x1**2 + y1**2) # 시작점 거리 추출
dist2 = sqrt(x2**2 + y2**2) # 끝점 거리 추출
print("ang1: " + str(ang1))
print("ang2: " + str(ang2))
print("dist1: " + str(dist1))
print("dist2: " + str(dist2))
x1 = int(ang1 / 31 * 640) + 640 # 시작점 영상 가로 좌표 계산
x2 = int(ang2 / 31 * 640) + 640 # 끝점 영상 가로 좌표 계산
if dist1 < 130: # 시작점 거리가
if dist1 <= 20:
y1 = 719
y1 = 720 - int(-0.0348 * (dist1 - 130)**2 + 385)
elif (dist1 <= 250):
y1 = 720 - int(-0.007 * (dist1 - 250) + 471)
else:
y1 = 720 - 471
if dist2 < 130:
if dist2 <= 20:
y2 = 719
y2 = 720 - int(-0.0348 * (dist2 - 130)**2 + 385)
elif (dist2 <= 250):
y2 = 720 - int(-0.007 * (dist2 - 250) + 471)
else:
y2 = 720 - 471
self.view_coords.append([x1, y1, x2, y2])
print("물체의 좌표(영상): " + str(self.view_coords))
return self.view_coords
def disconnect_lidar(self):
self.lidar.clear_input()
self.lidar.stop_motor()
self.lidar.stop()
self.lidar.disconnect()