def scan3d(self, distance):
import os, time
from math import cos, sin, pi, floor
from adafruit_rplidar import RPLidar
PORT_NAME = '/dev/ttyUSB0'
lidar = RPLidar(None, PORT_NAME)
max_distance = 0
maxPoints = 360
x = []
y = []
scan_data = [0]*360
times = []
print("scanning...")
for scan in lidar.iter_scans():
for (_, angle, distance) in scan:
scan_data[min([359, floor(angle)])] = distance
times.append(time.time_ns())
x, y = process_data(scan_data)
if len(x) >= maxPoints:
break
print('end')
self.x3d.extend(x)
self.y3d.extend(y)
self.lid_times.extend(times)
lidar.stop()
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 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()
def get_data(smqn, sman, smdn, smln, smsn, l, port_name):
#Apperantly this is the Correct way to do Shared Memory
smq = shared_memory.ShareableList(name=smqn)
sma = shared_memory.ShareableList(name=sman)
smd = shared_memory.ShareableList(name=smdn)
sml = shared_memory.ShareableList(name=smln)
sms = shared_memory.ShareableList(name=smsn)
lidar = RPLidar(None, port_name)
lis = lidar.iter_scans
#Speeding up locks
la = lock.acquire
lr = lock.release
while True: #Code Retry
try:
for scan in lis():
la() #Locking
if sms[0]: break #Graceful Shutdown Reciever
sml[0] = int(len(scan))
for x in range(0, sml[0]):
n = scan[x]
smq[x] = n[0]
sma[x] = n[1]
smd[x] = n[2]
lr() #Unlocking
except RPLidarException as e:
print('RPLidarException')
print(e)
break
except ValueError as e:
print('Failure Due to Access Bug')
print(e)
break
except KeyboardInterrupt as e:
pass
#End of Daemon
if l.locked(): lr() #allow code to run
lidar.stop()
lidar.set_pwm(0)
lidar.disconnect()
print('SCAN STOPPED!')
def getLidarScan(lidar_data_queue):
PORT_NAME = '/dev/ttyUSB0'
lidar = RPLidar(None, PORT_NAME)
scan_data = [0] * 360
try:
print(lidar.info)
for scan in lidar.iter_scans():
for (_, angle, distance) in scan:
scan_data[min([359, floor(angle)])] = distance
## Make sure there is only the newest data in the queue
if lidar_data_queue.empty() is True:
lidar_data_queue.put(scan_data)
else:
temp = lidar_data_queue.get()
lidar_data_queue.put(scan_data)
except KeyboardInterrupt:
print('Stoping.')
lidar.stop()
lidar.disconnect()
def collect_data(client):
lidar = RPLidar(None, PORT_NAME)
try:
motor.value = False
sleep(2)
motor.value = True
sleep(2)
print(lidar.info)
for scan in lidar.iter_scans():
post(client, scan)
except RPLidarException:
if lidar is not None:
lidar.stop()
lidar.disconnect()
collect_data(client)
except KeyboardInterrupt:
print('Stopping.')
lidar.stop()
lidar.disconnect()
def main(self, config, log, local):
from adafruit_rplidar import RPLidar
from math import cos, sin, radians
while True:
try:
lidar = RPLidar(None, config['lidarPort'])
lidar.set_pwm(config['lidarSpeed'])
try: # Rear LIDAR failure will not impact main LIDAR
import adafruit_tfmini
import serial
uart = serial.Serial("/dev/ttyS0", timeout=1)
rearLidar = adafruit_tfmini.TFmini(uart)
rearLidar.mode = adafruit_tfmini.MODE_SHORT
except Exception as exception:
log["exceptions"].append(str(exception))
while True:
for scan in lidar.iter_scans():
local.scan = scan
try:
rearScan = (rearLidar.distance -
config['rearLidarOverhang'],
rearLidar.strength, rearLidar.mode)
if rearScan[0] <= 0:
rearScan = (None, rearScan.strength,
rearScan.mode)
local.rearScan = rearScan
except Exception as exception:
log["exceptions"].append(str(exception))
except Exception as exception:
log["exceptions"].append(str(exception))
if type(
exception
) == KeyboardInterrupt: # This might not work because it runs it a separate process.
lidar.stop()
lidar.disconnect()
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)
from adafruit_rplidar import RPLidar
from math import floor
PORT_NAME = '/dev/ttyUSB0'
lidar = RPLidar(None, PORT_NAME)
print(lidar.info)
scan_data = [0] * 360
for scan in lidar.iter_scans():
for (_, angle, distance) in scan:
scan_data[min([359, floor(angle)])] = distance
break
with open("data.csv", "w+") as out:
out.write("ANGLE, DISTANCE\n")
for i in range(len(scan_data)):
out.write("{}, {}\n".format(i, scan_data[i]))
out.close()
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()
class ImageGatherer(SensorBase):
def __init__(self, args):
super(ImageGatherer, self).__init__(args)
self.lidar = RPLidar(None, '/dev/ttyUSB0')
self.scan_data = [0] * 360
self.snapshot_time = datetime.now()
image_paths = glob.glob("{}/*.jpg".format(args["imageoutput"]))
if image_paths:
self.snapshot_count = max([
int(os.path.splitext(path.split(os.path.sep)[-1])[0])
for path in image_paths
])
else:
self.snapshot_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):
# Capture a image
now = datetime.now()
if (now - self.snapshot_time).seconds > 5:
self.snapshot_time = now
# draw the timestamp on the frame
ts = datetime.now().strftime("%A %d %B %Y %I:%M:%S%p")
# write the current frame to output directory
filename = "{}.jpg".format(str(self.snapshot_count).zfill(16))
path = os.path.join(self.args["imageoutput"], filename)
print("[INFO] Saving captured image to ", path)
cv2.imwrite(path, frame)
self.snapshot_count += 1
r_multiplier = random.uniform(2.0, 3.0)
l_multiplier = 1.0
turn_duration = 2.0
dist_to_obstacle = 300
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.snapshot_count % 2 == 0 else l_multiplier
self.l_multiplier = l_multiplier if self.snapshot_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.disconnect()