added user callback function to vision and tested it (for mambo)

Mambo.py

@@ -133,7 +133,7 @@ def update(self, name, value, sensor_enum):
             self.sensors_dict[name] = value
 
         # call the user callback if it isn't None
-        if (not None):
+        if (self.user_callback_function is not None):
             self.user_callback_function(self.user_callback_function_args)
 
     def get_estimated_z_orientation(self):

MamboVision.py

@@ -18,6 +18,8 @@ def __init__(self, buffer_size=10):
         until you call open_video.
 
         :param buffer_size: number of frames to buffer in memory.  Defaults to 10.
+        :param user_callback_function: callback function that will be called every time a new frame is available (optional)
+        :param user_callback_args: arguments to the user callback function (optional)
         """
         self.fps = 30
 
@@ -28,10 +30,23 @@ def __init__(self, buffer_size=10):
         self.buffer_index = 0
 
         # setup the thread for monitoring the vision (but don't start it until we connect in open_video)
-        self.vision_thread = threading.Thread(target=self._buffer_vision, args=(buffer_size, ))
-
+        self.vision_thread = threading.Thread(target=self._buffer_vision,
+                                              args=(buffer_size, ))
+        self.user_vision_thread = None
         self.vision_running = True
 
+    def set_user_callback_function(self, user_callback_function=None, user_callback_args=None):
+        """
+        Set the (optional) user callback function for handling the new vision frames.  This is
+        run in a separate thread that starts when you start the vision buffering
+
+        :param user_callback_function: function
+        :param user_callback_args: arguments to the function
+        :return:
+        """
+        self.user_vision_thread = threading.Thread(target=self._user_callback,
+                                                   args=(user_callback_function, user_callback_args))
+
 
     def open_video(self, max_retries=3):
         """
@@ -48,10 +63,10 @@ def open_video(self, max_retries=3):
         :return True if the vision opened correctly and False otherwise
         """
         print("opening the camera")
-        self.capture = cv2.VideoCapture("rtsp://192.168.99.1/media/stream2")
+        #self.capture = cv2.VideoCapture("rtsp://192.168.99.1/media/stream2")
 
         # if you do 0, it opens the laptop webcam
-        #self.capture = cv2.VideoCapture(0)
+        self.capture = cv2.VideoCapture(0)
 
         # if it didn't open the first time, try again a maximum number of times
         try_num = 1
@@ -73,6 +88,26 @@ def start_video_buffering(self):
             print("starting vision thread")
             self.vision_thread.start()
 
+            if (self.user_vision_thread is not None):
+                self.user_vision_thread.start()
+
+    def _user_callback(self, user_vision_function, user_args):
+        """
+        Internal method to call the user vision functions
+
+        :param user_vision_function: user callback function to handle vision
+        :param user_args: optional arguments to the user callback function
+        :return:
+        """
+
+        while (self.vision_running):
+            if (self.new_frame):
+                user_vision_function(user_args)
+
+            # put the thread back to sleep for fps
+            time.sleep(1.0 / self.fps)
+
+
     def _buffer_vision(self, buffer_size):
         """
         Internal method to save valid video captures from the camera fps times a second
@@ -82,14 +117,18 @@ def _buffer_vision(self, buffer_size):
         """
 
         while (self.vision_running):
+            #reset the bit
+            self.new_frame = False
+
             # grab the latest image
             capture_correct, video_frame = self.capture.read()
             if (capture_correct):
                 self.buffer_index += 1
                 self.buffer_index %= buffer_size
                 #print video_frame
                 self.buffer[self.buffer_index] = video_frame
-
+                self.new_frame = True
+
             # put the thread back to sleep for fps
             time.sleep(1.0 / self.fps)
 

examples/demoMamboVision.py

@@ -9,19 +9,18 @@
 
 isAlive = False
 
-def save_pictures(vision):
-    index = 0
+class UserVision:
+    def __init__(self, vision):
+        self.index = 0
+        self.vision = vision
 
-    while (isAlive):
-        # grab the latest image
-        img = vision.get_latest_valid_picture()
+    def save_pictures(self, args):
+        img = self.vision.get_latest_valid_picture()
 
-        filename = "test_image_%06d.png" % index
+        filename = "test_image_%06d.png" % self.index
         cv2.imwrite(filename, img)
-        index +=1
+        self.index +=1
 
-        # put the thread back to sleep for fps
-        time.sleep(1.0 / 30.0)
 
 
 # you will need to change this to the address of YOUR mambo
@@ -43,16 +42,14 @@ def save_pictures(vision):
 
     print("Preparing to open vision")
     mamboVision = MamboVision(buffer_size=10)
+    userVision = UserVision(mamboVision)
+    mamboVision.set_user_callback_function(userVision.save_pictures, user_callback_args=None)
     success = mamboVision.open_video(max_retries=15)
+
     if (success):
         print("Vision successfully started!")
         mamboVision.start_video_buffering()
 
-        picture_thread = threading.Thread(target=save_pictures, args=(mamboVision, ))
-        isAlive = True
-        picture_thread.start()
-
-
     print("taking off!")
     mambo.safe_takeoff(5)