class Main: def __init__(self, **kwargs): self.mainloop = gobject.MainLoop() self.pipeline = gst.Pipeline("pipeline") self.verbose = kwargs.get('verbose', True) self.debug = kwargs.get('debug', False) cam_width = kwargs.get('cam_width', 640) cam_height = kwargs.get('cam_height', 480) host = kwargs.get('host', '127.0.0.1') port = kwargs.get('port', 5000) h264 = kwargs.get('h264', False) self.marker_spotted = False self.image_processing = ImageProcessing(area_threshold=10) self.state_estimate = StateEstimationAltitude() self.autopilot = AutoPilot(self.state_estimate) self.position_controller = PositionController(self.autopilot, self.state_estimate) if h264: self.videosrc = gst.parse_launch( 'uvch264_src device=/dev/video0 name=src auto-start=true src.vfsrc' ) else: self.videosrc = gst.element_factory_make('v4l2src', 'v4l2src') fps = 30 self.vfilter = gst.element_factory_make("capsfilter", "vfilter") self.vfilter.set_property( 'caps', gst.caps_from_string( 'image/jpeg, width=%s, height=%s, framerate=20/1' % (str(cam_width), str(cam_height)))) self.queue = gst.element_factory_make("queue", "queue") self.udpsink = gst.element_factory_make('udpsink', 'udpsink') self.rtpjpegpay = gst.element_factory_make('rtpjpegpay', 'rtpjpegpay') self.udpsink.set_property('host', host) self.udpsink.set_property('port', port) self.pipeline.add_many(self.videosrc, self.queue, self.vfilter, self.rtpjpegpay, self.udpsink) gst.element_link_many(self.videosrc, self.queue, self.vfilter, self.rtpjpegpay, self.udpsink) pad = next(self.queue.sink_pads()) # Sending frames to onVideBuffer where openCV can do processing. pad.add_buffer_probe(self.onVideoBuffer) self.pipeline.set_state(gst.STATE_PLAYING) self.i = 0 gobject.threads_init() context = self.mainloop.get_context() #previous_update = time.time() fpstime = time.time() while True: try: context.iteration(False) self.autopilot._read_sensors() if self.autopilot.auto_switch > 1700: self.position_controller.headingHold() self.position_controller.holdAltitude() self.autopilot.send_control_commands() else: self.position_controller.reset_targets() print self.autopilot.print_commands() except KeyboardInterrupt: fps = self.i / (time.time() - fpstime) print 'fps %f ' % fps self.autopilot.dump_log() self.autopilot.disconnect_from_drone() def onVideoBuffer(self, pad, idata): try: image = np.asarray( bytearray(idata), dtype=np.uint8, ) frame = cv2.imdecode(image, cv2.CV_LOAD_IMAGE_UNCHANGED) self.i += 1 marker = self.image_processing.recognize_marker(frame) self.autopilot.update_marker(marker) return True except: return True
class Main: def __init__(self, **kwargs): self.mainloop = gobject.MainLoop() self.pipeline = gst.Pipeline("pipeline") self.verbose = kwargs.get('verbose', True) self.debug = kwargs.get('debug', False) cam_width = kwargs.get('cam_width', 640) cam_height = kwargs.get('cam_height', 480) host = kwargs.get('host', '127.0.0.1') port = kwargs.get('port', 5000) h264 = kwargs.get('h264', False) self.marker_spotted = False self.image_processing = ImageProcessing(area_threshold=10) self.state_estimate = StateEstimationAltitude() self.autopilot = AutoPilot(self.state_estimate) self.position_controller = PositionController( self.autopilot, self.state_estimate) if h264: self.videosrc = gst.parse_launch( 'uvch264_src device=/dev/video0 name=src auto-start=true src.vfsrc') else: self.videosrc = gst.element_factory_make('v4l2src', 'v4l2src') fps = 30 self.vfilter = gst.element_factory_make("capsfilter", "vfilter") self.vfilter.set_property('caps', gst.caps_from_string( 'image/jpeg, width=%s, height=%s, framerate=20/1' % (str(cam_width), str(cam_height)))) self.queue = gst.element_factory_make("queue", "queue") self.udpsink = gst.element_factory_make('udpsink', 'udpsink') self.rtpjpegpay = gst.element_factory_make('rtpjpegpay', 'rtpjpegpay') self.udpsink.set_property('host', host) self.udpsink.set_property('port', port) self.pipeline.add_many( self.videosrc, self.queue, self.vfilter, self.rtpjpegpay, self.udpsink) gst.element_link_many( self.videosrc, self.queue, self.vfilter, self.rtpjpegpay, self.udpsink) pad = next(self.queue.sink_pads()) # Sending frames to onVideBuffer where openCV can do processing. pad.add_buffer_probe(self.onVideoBuffer) self.pipeline.set_state(gst.STATE_PLAYING) self.i = 0 gobject.threads_init() context = self.mainloop.get_context() #previous_update = time.time() fpstime = time.time() while True: try: context.iteration(False) self.autopilot._read_sensors() if self.autopilot.auto_switch > 1700: self.position_controller.headingHold() self.position_controller.holdAltitude() self.autopilot.send_control_commands() else: self.position_controller.reset_targets() print self.autopilot.print_commands() except KeyboardInterrupt: fps = self.i / (time.time() - fpstime) print 'fps %f ' % fps self.autopilot.dump_log() self.autopilot.disconnect_from_drone() def onVideoBuffer(self, pad, idata): try: image = np.asarray( bytearray(idata), dtype=np.uint8, ) frame = cv2.imdecode(image, cv2.CV_LOAD_IMAGE_UNCHANGED) self.i += 1 marker = self.image_processing.recognize_marker(frame) self.autopilot.update_marker(marker) return True except: return True
class OfflineVideo(): def __init__(self, video_file="data/test_5/drone_eye.avi"): self.cap = cv2.VideoCapture(video_file) self.cap.set(cv.CV_CAP_PROP_FPS, 30) self.image_processing = ImageProcessing(area_treshold=300) self.writer = cv2.VideoWriter(filename="kalman_tracking5.avi", fps=30, frameSize=( 320, 240), fourcc=cv.CV_FOURCC('M', 'J', 'P', 'G')) self.cam_altitude = [] self.observations = [] #self.autopilot = AutoPilot(simulate=True, thrust_step=30, pixel_threshold=10, cam_width=320, cam_height=240) # self.ras_MIN = np.array([150, 80, 80], np.uint8) # self.ras_MAX = np.array([175, 255, 255], np.uint8) def run(self): i = 0 try: while True: flag, frame = self.cap.read() if i % 3 == 0: marker = self.image_processing.recognize_marker(frame) if marker: self.cam_altitude.append(marker.z) self.observations.append(marker) cv2.circle(frame, (marker.x, marker.y), 5, 255, -1) # print marker.rect # print rect[] x, y, w, h = cv2.boundingRect(marker.best_cnt) cv2.rectangle( frame, (x, y), (x + w, y + h), (0, 255, 0), 2) cv2.putText(frame, 'altitude %s ' % marker.z, ( 20, 30), cv2.FONT_HERSHEY_PLAIN, 2, (0, 255, 0)) cv2.putText(frame, 'X: %s Y: %s ' % (str(marker.x), str(marker.y)), (20, 50), cv2.FONT_HERSHEY_PLAIN, 2, (0, 255, 0)) else: self.cam_altitude.append(None) self.observations.append(None) i += 1 #time.sleep(0.3) self.writer.write(frame) cv2.imshow('drone eye', frame) cv2.waitKey(5) except: # draw estimates pickle.dump(self.cam_altitude, open('cam_alt.dump', 'wb')) pickle.dump( self.observations, open('marker_observations5.dump', 'wb')) # pl.figure(size=(320,240)) x = [o.x for o in self.observations if o] y = [o.y for o in self.observations if o] # obs_scatter = pl.scatter(x, y, marker='x', color='b', # label='observations') position_line = pl.plot(x, y, linestyle='-', marker='o', color='r', label='position est.') #lines_true = pl.plot(self.cam_altitude, color='b') # observations = pl.plot(x, color='b') # lines_filt = pl.plot(filtered_state_means, color='r') # pl.legend((lines_true[0]), ('Camera altitude')) pl.show()