def findGoodFeatures(self, img):
gray = cv.CreateImage ((320, 240), cv.IPL_DEPTH_8U, 1)
cv.CvtColor(img, gray,cv.CV_BGR2GRAY)
eig_image = cv.CreateImage(cv.GetSize(img), cv.IPL_DEPTH_32F, 1)
temp_image = cv.CreateImage(cv.GetSize(img), cv.IPL_DEPTH_32F ,1)
for (x,y) in cv.GoodFeaturesToTrack(gray, eig_image, temp_image, 10, 0.04, 1.0, useHarris = True):
Utils.dprint(DEBUG, "good feature at ("+ str(x)+"," + str(y))
cv.Circle(img, (int(x), int(y)), 10, cv.RGB(17, 110, 255))
Utils.dprint(DEBUG, '=====================================================')
def run(self):
while not self.stopping:
for e in pygame.event.get(): # iterate over event stack
#print 'event : ' + str(e.type)
if e.type == pygame.JOYAXISMOTION:
roll = self.js.get_axis(0)
pitch = self.js.get_axis(1)
yaw = self.js.get_axis(3)
power2 = self.convert( self.js.get_axis(2) )
power1 = self.convert( self.js.get_axis(5) )
power = power1 - power2
self.drone.move(roll, pitch, power, yaw)
Utils.dprint( DEBUG, 'roll: ' + str(roll) + ' pitch: ' + str(pitch) + ' power: ' + str(power) + ' yaw: ' + str(yaw) )
elif e.type == pygame.JOYBUTTONDOWN: # 10
for b in xrange(self.js.get_numbuttons()):
if self.js.get_button(b) > 0:
print 'number of button pushed: ' + str(b)
if b==0:
self.drone.stop()
self.stop()
break
elif b==1:
self.drone.reset()
elif b==2:
self.drone.startVideo()
elif b==3:
self.drone.ledShow(6)
# elif b==4:
# pass
elif b==5:
self.drone.zap()
# elif b==6:
# pass
# elif b==7:
# pass
elif b==8:
if self.drone.getLanded():
if self.drone.takeoff():
self.drone.setLanded(False)
else:
if self.drone.land():
self.drone.setLanded(True)
# elif b==9:
# pass
# elif b==10:
# pass
elif b==11:
self.drone.rotate(-1)
elif b==12:
self.drone.rotate(1)
def __init__(self, test, record, multicast):
multiprocessing.Process.__init__(self)
self.DRONE_IP = '192.168.1.1'
self.INIT_PORT = 5555
self.INITMSG = "\x01\x00\x00\x00"
self.MCAST_GRP = '224.1.1.1'
self.VIDEO_PORT = 5555
self.window = "OpenCV window: " + str(datetime.datetime.now())
self.lock = multiprocessing.Lock()
self.com_pipe, self.com_pipe_other = multiprocessing.Pipe()
self.queue = multiprocessing.Queue(100000)
self.state = INIT
self.wakeup = 2500
self.MCAST = multicast
self.RECORD = record
self.TEST = test
self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP)
self.sock.setblocking(0)
self.sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
#self.sock.setsockopt(socket.SOL_SOCKET, socket.SO_RCVBUF, 200000)
Utils.dprint(DEBUG, 'buffer:\t' + str(self.sock.getsockopt(socket.SOL_SOCKET, socket.SO_RCVBUF)))
if self.TEST:
self.DRONE_IP = '127.0.0.1'
self.INIT_PORT = 5550
self.testdevice = TestDevice.TestDevice(False)
self.testdevice.start()
while not self.testdevice.isAlive():
pass
if self.RECORD:
Utils.ensure_dir('./testdata')
fps = 12
width, height = int(320), int(240)
fourcc = cv.CV_FOURCC('I','4','2','0')
self.writer = cv.CreateVideoWriter('out.avi', fourcc, fps, (width, height), 1)
if self.MCAST:
self.INITMSG = "\x02\x00\x00\x00"
self.sock.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_TTL, 32)
self.sock.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_LOOP, 1)
self.sock.setsockopt(socket.SOL_IP, socket.IP_MULTICAST_IF, socket.inet_aton('192.168.1.2'))
self.sock.setsockopt(socket.SOL_IP, socket.IP_ADD_MEMBERSHIP, socket.inet_aton(self.MCAST_GRP) + socket.inet_aton('192.168.1.2'))
self.sock.bind(('', self.VIDEO_PORT))
def run(self):
#self.queue.put('derpa')
cv.NamedWindow(self.window)
#cv.SetMouseCallback(self.window, self.callBack, None)
runs = 0
state = STARTED
mode = NORMAL
time_start = datetime.datetime.now()
self.initVideo()
while not state == STOPPING:
Utils.dprint(DEBUG, 'started')
inputready, outputready, exceptready = select.select([self.sock, self.com_pipe], [], [], 1)
if len(inputready) == 0:
self.initVideo()
for i in inputready:
if i == self.sock:
try:
data, addr = self.sock.recvfrom( 65535 )
except socket.error, e:
Utils.dprint(DEBUG, e)
if data:
runs += 1
img = self.updateScreen(data, runs)
if mode == PRINT:
vprint = self.extractPrint(img)
#self.queue.get(False)
self.queue.put(vprint)
mode = NORMAL
elif i == self.com_pipe:
if i.recv() == 'print':
mode = PRINT
else:
state = STOPPING
def main():
pygame.init()
pygame.joystick.init()
if pygame.joystick.get_count() > 0:
js = pygame.joystick.Joystick(0)
js.init()
print js.get_name()
else:
print 'No joystick available, exiting... should default to keyboard mode'
return False
Utils.dprint(DEBUG, '1')
drone = Drone.Drone(False)
controller = Controller(drone, js)
controller.start()
controller.join()
Utils.dprint(DEBUG, '2')
#js.quit()
pygame.joystick.quit()
pygame.quit()
Utils.dprint(DEBUG, '3')
def stop(self):
Utils.dprint(DEBUG, '4: stopping control thread')
self.stopping = True
def initVideo(self):
self.sock.sendto(self.INITMSG, (self.DRONE_IP, self.INIT_PORT))
self.sock.sendto(self.INITMSG, (self.DRONE_IP, self.INIT_PORT))
self.sock.sendto(self.INITMSG, (self.DRONE_IP, self.INIT_PORT))
Utils.dprint(DEBUG, 'initing')
