forked from lakshmanmallidi/PyLidar3
-
Notifications
You must be signed in to change notification settings - Fork 0
/
test.py
executable file
·52 lines (45 loc) · 1.14 KB
/
test.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
import threading
import PyLidar3
import matplotlib.pyplot as plt
import math
import time
def draw():
global is_plot
while is_plot:
plt.figure(1)
plt.cla()
plt.ylim(-5000,5000)
plt.xlim(-5000,5000)
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)
chunk = 3000
ports = ["/dev/ttyUSB0", "/dev/ttyUSB1"]
for port in ports:
Obj = PyLidar3.YdLidarX4(port, chunk) #PyLidar3.your_version_of_lidar(port,chunk_size)
if Obj.Connect():
break
else:
print("Error connecting to device")
exit(1)
threading.Thread(target=draw).start()
print(Obj.GetDeviceInfo())
gen = Obj.StartScanning(0, 180)
t = time.time() # start time
while time.time() < t + 30 : #scan for 30 seconds
#print("One loop turn")
data = next(gen)
#print(len(data), min(data), max(data))
for (angle, dist) in data.items():
if(dist>10):
x[angle] = dist * math.cos(math.radians(angle))
y[angle] = dist * math.sin(math.radians(angle))
is_plot = False
Obj.StopScanning()
Obj.Disconnect()