class RPLidarSensor(SensorBase):
def __init__(self, args):
super(RPLidarSensor, self).__init__(args)
self.lidar = RPLidar(None, '/dev/ttyUSB0')
self.scan_data = [0]*360
self.time = datetime.now()
self.count = 0
print(self.lidar.info)
print(self.lidar.health)
t = threading.Thread(target=self._read_scans, args=())
t.daemon = True
t.start()
def _read_scans(self):
for scan in self.lidar.iter_scans():
for (_, angle, distance) in scan:
self.scan_data[min([359, floor(angle)])] = distance
def update_internal(self, frame):
now = datetime.now()
if (now - self.time).seconds > 5:
self.time = now
self.count += 1
r_multiplier = random.uniform(3.0, 5.0)
l_multiplier = 1.0
turn_duration = 2.0
dist_to_obstacle = 350
roi = self.scan_data[135:225]
if any((dist > 0 and dist < dist_to_obstacle) for dist in roi):
self.r_multiplier = r_multiplier if self.count % 2 == 0 else l_multiplier
self.l_multiplier = l_multiplier if self.count % 2 == 0 else r_multiplier
self.motor_duration = turn_duration
print("[INFO] Avoiding obstacle!")
return True
return False
def shutdown(self):
print("[INFO] Stopping rplidar")
self.lidar.stop()
self.lidar.stop_motor()
self.lidar.disconnect()
def stop_motor(self):
self.lidar.stop_motor()
def main():
signal.signal(signal.SIGINT, interrupt_handler)
lidar = RPLidar(None, PORT_NAME, baudrate=256000)
logging.basicConfig(
filename="Datalog/Receiver/lidar_data_{}.log".format(time),
level=logging.INFO)
max_distance = 0
try:
logging.info("Starting scanning>>>")
logging.info(lidar.info)
sleep(10)
for scan in lidar.iter_scans():
for (_, angle, distance) in scan:
data = {"angle": angle, "distance": distance, "time": ctime()}
logging.info(str(data))
except KeyboardInterrupt:
logging.info("Stop scanning")
sys.exit(0)
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
sys.exit(0)
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()
src_lane_pts = []
for coord in data:
src_x = remap_to_ipm_x[coord[1], coord[0]]
if src_x <= 0: # x좌표 0일경우 무시
continue
src_y = remap_to_ipm_y[int(coord[1]), int(coord[0])]
src_y = src_y if src_y > 0 else 0 # 파이썬 조건부 표현식(삼항 연산자 ? : 와 비슷), y좌표 0일 경우 0으로 저장 0이 아니면 해당 값 저장
lane_pts = [src_x, src_y] # lane_pts 배열에 해당 x, y 좌표추가 (원래 뷰로 표현한 차선 좌표)
print("lane: "+str(lane_pts))
src_lane_pts.append(lane_pts) # src_lane_pts 배열에 IPM 변환 사용하여 구한 차선의 전체 x, y 좌표 저장
"""
cv2.imshow('Test', tmp_view)
if cv2.waitKey(10) == 27:
cv2.destroyAllWindows()
"""
while True:
p_data = get_data(lidar)
data = process_data(p_data)
print(data)
display_point(data)
"""
except KeyboardInterrupt:
print('Stoping.')
lidar.stop_motor()
lidar.stop()
lidar.disconnect()
lidar.stop_motor()
lidar.stop()
lidar.disconnect()
class lidar(Thread):
"""
This thread continually captures from SlamTec LIDARa
"""
# Initialize the Camera Thread
# Opens Capture Device and Sets Capture Properties
def __init__(self, camera_num: int = 0, res: (int, int) = None, # width, height
exposure: float = None):
# initialize
self.logger = logging.getLogger("lidar")
# Setup the RPLidar
PORT_NAME = '/dev/ttyUSB0'
self.lidar = RPLidar(None, PORT_NAME)
if not self.lidar == ??:
self.logger.log(logging.CRITICAL, "Status:Failed to open lidar!")
self.lidar.stop_motor()
self.scan = [0]*360
# Threading Locks, Events
self.lidar_lock = Lock() # before changing capture settings lock them
self.stopped = True
# Init Scan and Thread
self.scan_data = [0]*360
self.new_scan = False
self.stopped = False
self.measured_scanrate = 0.0
Thread.__init__(self)
#
# Thread routines #################################################
# Start Stop and Update Thread
def stop(self):
"""stop the thread"""
self.stopped = True
def start(self):
""" set the thread start conditions """
self.stopped = False
self.lidar.set_pwm(1023)
self.lidar.start_motor()
T = Thread(target=self.update, args=())
T.daemon = True # run in background
T.start()
# After Stating of the Thread, this runs continously
def update(self):
""" run the thread """
last_fps_time = time.time()
last_exposure_time = last_fps_time
num_scans = 0
while not self.stopped:
current_time = time.time()
# FPS calculation
if (current_time - last_fps_time) >= 5.0: # update frame rate every 5 secs
self.measured_fps = num_frames/5.0
self.logger.log(logging.DEBUG, "Status:SPS:{}".format(self.measured_sanrate))
num_scans = 0
last_fps_time = current_time
with self.lidar_lock:
raw_scan = lidar.iter_scans(max_buf_meas=500, min_len=360)
ordered_scan = [0]*360
for (_, angle, distance) in raw_scan:
ordered_scan[min([359, floor(angle)])] = distance
num_scans += 1
self.scan = ordered_scan
# creating point could data`
# not finihed
for angle in range(360):
distance = self.scan[angle]
if distance > 0:
radians = angle * pi / 180.0
x = distance * cos(radians)
y = distance * sin(radians)
if self.stopped:
self.lidar.stop_motor()
#
# Scan routines ##################################################
# Each lidar stores scan locally
###################################################################
@property
def scan(self):
""" returns most recent frame """
self._new_scan = False
return self._scan
@scan.setter
def scan(self, scan):
""" set new frame content """
with self.scan_lock:
self._scan = scan
self._new_scan = True
@property
def new_scan(self):
""" check if new scan available """
out = self._new_scan
return out
@new_scan.setter
def new_scan(self, val):
""" override wether new scan is available """
self._new_scan = val