-
Notifications
You must be signed in to change notification settings - Fork 0
/
rplidar_datapoint_visualisation.py
60 lines (59 loc) · 4.1 KB
/
rplidar_datapoint_visualisation.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
51
52
53
54
55
56
57
58
59
60
#doesn't work yet
import os
from math import cos, sin, pi, floor
import pygame
import time
# from adafruit_rplidar import RPLidar
# Set up pygame and the display
os.putenv('SDL_FBDEV', '/dev/fb1')
pygame.init()
# Open a window on the screen
screen_width=320
screen_height=240
lcd = pygame.display.set_mode([screen_width,screen_height],pygame.RESIZABLE)
pygame.mouse.set_visible(False)
lcd.fill((0,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
scan_data = [(15.0, 92.578125, 434.5), (15.0, 94.015625, 431.5), (15.0, 95.375, 430.0), (15.0, 96.578125, 429.25), (15.0, 97.703125, 430.5), (15.0, 99.03125, 432.25),
(15.0, 100.390625, 432.25), (15.0, 102.0, 433.75), (15.0, 103.03125, 437.25), (15.0, 104.59375, 440.75), (15.0, 105.859375, 444.0), (15.0, 107.265625, 446.5),
(15.0, 108.296875, 449.5), (15.0, 109.703125, 453.75), (15.0, 111.046875, 459.5), (15.0, 112.140625, 465.0), (15.0, 122.9375, 3427.5), (15.0, 124.296875, 3365.0),
(15.0, 125.609375, 3378.25), (15.0, 139.90625, 1058.0), (15.0, 145.125, 1193.25), (15.0, 146.46875, 1177.5), (15.0, 147.765625, 1168.5), (15.0, 149.171875, 1159.75),
(15.0, 150.46875, 1143.25), (15.0, 151.890625, 1130.5), (15.0, 153.15625, 1128.25), (15.0, 154.53125, 1133.0), (15.0, 155.8125, 1147.5), (15.0, 161.578125, 5617.0),
(15.0, 162.90625, 5560.5), (15.0, 164.234375, 5517.5), (15.0, 165.578125, 5433.0), (15.0, 166.921875, 5348.75), (15.0, 168.265625, 5300.25), (15.0, 198.125, 436.75),
(15.0, 201.015625, 404.5), (15.0, 202.640625, 406.25), (15.0, 203.625, 406.25), (15.0, 210.125, 482.75), (15.0, 211.453125, 795.25), (15.0, 215.46875, 812.0),
(15.0, 216.8125, 818.25), (15.0, 218.03125, 822.0), (15.0, 219.4375, 829.0), (15.0, 220.671875, 836.75), (15.0, 222.0625, 846.25), (15.0, 223.34375, 855.75),
(15.0, 224.78125, 866.25), (15.0, 226.09375, 877.0), (15.0, 227.375, 889.0), (15.0, 228.734375, 902.25), (15.0, 233.9375, 1006.25), (15.0, 235.359375, 976.5),
(15.0, 246.125, 334.0), (10, 247.6875, 785.25), (15.0, 249.125, 770.0), (15.0, 250.484375, 756.25), (15.0, 251.8125, 743.0), (15.0, 253.28125, 730.75), (15.0, 254.75, 719.5),
(15.0, 260.828125, 495.75), (15.0, 267.0, 647.5), (15.0, 268.453125, 641.75), (15.0, 269.671875, 636.75), (15.0, 271.1875, 632.0), (15.0, 272.515625, 628.5),
(15.0, 273.703125, 625.0), (15.0, 275.203125, 621.75), (15.0, 276.4375, 619.0), (15.0, 277.734375, 616.5), (15.0, 279.15625, 614.5), (15.0, 280.5, 612.5),
(15.0, 281.84375, 610.75), (15.0, 283.203125, 609.5), (15.0, 284.53125, 608.5), (15.0, 285.875, 607.75), (15.0, 287.203125, 605.5), (15.0, 288.59375, 606.0),
(15.0, 289.9375, 609.0), (15.0, 291.21875, 610.0), (15.0, 292.515625, 612.0), (15.0, 293.828125, 614.25), (15.0, 295.1875, 615.75), (15.0, 296.5625, 617.75),
(15.0, 297.78125, 620.5), (15.0, 299.140625, 623.25), (15.0, 308.640625, 232.0), (15.0, 320.09375, 334.0), (15.0, 332.390625, 273.0), (15.0, 333.46875, 275.5),
(15.0, 334.515625, 278.25), (15.0, 336.265625, 282.25), (15.0, 344.703125, 400.75), (15.0, 346.359375, 399.0), (15.0, 347.40625, 397.25), (15.0, 348.96875, 397.5)]
def process_data(data):
for scan_data in data:
global max_distance
lcd.fill((0,0,0))
_, angle, distance = scan_data
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)
y = distance * sin(radians)
point = (160 + int(x / max_distance * 119), 120 + int(y / max_distance * 119))
lcd.set_at(point, pygame.Color(255, 255, 255))
time.sleep(1)
pygame.display.update()
try:
# for scan in scan_data:
# (_, angle, distance) = scan
# scan[min([359, floor(angle)])] = distance
process_data(scan_data)
# time.sleep(50)
except KeyboardInterrupt:
print('Stoping.')