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)
def __init__(self):
# Setup the RPLidar, 라이다 센서 셋업
PORT_NAME = '/dev/ttyUSB0' # 포트네임 설정
self.lidar = RPLidar(
None,
PORT_NAME) # 해당 포트 네임으로 라이다 객체 생성, None -> motor pin, 모터 작동 시작
time.sleep(2)
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 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()
def run_lidar():
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))
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()
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()
class Robot:
# Setup the RPLidar
PORT_NAME = '/dev/ttyUSB0'
lidar = RPLidar(None, PORT_NAME)
scan_data = [0] * 360
# Setup I2C
addr = 0x8 # bus address
bus = SMBus(1) # i2c channel
# Define variables
MODE_COLOR = 1
MODE_LINE = 2
Mode = MODE_LINE
# initialize wifi
# wifi = WifiSender()
broadcast = UDPBroadcast()
udp = UDPComm()
try:
print(lidar.info)
for scan in lidar.iter_scans():
for (_, angle, distance) in scan:
scan_data[min([359, floor(angle)])] = distance
# print(scan_data[30])
# bus.write_byte_data(addr, i+1, numb)
except KeyboardInterrupt:
print('Stopping.')
lidar.stop()
lidar.disconnect()
def new_lidar():
# Setup the RPLidar, 라이다 센서 셋업
PORT_NAME = '/dev/ttyUSB0' # 포트네임 설정
lidar = RPLidar(
None, PORT_NAME) # 해당 포트 네임으로 라이다 객체 생성, None -> motor pin, 모터 작동 시작
# time.sleep(2)
return lidar
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 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 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)
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()
from math import cos, sin, pi, floor
from adafruit_rplidar import RPLidar
import socket
from time import sleep
import time
import board
import digitalio
PORT_NAME = '/dev/ttyUSB0'
lidar = RPLidar(None, PORT_NAME)
# used to scale data to fit on the screen
max_distance = 0
scan_data = [0] * 360
led = digitalio.DigitalInOut(board.D7)
led.direction = digitalio.Direction.OUTPUT
def collect_data():
try:
print(lidar.info)
for scan in lidar.iter_scans():
for (_, angle, distance) in scan:
scan_data[min([359, floor(angle)])] = distance
process_data(scan_data)
except KeyboardInterrupt:
print('Stoping.')
lidar.stop()
lidar.disconnect()
port_name = '/dev/ttyUSB0'
#Basic Initialization
sumlist = [] #Data Array
sumnum = 0
a=0.0
b=0.0
c=0.0
#Class Call Speedups
sa = sumlist.append
tt = time.time
mf = math.floor
#RPLidar Hardware Initialization
lidar = RPLidar(None, port_name)
lis = lidar.iter_scans
#DANGER!!!
#lidar.set_pwm(1023)
#Initalising Size
scan_data = [0]*400
#Getting Scan Data Loop
try:
for scan in lis(): #Wait for Rotation to Complete
a=tt() #Start Time
#Offloading Scan Data
for (_, angle, distance) in scan: #Offloading Scan Data
import os
from math import cos, sin, pi, floor
from adafruit_rplidar import RPLidar
lidar = RPLidar(None, '/dev/ttyUSB0')
try:
print("RPLidar INFO: ")
print(lidar.info)
for scan in lidar.iter_scans():
for (_, angle, distance) in scan:
print([min([359, floor(angle)])], distance)
except KeyboardInterrupt:
print('Stoping.')
lidar.stop()
lidar.disconnect()
from L3GD20 import L3GD20
import smbus
#other imports
import time
import threading
import numpy
import serial
import sqlite3 as lite
import pandas as pd
#Initial Database Connection
DB_NAME = "tempDB"
#Inital lidar setup
lidar = RPLidar(None, '/dev/ttyUSB0') #check usb port with dmesg
start_time = int(round(time.time() * 1000))
#Gyroscope Function
def run_gyroscope():
gyro_conn = lite.connect(DB_NAME)
gyro_curs = gyro_conn.cursor()
gyro = L3GD20(busId=1, slaveAddr=0x6b, ifLog=False, ifWriteBlock=False)
gyro.Set_PowerMode("Normal")
gyro.Set_FullScale_Value("250dps")
gyro.Set_AxisX_Enabled(True)
gyro.Set_AxisY_Enabled(True)
gyro.Set_AxisZ_Enabled(True)
gyro.Init()
gyro.Calibrate()
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 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
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
if __name__ == "__main__":
# Setup the RPLidar, 라이다 센서 셋업
PORT_NAME = '/dev/ttyUSB0' # 포트네임 설정
lidar = RPLidar(
None, PORT_NAME) # 해당 포트 네임으로 라이다 객체 생성, None -> motor pin, 모터 작동 시작
time.sleep(2)
"""
try:
scan_data = [0]*360 # 360개의 버퍼 생성
print(lidar.info) # 라이다 정보 출력
# iter_scan -> 모터 시작 & 스캔 시작, 단일회전(한바퀴)에 대한 측정값 누적하여 저장(튜플) 값은 부동소수점
sta_time = time.time()
scan = next(lidar.iter_scans())
for (_, angle, distance) in scan: # 레이저 강도, 각도, 거리 순
scan_data[min([359, floor(angle)])] = distance # floor->내림값 구하는 함수, 내림값 구한후 359보다 작은 값인지 검사후 저장, 각도가 인덱스값이 됨
# print(scan_data)
print("소요시간: " + str(time.time()-sta_time))
import os
from math import cos, sin, pi, floor
from adafruit_rplidar import RPLidar
lidar = RPLidar(None, '/dev/ttyUSB0') #check usb port with dmesg
try:
print("RPLidar INFO: ")
print(lidar.info)
for scan in lidar.iter_scans():
for (quality, angle, distance) in scan:
if (angle >= 330 or angle <= 30):
print(quality, angle, distance)
except KeyboardInterrupt:
print('Stoping.')
lidar.stop()
lidar.disconnect()
from math import floor
from adafruit_rplidar import RPLidar
global end_value, count, work, ignore, deq2, stop_value, detect
detect = 0
stop_value = 0
end_value = 0
count = 0
ignore = 0
work = 1 # 동작 가능한 상태는 1 q를 누르면 0이다.
deq2 = deque(
maxlen=1000) #Empty Q create(size 1000) 큐를 전역 변수로 만들어서 모든 곳에서 사용 가능하게 한다.
PORT_NAME = '/dev/ttyUSB0'
lidar = RPLidar(None, PORT_NAME, timeout=3)
####A-Star Node
class Node:
def __init__(self, parent=None, position=None):
self.parent = parent
self.position = position
self.g = 0
self.h = 0
self.f = 0
def __eq__(self, other):
return self.position == other.position
#importing all sensor imports
import gps
from adafruit_rplidar import RPLidar
from L3GD20 import L3GD20
#other imports
import os
import time
import math
import statistics
#declaring connections to sensors
lidar = RPLidar(None, '/dev/ttyUSB0') #check usb port with dmesg
gyro = L3GD20(busId=1, slaveAddr=0x6b, ifLog=False, ifWriteBlock=False)
ugps = gps.gps("localhost", "2947")
ugps.stream(gps.WATCH_ENABLE | gps.WATCH_NEWSTYLE)
try:
#Setup and Calibration for Gyro
print("Gyroscope Setup/Info: ")
gyro.Set_PowerMode("Normal")
gyro.Set_FullScale_Value("250dps")
gyro.Set_AxisX_Enabled(True)
gyro.Set_AxisY_Enabled(True)
gyro.Set_AxisZ_Enabled(True)
gyro.Init()
gyro.Calibrate()
print("Gyroscope calibrated and ready to go!\n")
#Setup and Calibration for Lidar
print("RPLidar Setup/Info: ")
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()
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()
H = 480
SCAN_BYTE = b'\x20'
SCAN_TYPE = 129
# Set up pygame and the display
#os.putenv('SDL_FBDEV', '/dev/fb1')
pygame.display.init()
lcd = pygame.display.set_mode((W, H))
pygame.mouse.set_visible(False)
lcd.fill((200, 0, 0))
pygame.display.update()
# Setup the RPLidar
PORT_NAME = '/dev/ttyUSB0'
lidar = RPLidar(None, PORT_NAME)
# used to scale data to fit on the screen
max_distance = 0
#pylint: disable=redefined-outer-name,global-statement
def process_data(data):
global max_distance
lcd.fill((0, 0, 0))
point = (int(W / 2), int(H / 2))
pygame.draw.circle(lcd, pygame.Color(255, 255, 255), point, 10)
pygame.draw.circle(lcd, pygame.Color(100, 100, 100), point, 100, 1)
pygame.draw.line(lcd, pygame.Color(100, 100, 100), (0, int(H / 2)),
(W, int(H / 2)))
import os
from math import cos, sin, pi, floor
import pygame
from adafruit_rplidar import RPLidar
# Set up pygame and the display
os.putenv('SDL_FBDEV', '/dev/fb1')
pygame.init()
lcd = pygame.display.set_mode((320, 240))
pygame.mouse.set_visible(False)
lcd.fill((0, 0, 0))
pygame.display.update()
# Setup the RPLidar
PORT_NAME = 'COM7'
lidar = RPLidar(port='COM7', timeout=1)
# used to scale data to fit on the screen
max_distance = 0
# pylint: disable=redefined-outer-name,global-statement
def process_data(data):
global max_distance
lcd.fill((0, 0, 0))
for angle in range(360):
distance = data[angle]
if distance > 0: # ignore initially ungathered data points
max_distance = max([min([5000, distance]), max_distance])
radians = angle * pi / 180.0
x = distance * cos(radians)
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()
#####################################
cartAngle = 0 ##The custom angle to be returned.
#####################################
if(0 <= angle <= 180):
cartAngle = 180 - angle
elif(181 <= angle <= 360):
cartAngle = 540 - angle
return cartAngle
from adafruit_rplidar import RPLidar
lidar = RPLidar(None, '/dev/ttyUSB0')
sensorDistances = [0.0]*360
count = 0
prevCount = 0
totalScans = 5
scans = 0
measures = 0
APS = [0]*totalScans
import time
start = time.time()
for i, scan in enumerate(lidar.iter_scans(), start=1):
prevCount = count
