PyIoTsim/setupmap.py at master · mpuheim/PyIoTsim · GitHub

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
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
from modules import gui
from modules.config import res,simstep
import time

### --- CONFIG --- ###
image_path = "tracks/default.png"
track_path = "newtrack.track"
### -END-OF-CONFIG- ###

### screen initialization
screen = gui.Init(res)
### load map image
image = gui.LoadImage(image_path)
### draw image on screen
gui.DrawImage(screen,image)

### prepare trajectory list
track = []

### capture trajectory
while True:
    # refresh GUI and check for mouse click
    event = gui.CaptureClick(screen)
    # handle program exit
    if event == 0: break
    # continue if there was no click
    if event == 1: continue
    # save new track point
    click = event
    track.append(click)
    print(click)
    # show track on map
    gui.DrawDot(screen, click[0], click[1], size=2, color=(0, 180, 0))
    if len(track)>1:
        gui.DrawLine(screen, track[-1][0], track[-1][1], track[-2][0], track[-2][1])

### prepare camera list
cams = []

### setup cameras
map = screen.surface.copy()
cam = ()
while True:
    # get mouse position
    pointer = gui.MousePosition(screen)
    # draw FOV line
    if len(cam)>=2:
        gui.ClrScr(screen)
        gui.DrawSurface(screen,map)
        gui.DrawLine(screen, cam[0], cam[1], pointer[0], pointer[1], color=(255, 0, 0))
    # refresh GUI and check for mouse click
    event = gui.CaptureClick(screen)
    # continue if there was no click
    if event == 1: continue
    # handle program exit
    if event == 0: break
    # save cam coordinates
    cam += event
    # draw cam position
    if len(cam)==2: gui.DrawDot(screen, cam[0], cam[1], size=3, color=(255, 0, 0))
    # save completed coordinates to list
    if len(cam)==6:
        # draw complete FOV polygon
        gui.DrawLine(screen, cam[2], cam[3], cam[4], cam[5], color=(255, 255, 0))
        # add cam to list
        cams.append(cam)
        cam = ()
        print(cams[-1])
    # save map surface
    map = screen.surface.copy()

### close map screen
gui.CloseScreen(screen)

### save results to file
with open(track_path,"w",encoding="utf-8") as f:
    f.write("# Map image file: \n")
    f.write(image_path + "\n")
    f.write("# Map size: \n")
    f.write(str(res[0])+" "+str(res[1])+"\n")
    f.write("# Track coordinates: \n")
    for coords in track:
        f.write(str(coords[0])+" "+str(coords[1])+"\n")
    f.write("# Cam coordinates: \n")
    for coords in cams:
        f.write(" ".join([str(c) for c in coords])+"\n")

### print info
print("\nTrack saved to file '"+track_path+"'")
print("\nTrack coordinates:",track)
print("\nCam coordinates:",cams)
time.sleep(2)
input("\nPres enter to exit...")