-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathLidarTest.py
43 lines (39 loc) · 1.14 KB
/
LidarTest.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
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(-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 = input("Enter port name which lidar is connected:") #windows
Obj = PyLidar3.YdLidarX4(port) #PyLidar3.your_version_of_lidar(port,chunk_size)
threading.Thread(target=draw).start()
if(Obj.Connect()):
print(Obj.GetDeviceInfo())
gen = Obj.StartScanning()
t = time.time() # start time
while (time.time() - t) < 300: #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:
print("Error connecting to device")