WIP changes to camera functions

Author: elsw

camera.py

@@ -11,6 +11,10 @@
 from viz import camera, streamer, image, blob
 import config
 
+import picamera
+import picamera.array
+import io
+
 CAMERA_REFRESH_INTERVAL=0.1
 MAX_IMAGE_AGE = 0.0
 PHOTO_PATH = "./photos"
@@ -20,6 +24,10 @@
 PHOTO_THUMB_SUFFIX = "_thumb"
 PHOTO_THUMB_SIZE = (240,180)
 VIDEO_ELAPSE_MAX = 900
+PHOTO_FILE_EXT = ".jpg"
+FFMPEG_CMD = "MP4Box"
+VIDEO_FILE_EXT = ".mp4"
+VIDEO_FILE_EXT_H264 = ".h264"
 
 class Camera(Thread):
 
@@ -28,6 +36,8 @@ class Camera(Thread):
   _warp_corners_1 = [(0, -120), (640, -120), (380, 480), (260, 480)]
   _warp_corners_2 = [(0, -60), (320, -60), (190, 240), (130, 240)]
   _warp_corners_4 = [(0, -30), (160, -30), (95, 120), (65, 120)]
+  _image_cache = None
+  _image_cache_updated = False
   stream_port = 9080
 
   @classmethod
@@ -39,19 +49,7 @@ def get_instance(cls):
 
   def __init__(self):
     logging.info("starting camera")
-    cam_props = {
-      "width":640,
-      "height":480,
-      "exposure_mode": config.Config.get().get("camera_exposure_mode"),
-      "rotation": config.Config.get().get("camera_rotation")
-    }
-    #try initialising the camera
-    try:
-      self._camera = camera.Camera(props=cam_props)
-    except:
-      self._camera = None
-      logging.error("Unexpected error:" + str(sys.exc_info()[0]))
-      pass
+    self.init_camera()
 
     self._streamer = streamer.JpegStreamer("0.0.0.0:"+str(self.stream_port), st=0.1)
     #self._cam_off_img.save(self._streamer)
@@ -70,39 +68,100 @@ def __init__(self):
 
     super(Camera, self).__init__()
 
+  def init_camera(self):
+    try:
+      props = {
+        "width":640,
+        "height":480,
+        "exposure_mode":config.Config.get().get("camera_exposure_mode"),
+        "rotation":config.Config.get().get("camera_rotation")
+      }
+      self._camera = picamera.PiCamera()
+      cam = self._camera
+      res = (props.get("width",640), props.get("height",480))
+      cam.resolution = res
+      cam.framerate = 30
+      cam.exposure_mode = props.get("exposure_mode")
+      cam.rotation = props.get("rotation")
+      self.out_jpeg = io.BytesIO()
+      self.out_rgb = picamera.array.PiRGBArray(cam, size=res)
+      self.h264_encoder = None
+      self.recording = None
+      self.video_filename = None
+    except:
+      self._camera = None
+      logging.error("Could not initialise camera:" + str(sys.exc_info()[0]))
+      pass
+
+  def grab_start(self):
+    logging.debug("grab_start")
+    camera_port_0, output_port_0 = self._camera._get_ports(True, 0)
+    self.jpeg_encoder = self._camera._get_image_encoder(camera_port_0, output_port_0, 'jpeg', None, quality=40)
+    camera_port_1, output_port_1 = self._camera._get_ports(True, 1)
+    self.rgb_encoder = self._camera._get_image_encoder(camera_port_1, output_port_1, 'bgr', res)
+    
+    with self._camera._encoders_lock:
+      self._camera._encoders[0] = self.jpeg_encoder
+      self._camera._encoders[1] = self.rgb_encoder
+
+  def grab_one(self):
+    self.out_jpeg.seek(0)
+    self.out_rgb.seek(0)
+    self.jpeg_encoder.start(self.out_jpeg)
+    self.rgb_encoder.start(self.out_rgb)
+    if not self.jpeg_encoder.wait(10):
+      raise picamera.PiCameraError('Timed Out')
+    if not self.rgb_encoder.wait(10):
+      raise picamera.PiCameraError('Timed Out')
+
+  def grab_stop(self):
+    logging.debug("grab_stop")
+    
+    with self._camera._encoders_lock:
+      del self._camera._encoders[0]
+      del self._camera._encoders[1]
+    
+    self.jpeg_encoder.close()
+    self.rgb_encoder.close()
+
   def run(self):
     if self._camera is None:
       return
     try:
-      self._camera.grab_start()
+      self.grab_start()
       while self._run:
         sleep_time = CAMERA_REFRESH_INTERVAL - (time.time() - self._image_time)
         if sleep_time <= 0:
           ts = time.time()
           #print "run.1"
           self._image_lock.acquire()
-          self._camera.grab_one()
+          self.grab_one()
           self._image_lock.release()
           #print "run.2: " + str(time.time()-ts)
           #self.save_image(image.Image(self._camera.get_image_bgr()).open().binarize().to_jpeg())
-          self.save_image(self._camera.get_image_jpeg())
+          if self._image_cache_updated is True:
+            self.save_image(self._image_cache.to_jpeg())
+            self._image_cache_updated = False
+          else:
+            self.save_image(self.out_jpeg.getvalue())
           #print "run.3: " + str(time.time()-ts)
         else:
           time.sleep(sleep_time)
 
         if self.recording and time.time() - self.video_start_time > VIDEO_ELAPSE_MAX:
           self.video_stop()
 
-      self._camera.grab_stop()
+      self.grab_stop()
     except:
       logging.error("Unexpected error:" + str(sys.exc_info()[0]))
       raise
 
   def get_image(self, maxage = MAX_IMAGE_AGE):
     if self._camera is None:
+      print "get_image: Default image"
       return Image(cv2.imread(DEFAULT_IMAGE))
     else:
-      return image.Image(self._camera.get_image_bgr())
+      return image.Image(self.out_rgb.array)
 
   def save_image(self, image_jpeg):
     self._streamer.set_image(image_jpeg)
@@ -111,13 +170,13 @@ def save_image(self, image_jpeg):
   def set_text(self, text):
     if self._camera is None:
       return
-    self._camera.set_overlay_text(str(text))
+    self._camera.annotate_text(str(text))
 
   def get_next_photo_index(self):
     last_photo_index = 0
     for p in self._photos:
       try:
-        index = int(p[len(PHOTO_PREFIX):-len(self._camera.PHOTO_FILE_EXT)])
+        index = int(p[len(PHOTO_PREFIX):-len(self.PHOTO_FILE_EXT)])
         if index > last_photo_index:
           last_photo_index = index
       except:
@@ -127,21 +186,21 @@ def get_next_photo_index(self):
   def rotate(self):
     if self._camera is None:
       return
-    rot = self._camera.camera.rotation
+    rot = self._camera.rotation
     rot = rot + 90
     if rot > 271:
       rot = 0
-    self._camera.camera.rotation = rot
+    self._camera.rotation = rot
 
   def photo_take(self):
     if self._camera is None:
       return
     photo_index = self.get_next_photo_index()
-    filename = PHOTO_PREFIX + str(photo_index) + self._camera.PHOTO_FILE_EXT;
-    filename_thumb = PHOTO_PREFIX + str(photo_index) + PHOTO_THUMB_SUFFIX + self._camera.PHOTO_FILE_EXT;
+    filename = PHOTO_PREFIX + str(photo_index) + PHOTO_FILE_EXT;
+    filename_thumb = PHOTO_PREFIX + str(photo_index) + PHOTO_THUMB_SUFFIX + PHOTO_FILE_EXT;
     of = open(PHOTO_PATH + "/" + filename, "w+")
     oft = open(PHOTO_PATH + "/" + filename_thumb, "w+")
-    im_str = self._camera.get_image_jpeg()
+    im_str = self.out_jpeg.getvalue()
     of.write(im_str)
     # thumb
     im_pil = PILImage.open(StringIO(im_str)) 
@@ -152,12 +211,11 @@ def is_recording(self):
     return self.recording
 
   def video_rec(self, video_name=None):
-    if self.is_recording():
+    if self.recording:
       return
-    self.recording = True
     if self._camera is None:
       return
-    
+    self.recording = True
     if video_name is None:
       video_index = self.get_next_photo_index()
       filename = VIDEO_PREFIX + str(video_index) + self._camera.VIDEO_FILE_EXT;
@@ -328,29 +386,48 @@ def find_color(self, s_color):
     self._image_lock.acquire()
     img = self.get_image(0)
     self._image_lock.release()
+    if img is None:
+      print "Image from get_image is None!"
+      return [0,0]
     bw = img.filter_color(color)
+    contours, hierarchy = bw.find_contours()
+    for contour in contours:
+      img.draw_contour_bound_circle(contour)
+    self._image_cache = img
+    self._image_cache_updated = True
+    if self._image_cache is None:
+      print "Image cache is None!"
     #self.save_image(bw.to_jpeg())
-    objects = bw.find_blobs(minsize=20, maxsize=1000)
-    logging.debug("objects: " + str(objects))
-    dist = -1
-    angle = 180
-
-    if objects and len(objects):
-      obj = objects[-1]
-      bottom = obj.bottom
-      logging.info("bottom: " + str(obj.center[0]) + " " +str(obj.bottom))
-      coords = bw.transform([(obj.center[0], obj.bottom)])
-      logging.info("coordinates: " + str(coords))
-      x = coords[0][0]
-      y = coords[0][1]
-      dist = math.sqrt(math.pow(12 + (68 * (120 - y) / 100),2) + (math.pow((x-80)*60/160,2)))
-      angle = math.atan2(x - 80, 120 - y) * 180 / math.pi
-      logging.info("object found, dist: " + str(dist) + " angle: " + str(angle))
+    #objects = bw.find_blobs(minsize=5, maxsize=100)
+    #logging.debug("objects: " + str(objects))
+    #dist = -1
+    #angle = 180
+
+    #if objects and len(objects):
+      #obj = objects[-1]
+      #bottom = obj.bottom
+      #logging.info("bottom: " + str(obj.center[0]) + " " +str(obj.bottom))
+      #coords = bw.transform([(obj.center[0], obj.bottom)])
+      #logging.info("coordinates: " + str(coords))
+      #x = coords[0][0]
+      #y = coords[0][1]
+      #dist = math.sqrt(math.pow(12 + (68 * (120 - y) / 100),2) + (math.pow((x-80)*60/160,2)))
+      #angle = math.atan2(x - 80, 120 - y) * 180 / math.pi
+      #logging.info("object found, dist: " + str(dist) + " angle: " + str(angle))
     #self.save_image(img.to_jpeg())
     #print "object: " + str(time.time() - ts)
-    return [dist, angle]
+    #return [dist, angle]
+    return [0,0]
+      
 
   def sleep(self, elapse):
     logging.debug("sleep: " + str(elapse))
     time.sleep(elapse)
 
+  def drawCircle(self,colour,x,y):
+    cv2.circle(self._camera.get_image_bgr(),(x,y),30,(0,0,255), -1)
+    #cv2.circle(self.get_image(0)
+    
+
+      
+