/
RoverPylot.py
185 lines (155 loc) · 5.22 KB
/
RoverPylot.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
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
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
import cv2
import pygame
from rover import RoverShell
from pygame.locals import *
from time import sleep
from datetime import date
from random import choice
from string import ascii_lowercase, ascii_uppercase
import cStringIO
import qrtools
import numpy as np
#https://github.com/circleupx/RALVINN2.0/commit/6976987d3cbd0183641eea8059f090a4f41bbed9
def create_opencv_image_from_stringio(img_stream, cv2_img_flag=1):
img_stream.seek(0)
img_array = np.asarray(bytearray(img_stream.read()), dtype=np.uint8)
return cv2.imdecode(img_array, cv2_img_flag)
def cvimage_to_pygame(image):
new_image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
return pygame.image.frombuffer(new_image.tostring(), new_image.shape[1::-1], "RGB")
class RoverControl():
def __init__(self):
pygame.init()
pygame.display.init()
self.quit = False
self.rover = RoverShell()
self.fps = 48
self.windowSize = [290, 230]
self.imageRect = (0, 0, 320, 240)
self.displayCaption = "RALVINN CAMERA"
pygame.display.set_caption(self.displayCaption)
print "Battery: " + str(self.rover.getBatteryPercentage())
self.screen = pygame.display.set_mode(self.windowSize)
self.clock = pygame.time.Clock()
self.run()
def run(self):
sleep(1.5)
while not self.quit:
self.update_rover_state()
self.resfresh_video_feed()
self.qr_code()
self.rover.quit = True
pygame.quit()
def resfresh_video_feed(self):
"""
"""
self.rover.lock.acquire()
image = self.rover.currentImage
self.rover.lock.release()
cv_image = create_opencv_image_from_stringio(cStringIO.StringIO(image))
image = cvimage_to_pygame(cv_image)
# Save the array of bytes into an image file named qr.png
fname = 'qr.png'
fd = open(fname, 'w')
fd.write(str(self.rover.currentImage))
fd.close()
self.screen.blit(image, (0, 0))
# Show image array over PyGame display
pygame.display.update(self.imageRect)
self.clock.tick(self.fps)
@staticmethod
def qr_code():
# decode the qr image.
qr = qrtools.QR()
if qr.decode('qr.png'):
print qr.data
else:
pass
def update_rover_state(self):
"""
"""
for event in pygame.event.get():
if event.type == QUIT:
self.quit = True
elif event.type == KEYDOWN:
if event.key in (K_j, K_k, K_SPACE, K_u, K_i, K_o):
self.updatePeripherals(event.key)
elif event.key in (K_w, K_a, K_s, K_d, K_q, K_e, K_z, K_c, K_r, K_l):
self.updateTreads(event.key)
else:
pass
elif event.type == KEYUP:
if event.key in (K_w, K_a, K_s, K_d, K_q, K_e, K_z, K_c, K_r, K_l):
self.updateTreads()
elif event.key in (K_j, K_k):
self.updatePeripherals()
else:
pass
else:
pass
def takepicure(self):
"""
"""
with open(self.newpicturename, 'w') as pic:
self.rover.lock.acquire()
pic.write(self.rover.currentImage)
self.rover.lock.release()
@property
def newpicturename(self):
"""
:return:
"""
todaysDate = str(date.today())
uniquekey = ''.join(choice(ascii_lowercase + ascii_uppercase))
for _ in range(4):
return todaysDate + '_' + uniquekey + '.jpq'
def updateTreads(self, key=None):
# tread speed ranges from 0 (none) to one (full speed) so [.5 ,.5] would be half full speed
"""
:param key:
"""
if key is None:
self.rover.treads = [0, 0]
elif key is K_w:
self.rover.treads = [1, 1]
elif key is K_s:
self.rover.treads = [-1, -1]
elif key is K_a:
self.rover.treads = [-1, 1]
elif key is K_d:
self.rover.treads = [1, -1]
elif key is K_q:
self.rover.treads = [.1, 1]
elif key is K_e:
self.rover.treads = [1, .1]
elif key is K_z:
self.rover.treads = [-.1, -1]
elif key is K_c:
self.rover.treads = [-1, -.1]
elif key is K_l:
pass
else:
pass
def updatePeripherals(self, key=None):
"""
:param key:
"""
if key is None:
self.rover.peripherals['camera'] = 0
elif key is K_j:
self.rover.peripherals['camera'] = 1
elif key is K_k:
self.rover.peripherals['camera'] = -1
elif key is K_u:
self.rover.peripherals['stealth'] = not \
self.rover.peripherals['stealth']
elif key is K_i:
self.rover.peripherals['lights'] = not \
self.rover.peripherals['lights']
elif key is K_o:
self.rover.peripherals['detect'] = not \
self.rover.peripherals['detect']
elif key is K_SPACE:
self.takepicure()
else:
pass