def test_py_lidar3():
import PyLidar3
import serial
import glob
import time # Time module
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)
except serial.SerialException:
pass
print("available ports", result)
port = result[0] # linux
# PyLidar3.your_version_of_lidar(port,chunk_size)
ydlidar = PyLidar3.YdLidarX4(port)
if ydlidar.Connect():
print(ydlidar.GetDeviceInfo())
gen = ydlidar.StartScanning()
t = time.time() # start time
while (time.time() - t) < 30: # scan for 30 seconds
print(next(gen))
time.sleep(0.5)
ydlidar.StopScanning()
ydlidar.Disconnect()
else:
print("Error connecting to device")
def __init__(self):
#Link from /dev/ttyUSB0 to /dev/ydlidar previously made
self.lidar = PyLidar3.YdLidarX4("/dev/ttyUSB0")
if not self.lidar.Connect():
print("Error connecting to the Lidar")
raise ValueError("Error connecting to the Lidar")
def findYDLidarX4(ttys = ["/dev/ttyUSB0", "/dev/ttyUSB1"]):
try:
for tmptty in ttys:
Obj = PyLidar3.YdLidarX4(tmptty) #PyLidar3.your_version_of_lidar(port,chunk_size)
if(Obj.Connect()):
print(Obj.GetDeviceInfo())
Obj.Disconnect()
return tmptty
t = 0/0
except:
return None
def __init__(self, port='/dev/ttyUSB0'):
import PyLidar3
self.port = port
self.distances = [] #a list of distance measurements
self.angles = [] # a list of angles corresponding to dist meas above
self.lidar = PyLidar3.YdLidarX4(port)
if(self.lidar.Connect()):
print(self.lidar.GetDeviceInfo())
self.gen = self.lidar.StartScanning()
else:
print("Error connecting to lidar")
self.on = True
def main():
screen = initScreen("Lidar GUI",SIZE)
done = False
clock = pygame.time.Clock()
#Serial port to which lidar connected, Get it from device manager windows
#In linux type in terminal -- ls /dev/tty*
lidar = PyLidar3.YdLidarX4("/dev/ttyUSB0")
if(lidar.Connect()):
print(lidar.GetDeviceInfo())
gen = lidar.StartScanning()
start = time.time()
# Run for given time and make sure window is not closed.
while (time.time() - start) < 5 and not done:
data = next(gen)
#print("Data:",type(data),data)
screen.fill(WHITE) # Always clear the screen.
for event in pygame.event.get(): # User did something!
if event.type == pygame.QUIT: # If user clicked close...
done = True # Flag done so we exit this loop
for angle in range(360):
r = data[angle]
drawRangeBearing(screen,CENTER,r,angle)
pygame.display.flip() # Update screen to display all changes made.
clock.tick(2) # Limits loop to this many times per second.
lidar.StopScanning()
lidar.Disconnect()
print("Device disconnected")
else:
print("Error connecting to device")
pygame.quit() # Be IDLE friendly
def main():
screen = initScreen("Lidar GUI",SIZE)
done = False
clock = pygame.time.Clock()
#Serial port to which lidar connected, Get it from device manager windows
#In linux type in terminal -- ls /dev/tty*
port = input("Enter port name which lidar is connected:") #windows
#port = "/dev/ttyUSB0" #linux
lidar = PyLidar3.YdLidarX4(port) #PyLidar3.your_version_of_lidar(port,chunk_size)
if lidar.Connect():
#PyGame chunk
while not done:
screen.fill(WHITE) # Always clear the screen.
clock.tick(60) # Limits loop to this many times per second.
for event in pygame.event.get(): # User did something!
if event.type == pygame.QUIT: # If user clicked close...
done = True # Flag done so we exit this loop
data = get_data()
for d in data:
drawRangeBearing(screen,CENTER,d[0],d[1])
pygame.display.flip() # Update screen to display all changes made.
print(lidar.GetDeviceInfo())
gen = lidar.StartScanning()
t = time.time() # start time
data = next(gen)
print(type(data))
lidar.StopScanning()
def start(port):
global x, y
for _ in range(360):
x.append(0)
y.append(0)
Obj = PyLidar3.YdLidarG4(port)
if (Obj.Connect()):
print(Obj.GetDeviceInfo())
gen = Obj.StartScanning()
t = time.time() # start time
while (time.time() - t) < 30: # scan for 30 seconds
data = next(gen)
for angle in range(0, 360):
if (data[angle] > 1000):
x[angle] = data[angle] * math.cos(math.radians(angle))
y[angle] = data[angle] * math.sin(math.radians(angle))
draw()
plt.show()
Obj.StopScanning()
Obj.Disconnect()
else:
print("Error connecting to device")
def scanYDLidarX4():
global scantime
global lidarport
myscan = [0.0 for deg in range(360)] #we have one value for every angle
Obj = PyLidar3.YdLidarX4(lidarport)
if(Obj.Connect()):
gen = Obj.StartScanning()
t = time.time() # start time
scanlist = []
while (time.time() - t) < scantime:
scanlist.append(next(gen))
time.sleep(0.5)
Obj.StopScanning()
Obj.Disconnect()
for i in range(360):
sum = 0.0
for tmpscan in scanlist:
sum = sum +tmpscan[i]
myscan[i] = (float(sum)/len(scanlist))/1000.0
print(len(scanlist))
else:
print("Error connecting to device")
return myscan
def runLidar(t_max):
"""
Description: Establishes a serial connection with the LiDAR, starts scanning and returns the data
:param t_max: Maximum run time
:return raw_data: The raw data
"""
lidar = PyLidar3.YdLidarG4(port)
if lidar.Connect():
print(lidar.GetDeviceInfo())
gen = lidar.StartScanning()
t1 = time.time() # start time
t2 = t1
raw_data = []
while t2 - t1 < t_max:
d = next(gen)
t2 = time.time()
raw_data.append([t2, d])
lidar.StopScanning()
lidar.Disconnect()
return raw_data
else:
print("Error connecting to device")
return 0
#implementation of Vector Field Histogram+ (VFH+) algorithm in Python #Created by Silvester for bachelor final project #Email: [email protected] import threading import PyLidar3 # import matplotlib.pyplot as plt import math import time import pyqtgraph as pg from pyqtgraph.Qt import QtCore, QtGui port = ("COM10") #windows, cara langsung Obj = PyLidar3.YdLidarX4(port) window = pg.plot(title="VFH+") x1 = [] # mendeklarasikan bahwa x adalah list, x adalah sudut dalam radian y1 = [] # y adalah jarak (radius) prim = [] binA = [] binB = [] binB1 = [] binC = [] mask = [] a_index = [] b_index = [] index = [] finalHis = [] plot_keluaran = [] jarak = [] binA_plot = [] binB_plot = []
direction = 0.0 # direction in radians
def drive():
while True:
romi.twist(speed, 0)
x = []
y = []
for _ in range(360):
x.append(0)
y.append(0)
port = "/dev/ttyUSB0" #linux
Obj = PyLidar3.YdLidarX4(
port) #PyLidar3.your_version_of_lidar(port,chunk_size)
threading.Thread(target=drive).start()
if (Obj.Connect()):
print(Obj.GetDeviceInfo())
gen = Obj.StartScanning()
t = time.time() # start time
while (time.time() - t) < 10: #scan for 30 seconds
data = next(gen)
print("got new data")
for angle in range(0, 360):
if (data[angle] > 1000):
x[angle] = data[angle] * math.cos(math.radians(angle))
y[angle] = data[angle] * math.sin(math.radians(angle))
Obj.StopScanning()
Obj.Disconnect()
else:
plt.ylim(-9000, 9000)
plt.xlim(-9000, 9000)
plt.scatter(x, y, c='r', s=8)
plt.pause(0.001)
plt.close("all")
is_plot = True
x = []
y = []
for _ in range(360):
x.append(0)
y.append(0)
port = "/dev/ttyACM0" #linux
Obj = PyLidar3.YdLidarG4(port)
threading.Thread(target=draw).start()
if (Obj.Connect()):
print(Obj.GetDeviceInfo())
gen = Obj.StartScanning()
t = time.time() # start time
while (time.time() - t) < 30: #scan for 30 seconds
data = next(gen)
for angle in range(0, 360):
if (data[angle] > 1000):
x[angle] = data[angle] * math.cos(math.radians(angle))
y[angle] = data[angle] * math.sin(math.radians(angle))
is_plot = False
Obj.StopScanning()
Obj.Disconnect()
else:
import time
import math
import PyLidar3
Sweeper = PyLidar3.YdLidarX4('COM6', 6000)
Sweeper.Connect()
SWEEPER_GENERATOR = Sweeper.StartScanning()
t = time.time()
while time.time() - t < 2:
lidardata = next(SWEEPER_GENERATOR)
for angle in range(0, 360):
print(lidardata[angle], angle)
Sweeper.StopScanning()
Sweeper.Disconnect()
if not "command" in query:
if log == 1:
print(threaderror, "Faltan parámetros")
continue
# Leer comandos, configurar lidar y preparar respuesta
if query['command'] == Command.CMD_CONNECT:
if log == 1:
print(threadcomm, str(client),
" >> Conectar a lidar")
storedata['lidar'] = PyLidar3.YdLidarX4(PORT, 6000)
if storedata['lidar'].Connect():
response['response'] = Response.RES_OK
else:
response['response'] = Response.RES_FAIL
if query['command'] == Command.CMD_START_SCANNING:
if log == 1:
print(threadcomm, str(client),
" >> Iniciar escaneo")
try:
storedata['lidargen'] = storedata[
import PyLidar3
import time
lidar = PyLidar3.YdLidarX4("/dev/ttyUSB0")
if (lidar.Connect()):
print(lidar.GetDeviceInfo())
gen = lidar.StartScanning()
t = time.time() # start time
while (time.time() - t) < 5: # scan for 30sec
print(next(gen))
time.sleep(0.5)
lidar.StopScanning()
lidar.Disconnect()
print("Device disconnected")
else:
print("Error connecting to device")
