def test_simple_express_scan():
from pyrplidar import PyRPlidar
import time
lidar = PyRPlidar()
lidar.connect(port="/dev/ttyUSB0", baudrate=115200, timeout=3)
# Linux : "/dev/ttyUSB0"
# MacOS : "/dev/cu.SLAB_USBtoUART"
# Windows : "COM5"
lidar.set_motor_pwm(500)
time.sleep(2)
scan_generator = lidar.start_scan_express(4)
for count, scan in enumerate(scan_generator()):
print(count, scan)
if count == 20: break
lidar.stop()
lidar.set_motor_pwm(0)
lidar.disconnect()
def simple_scan():
lidar = PyRPlidar()
lidar.connect(port="/dev/ttyUSB0", baudrate=256000, timeout=3)
# Linux : "/dev/ttyUSB0"
# MacOS : "/dev/cu.SLAB_USBtoUART"
# Windows : "COM5"
lidar.set_motor_pwm(500)
time.sleep(2)
scan_generator = lidar.force_scan()
for count, scan in enumerate(scan_generator()):
print(count, scan)
if count == 20: break
lidar.stop()
lidar.set_motor_pwm(0)
lidar.disconnect()
from pyrplidar import PyRPlidar
import time
import paho.mqtt.client as mqtt
import json
# Setup the lidar
lidar = PyRPlidar()
lidar.connect(port="/dev/ttyUSB0", baudrate=115200, timeout=3)
lidar.set_motor_pwm(500)
print('Spinning up...')
time.sleep(2)
print('...ready!')
# Connect to the Brain client
broker_url, broker_port = "192.168.1.230", 1883
client = mqtt.Client()
client.connect(broker_url, broker_port)
try:
while True:
# Start the lidar scan
lidar.connect(port="/dev/ttyUSB0", baudrate=115200, timeout=3)
# Run scan
scan_generator = lidar.force_scan()
for count, scan in enumerate(scan_generator()):
# print(count, scan)
# Reset scan after a large number of scans with this generator
# if count > 5000: break
# Package with json and send
message = json.dumps([scan.angle, scan.distance]).encode('utf-8')
# results in more points collected 10 times around gives a high density of points but take longer
distancelimit = 4500 # throw away points that are too far out.
RPLIDARanglecorrection = 0.01745329 * 1.00 #.98 #1 DEG*FACTOR - the rplidar x axis "0 degrees" this horizontal this adgusts the plane.
zaxiscorrection = math.radians(0.0) #
#SET UP Z AXIS STEP SIZE AND STEPPER MOTOR RUN TIME (NO ANGLE SENSOR)
zaxisanglestep = 2 * pi / zaxisindex # 360degrees divided by how many steps to take
zaxisruntime = scanner_support_functions.getzaxisruntime(
frequency, motorsteps, driveratio, m1, m2, zaxisindex, Nscancycles)
timeperrev = zaxisruntime * zaxisindex # this is used to bring the scanner back home after scannning will use it to tune the stepper driver too
# SET UP RPLIDAR
lidar = PyRPlidar()
lidar.connect(port="/dev/ttyUSB0", baudrate=115200,
timeout=5) #USB also can set up uart pins if desired
lidar.set_motor_pwm(
1000) # not needed for RPLIDAR A1 AS YOU CAN'T CHANGE THE SPEED.
scan_generator = lidar.start_scan_express(
0, "raw"
) # raw if using typed data from scanner, not raw for data packed in string
#0,1,2 is the scan type 0 is the express scan it gives 4000 measurements around generally the slower the better the accuracy appears to be
time.sleep(2) # wait for rplidar to finish set up
#open file for writing scan points as ascii
filename = scanner_support_functions.getfilename()
rootname = "_points_a.xyz"
f = open(filename + rootname,
'a') #open point file for appending scan points xyz ascii data
rootname = "_points_b.xyz"
g = open(filename + rootname,
'a') #open point file for appending scan points xyz ascii data
#scanning routine
class proc_lidar_extern():
def __init__(self):
# Setup the RPLidar, 라이다 센서 셋업
self.lidar = PyRPLidar()
self.lidar.connect(port="/dev/ttyUSB0", baudrate=115200, timeout=3)
def start_motor(self, speed):
self.lidar.set_motor_pwm(speed)
self.scan_generator = self.lidar.force_scan()
def get_data(self):
self.scan_data = [0] * 360 # 360개의 버퍼 생성
for idx, scan in enumerate(self.scan_generator()):
# floor->내림값 구하는 함수, 내림값 구한후 359보다 작은 값인지 검사후 저장, 각도가 인덱스값이 됨
self.scan_data[min([359, floor(scan.angle)])] = scan.distance
if idx == 260:
break
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 = self.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(self.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
if (x1 == x2) or (y1 == y2): # 두 점이 같은 점 일경우(노이즈)
continue
print("물체 좌표(탑-뷰 좌표): {0}, {1}, {2}, {3}".format(x1, y1, x2, y2))
ang1 = atan(x1 / y1) * (180 / pi) # 시작점 각도 추출
ang2 = atan(x2 / y2) * (180 / pi) # 끝점 각도 추출
if x1 == 0: # x좌표 0일 경우
ang1 = 0
else:
ang1 = atan(x1 / y1) * (180 / pi) # 시작점 각도 추출
if x2 == 0: # x 좌표 0일 경우
ang2 = 0
else:
ang2 = atan(x2 / y2) * (180 / pi) # 끝점 각도 추출
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: # 시작점 거리가 sta_ang
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 draw_obj_img_to_list(self, bg_img):
obj_path = "lidar/bmw-x4.png"
obj_img = cv2.imread(obj_path, cv2.IMREAD_UNCHANGED)
bg_h, bg_w = bg_img.shape
for x1, y1, x2, y2 in self.view_coords:
cols = abs(x2 - x1)
rows = 200
pos_x = x1
pos_y = y1 - rows
print("장애물 크기: [{0}, {1}]".format(cols, rows))
resize_car = cv2.resize(obj_img, dsize=(cols, rows))
resize_car = resize_car * 1.0 # 투명도 조절
obj_height = int((x2 - x1) / (2 / 3))
cv2.rectangle(bg_img, (x1, y1 - obj_height), (x2, y2),
(255, 255, 255), 3)
for i in range(rows): # 행 순회
for j in range(cols): # 열 순회
alpha = resize_car[i, j, 3] / 255.0
if i + pos_y >= bg_h or j + pos_x >= bg_w:
continue
bg_img[i + pos_y, j + pos_x] = (1. - alpha) * bg_img[
i + pos_y, j + pos_x] + alpha * resize_car[i, j,
0] # R채널
# bg_img[i+pos_y, j+pos_x, 1] = (1. - alpha) * bg_img[i+pos_y, j+pos_x, 1] + alpha * resize_car[i, j, 1] # G채널
# bg_img[i+pos_y, j+pos_x, 2] = (1. - alpha) * bg_img[i+pos_y, j+pos_x, 2] + alpha * resize_car[i, j, 2] # B채널
return bg_img
def disconnect_lidar(self):
self.lidar.stop()
self.lidar.set_motor_pwm(0)
self.lidar.disconnect()