Rose-Hulman Robotics Team

Changeset 665

Show
Ignore:
Timestamp:
02/11/10 01:33:02 (7 months ago)
Author:
mosttw
Message:

Fixed issues with GStreamer camera driver

Location:
trunk/software
Files:
3 modified

Legend:

Unmodified
Added
Removed
  • trunk/software/Makefile

    r622 r665  
    2121 
    2222opencv: 
    23         (cd lib/opencv/; cmake ./ && make -j3 ) 
     23        (cd lib/opencv/; cmake ./ -DWITH_GTK=0 -DWITH_GSTREAMER=0 && make -j3 ) 
    2424        echo "Now run cd libs/opencv/; sudo make install" 
    2525 
  • trunk/software/rb/vision/_camera.c

    r664 r665  
    11// rb.vision._camera -- Python interface to OpenCV camera functions 
    2 // Copyright (C) 2009 Andy Spencer, Tom Most 
     2// Copyright (C) 2009 Andy Spencer 
     3// Copyright (C) 2009-2010 Thomas W. Most 
    34//  
    45// This program is free software: you can redistribute it and/or modify 
     
    147148 
    148149 
    149  
    150150////// Camera OBJECT ////// 
    151151 
     
    230230        {"get_frame", (PyCFunction) Camera_get_frame, METH_NOARGS, 
    231231                "Capture a frame from the camera.  Returns an `IplImage`."}, 
    232         {NULL}  /* Sentinel */ 
     232        {NULL}  // Sentinel 
    233233}; 
    234234 
    235235static PyTypeObject CameraType = { 
    236236        PyObject_HEAD_INIT(NULL) 
    237         0,                                        /*ob_size*/ 
    238     "rb.vision._camera.Camera",               /*tp_name*/ 
    239     sizeof(Camera),                           /*tp_basicsize*/ 
    240     0,                                        /*tp_itemsize*/ 
    241     (destructor) Camera_dealloc,              /*tp_dealloc*/ 
    242     0,                                        /*tp_print*/ 
    243     0,                                        /*tp_getattr*/ 
    244     0,                                        /*tp_setattr*/ 
    245     0,                                        /*tp_compare*/ 
    246     0,                                        /*tp_repr*/ 
    247     0,                                        /*tp_as_number*/ 
    248     0,                                        /*tp_as_sequence*/ 
    249     0,                                        /*tp_as_mapping*/ 
    250     0,                                        /*tp_hash */ 
    251     0,                                        /*tp_call*/ 
    252     0,                                        /*tp_str*/ 
    253     0,                                        /*tp_getattro*/ 
    254     0,                                        /*tp_setattro*/ 
    255     0,                                        /*tp_as_buffer*/ 
    256     Py_TPFLAGS_DEFAULT | Py_TPFLAGS_BASETYPE, /*tp_flags*/ 
     237        0,                                        // ob_size 
     238    "rb.vision._camera.Camera",               // tp_name 
     239    sizeof(Camera),                           // tp_basicsize 
     240    0,                                        // tp_itemsize 
     241    (destructor) Camera_dealloc,              // tp_dealloc 
     242    0,                                        // tp_print 
     243    0,                                        // tp_getattr 
     244    0,                                        // tp_setattr 
     245    0,                                        // tp_compare 
     246    0,                                        // tp_repr 
     247    0,                                        // tp_as_number 
     248    0,                                        // tp_as_sequence 
     249    0,                                        // tp_as_mapping 
     250    0,                                        // tp_hash  
     251    0,                                        // tp_call 
     252    0,                                        // tp_str 
     253    0,                                        // tp_getattro 
     254    0,                                        // tp_setattro 
     255    0,                                        // tp_as_buffer 
     256    Py_TPFLAGS_DEFAULT | Py_TPFLAGS_BASETYPE, // tp_flags 
    257257    "Fetches images using OpenCV's camera capture functions.  " 
    258258    "The sole optional argument, `index`, is an integer indicating " 
    259259    "the camera to open (-1, the default, just opens some camera). " 
    260     "Throws `RuntimeError` if unable to open a camera.", /* tp_doc */ 
    261     0,                                            /* tp_traverse */ 
    262     0,                                            /* tp_clear */ 
    263     0,                                            /* tp_richcompare */ 
    264     0,                                            /* tp_weaklistoffset */ 
    265     0,                                            /* tp_iter */ 
    266     0,                                            /* tp_iternext */ 
    267     Camera_methods,                           /* tp_methods */ 
    268     0,                                        /* tp_members */ 
    269     0,                                        /* tp_getset */ 
    270     0,                                        /* tp_base */ 
    271     0,                                        /* tp_dict */ 
    272     0,                                        /* tp_descr_get */ 
    273     0,                                        /* tp_descr_set */ 
    274     0,                                        /* tp_dictoffset */ 
    275     (initproc) Camera_init,                   /* tp_init */ 
    276     0,                                        /* tp_alloc */ 
    277     Camera_new,                               /* tp_new */ 
     260    "Throws `RuntimeError` if unable to open a camera.", // tp_doc 
     261    0,                                            // tp_traverse 
     262    0,                                            // tp_clear 
     263    0,                                            // tp_richcompare 
     264    0,                                            // tp_weaklistoffset 
     265    0,                                            // tp_iter 
     266    0,                                            // tp_iternext 
     267    Camera_methods,                           // tp_methods 
     268    0,                                        // tp_members 
     269    0,                                        // tp_getset 
     270    0,                                        // tp_base 
     271    0,                                        // tp_dict 
     272    0,                                        // tp_descr_get 
     273    0,                                        // tp_descr_set 
     274    0,                                        // tp_dictoffset 
     275    (initproc) Camera_init,                   // tp_init 
     276    0,                                        // tp_alloc 
     277    Camera_new,                               // tp_new 
    278278}; 
    279279 
  • trunk/software/rb/vision/gstcam.py

    r629 r665  
    1616import time 
    1717import logging 
    18 from thread import start_new_thread 
     18from rb.vision._camera import ipl_image_from_rgb_buffer, save_image 
     19 
    1920import gst 
    2021 
    21 from rb.vision._camera import ipl_image_from_rgb_string, save_image 
    2222 
    2323 
     
    3838                        'appsink name=out max_buffers=1 drop=1' 
    3939                ) % source 
    40                 import pdb; pdb.set_trace() 
     40                logger.info('Launching pipeline %r', launch_string) 
    4141                self.pipeline = gst.parse_launch(launch_string) 
    4242                self.appsink = self.pipeline.get_by_name('out') 
     43                assert self.appsink 
    4344         
    4445                bus = self.pipeline.get_bus() 
     
    4647                bus.connect('message', self.message_cb) 
    4748                 
    48                 start_new_thread(self._main, ()) 
     49                self.pipeline.set_state(gst.STATE_PLAYING) 
    4950         
    50         def _main(self): 
    51                 ''' 
    52                 Main loop: pull images from the buffer and dispatch them. 
    53                 ''' 
    54                 self.pipeline.set_state(gst.STATE_PLAYING) 
     51        def get_last_image(self): 
     52                buffer = self.appsink.get_last_buffer() 
     53                if buffer is None: 
     54                        return None 
     55                return gst_buffer_to_ipl_image(buffer) 
    5556 
    56                 while self.running: 
    57                         # TODO: Detect EOS (end of stream) 
    58                         # TODO: Make this non-blocking 
    59                         buffer = self.appsink.emit('pull-buffer') 
    60                         if buffer is None: 
    61                                 logger.error("Pulled None buffer") 
    62                                 time.sleep(0.1) 
    63                                 continue 
    64                         struct = buffer.caps[0] 
    65                         size = (struct['width'], struct['height']) 
    66                         image = ipl_image_from_rgb_string(buffer, size) 
    67                         if self.controller: 
    68                                 self.controller.send('camera_image', (self.source, image)) 
    69                                 logger.debug("Captured image") 
    70                         else: 
    71                                 fn = "cap%0.2f.png" % time.time() 
    72                                 print "Got image; saving to", fn 
    73                                 save_image(fn, image) 
    74  
    75          
    7657        def message_cb(self, bus, message): 
    7758                if message.type == gst.MESSAGE_STATE_CHANGED: 
     
    8263                        self.pipeline.set_state(gst.STATE_NULL) # Stop playback 
    8364 
     65 
     66def gst_buffer_to_ipl_image(buffer): 
     67        struct = buffer.caps[0] 
     68        size = (struct['width'], struct['height']) 
     69        return ipl_image_from_rgb_buffer(buffer, size) 
     70 
     71 
    8472if __name__ == '__main__': 
    85         import sys, gtk 
     73        import sys, gtk, gobject 
    8674        logging.basicConfig(level=logging.DEBUG) 
    8775        try: 
     
    8977        except IndexError: 
    9078                source = 'v4l2src' 
    91         gtk.gdk.threads_init() 
    9279        cam = GstCamera(None, source) 
     80 
     81        def save_pic(): 
     82                ipl_image = cam.get_last_image() 
     83                if ipl_image is None: 
     84                        print "Got None" 
     85                else: 
     86                        fn = "cap%0.4f.png" % time.time() 
     87                        print "Got image, saving to", fn 
     88                        save_image(fn, ipl_image) 
     89                return True 
     90 
     91        gobject.timeout_add(1000, save_pic) 
    9392        gtk.main() 
    9493