forked from faizanparabtani/SLAMLidar
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathmain.py
28 lines (26 loc) · 895 Bytes
/
main.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
import pygame
import env
import sensor
environment = env.buildEnvironment((600, 1200))
environment.originalMap = environment.map.copy()
laser = sensor.LaserSensor(200, environment.originalMap, uncertainty=(0.5, 0, 0.1))
environment.map.fill((0, 0, 0))
environment.infomap = environment.map.copy()
running = True
while running:
sensorOn = False
for event in pygame.event.get():
if event.type == pygame.QUIT:
running = False
if pygame.mouse.get_focused():
sensorOn = True
elif not pygame.mouse.get_focused():
sensorOn = False
if sensorOn:
position = pygame.mouse.get_pos()
laser.position = position
sensor_data = laser.senseObstacles()
environment.dataStorage(sensor_data)
environment.showSensorData()
environment.map.blit(environment.infomap, (0, 0))
pygame.display.update()