Fawkes API  Fawkes Development Version
acquisition_thread.cpp
00001 
00002 /***************************************************************************
00003  *  acquisition_thread.h - FireVision Acquisition Thread
00004  *
00005  *  Created: Wed Jun 06 19:01:10 2007
00006  *  Copyright  2006-2009  Tim Niemueller [www.niemueller.de]
00007  *
00008  ****************************************************************************/
00009 
00010 /*  This program is free software; you can redistribute it and/or modify
00011  *  it under the terms of the GNU General Public License as published by
00012  *  the Free Software Foundation; either version 2 of the License, or
00013  *  (at your option) any later version.
00014  *
00015  *  This program is distributed in the hope that it will be useful,
00016  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018  *  GNU Library General Public License for more details.
00019  *
00020  *  Read the full text in the LICENSE.GPL file in the doc directory.
00021  */
00022 
00023 #include "acquisition_thread.h"
00024 #include "aqt_vision_threads.h"
00025 
00026 #include <core/exceptions/system.h>
00027 #include <core/exceptions/software.h>
00028 #ifdef FVBASE_TIMETRACKER
00029 #include <utils/time/clock.h>
00030 #include <utils/time/tracker.h>
00031 #endif
00032 #include <utils/logging/logger.h>
00033 
00034 #include <cams/shmem.h>
00035 #include <fvutils/color/conversions.h>
00036 
00037 #ifndef _GNU_SOURCE
00038 #define _GNU_SOURCE
00039 #endif
00040 #include <cstdio>
00041 #include <string>
00042 #include <algorithm>
00043 
00044 using namespace fawkes;
00045 using namespace firevision;
00046 
00047 /** @class FvAcquisitionThread "acquisition_thread.h"
00048  * FireVision base application acquisition thread.
00049  * This thread is used by the base application to acquire images from a camera
00050  * and call dependant threads when new images are available so that these
00051  * threads can start processing the images.
00052  * @author Tim Niemueller
00053  */
00054 
00055 /** Constructor.
00056  * @param logger logger
00057  * @param id id to be used for the shared memory segment and to announce changes
00058  * to the base thread
00059  * @param camera camera to manage
00060  * @param clock clock to use for timeout measurement (system time)
00061  */
00062 FvAcquisitionThread::FvAcquisitionThread(const char *id,  Camera *camera,
00063                                          Logger *logger, Clock *clock)
00064   : Thread((std::string("FvAcquisitionThread::") + id).c_str())
00065 {
00066   __logger        = logger;
00067   __image_id      = strdup(id);
00068 
00069   vision_threads  = new FvAqtVisionThreads(clock);
00070   raw_subscriber_thread = NULL;
00071 
00072   __camera        = camera;
00073   __width         = __camera->pixel_width();
00074   __height        = __camera->pixel_height();
00075   __colorspace    = __camera->colorspace();
00076   logger->log_debug(name(), "Camera opened, w=%u  h=%u  c=%s", __width, __height,
00077                     colorspace_to_string(__colorspace));
00078 
00079   __mode = AqtContinuous;
00080   __enabled = false;
00081 
00082 #ifdef FVBASE_TIMETRACKER
00083   __tt = new TimeTracker();
00084   __loop_count = 0;
00085   __ttc_capture = __tt->add_class("Capture");
00086   __ttc_lock    = __tt->add_class("Lock");
00087   __ttc_convert = __tt->add_class("Convert");
00088   __ttc_unlock  = __tt->add_class("Unlock");
00089   __ttc_dispose = __tt->add_class("Dispose");
00090 #endif
00091 }
00092 
00093 
00094 /** Destructor. */
00095 FvAcquisitionThread::~FvAcquisitionThread()
00096 {
00097   __camera->close();
00098 
00099   delete vision_threads;
00100   delete __camera;
00101   free(__image_id);
00102 }
00103 
00104 
00105 /** Get a camera instance.
00106  * This will return a camera instance suitable for accessing the image
00107  * buffer. Note, that this is not the camera provided to the constructor,
00108  * but rather a SharedMemoryCamera instance accessing a shared memory buffer
00109  * where the image is copied to (or a conversion result is posted to).
00110  * The returned instance has to bee freed using delete when done with it.
00111  *
00112  * You can decide whether you want to get access to the raw camera image
00113  * that has not been modified in any way or to the YUV422_PLANAR image buffer
00114  * (a conversion is done if needed). Use the raw parameter to decide whether
00115  * to get the raw image (true) or the YUV422_PLANAR image (false).
00116  *
00117  * When a thread is added it is internally put into a waiting queue. Since
00118  * at the time when it is added the thread is not yet started, and its
00119  * initialization may even fail. For this reason the acquisition thread
00120  * registers itself to receive status notifications of the thread. If the
00121  * thread signals successful startup it is moved to the running queue and
00122  * from then on woken up when new image material can be processed. If the
00123  * thread fails for whatever reason it is dropped.
00124  *
00125  * The acquisition thread has a timeout. If no thread is in the running or
00126  * waiting queue for this number of seconds, the base thread is signalled
00127  * to shut down this acquisition thread (which the base thread may do or
00128  * deny). This is done so that if a plugin is just unloaded shortly and
00129  * then quickly loaded again the overhead of closing the camera and then
00130  * opening it again is avoided.
00131  *
00132  * @param cspace the desired colorspace the image should be converted to.
00133  * See general notes in VisionMaster::register_for_camera().
00134  * @param deep_copy given to the shared memory camera.
00135  * @return camera instance
00136  * @see SharedMemoryCamera
00137  */
00138 Camera *
00139 FvAcquisitionThread::camera_instance(colorspace_t cspace, bool deep_copy)
00140 {
00141   const char *img_id = NULL;
00142 
00143   if (cspace == CS_UNKNOWN) {
00144     if (raw_subscriber_thread) {
00145       // There may be only one
00146       throw Exception("Only one vision thread may access the raw camera.");
00147     } else {
00148       return __camera;
00149     }
00150   } else {
00151     char *tmp =  NULL;
00152     if (__shm.find(cspace) == __shm.end()) {
00153       if ( asprintf(&tmp, "%s.%zu", __image_id, __shm.size()) == -1) {
00154         throw OutOfMemoryException("FvAcqThread::camera_instance(): Could not create image ID");
00155       }
00156       img_id = tmp;
00157       __shm[cspace] = new SharedMemoryImageBuffer(img_id, cspace, __width, __height);
00158     } else {
00159       img_id = __shm[cspace]->image_id();
00160     }
00161 
00162     SharedMemoryCamera *c = new SharedMemoryCamera(img_id, deep_copy);
00163 
00164     if (tmp)  free(tmp);
00165     return c;
00166   }
00167 }
00168 
00169 
00170 /** Get the Camera of this acquisition thread.
00171  * This is just used for the camera controls, if you want to access the camera,
00172  * use camera_instance()
00173  * @return a pointer to the Camera
00174  */
00175 Camera *
00176 FvAcquisitionThread::get_camera()
00177 {
00178   return __camera;
00179 }
00180 
00181 
00182 /** Set acquisition thread mode.
00183  * Note that this may only be called on a stopped thread or an
00184  * exception will be thrown by Thread::set_opmode()!
00185  * @param mode new acquisition thread mode
00186  */
00187 void
00188 FvAcquisitionThread::set_aqtmode(AqtMode mode)
00189 {
00190   if ( mode == AqtCyclic ) {
00191     //__logger->log_info(name(), "Setting WAITFORWAKEUPMODE");
00192     set_opmode(Thread::OPMODE_WAITFORWAKEUP);
00193   } else if ( mode == AqtContinuous ) {
00194     //__logger->log_info(name(), "Setting CONTINUOUS");
00195     set_opmode(Thread::OPMODE_CONTINUOUS);
00196   }
00197   __mode = mode;
00198 }
00199 
00200 
00201 /** Enable or disable image retrieval.
00202  * When the acquisition thread is enabled image data will be converted or copied
00203  * to the shared memory buffer, otherwise only the capture/dispose cycle is
00204  * executed.
00205  * @param enabled true to enable acquisition thread, false to disable
00206  */
00207 void
00208 FvAcquisitionThread::set_enabled(bool enabled)
00209 {
00210   __enabled = enabled;
00211 }
00212 
00213 
00214 /** Get acquisition thread mode.
00215  * @return acquisition thread mode.
00216  */
00217 FvAcquisitionThread::AqtMode
00218 FvAcquisitionThread::aqtmode()
00219 {
00220   return __mode;
00221 }
00222 
00223 
00224 /** Set prepfin hold status for vision threads.
00225  * @param hold prepfin hold status
00226  * @see Thread::set_prepfin_hold()
00227  */
00228 void
00229 FvAcquisitionThread::set_vt_prepfin_hold(bool hold)
00230 {
00231   try {
00232     vision_threads->set_prepfin_hold(hold);
00233   } catch (Exception &e) {
00234     __logger->log_warn(name(), "At least one thread was being finalized while prepfin hold "
00235                       "was about to be acquired");
00236     throw;
00237   }
00238 }
00239 
00240 
00241 void
00242 FvAcquisitionThread::loop()
00243 {
00244   // We disable cancelling here to avoid problems with the write lock
00245   Thread::CancelState old_cancel_state;
00246   set_cancel_state(Thread::CANCEL_DISABLED, &old_cancel_state);
00247 
00248 #ifdef FVBASE_TIMETRACKER
00249   try {
00250     __tt->ping_start(__ttc_capture);
00251     __camera->capture();
00252     __tt->ping_end(__ttc_capture);
00253 
00254     if ( __enabled ) {
00255       for (__shmit = __shm.begin(); __shmit != __shm.end(); ++__shmit) {
00256         if (__shmit->first == CS_UNKNOWN)  continue;
00257         __tt->ping_start(__ttc_lock);
00258         __shmit->second->lock_for_write();
00259         __tt->ping_end(__ttc_lock);
00260         __tt->ping_start(__ttc_convert);
00261         convert(__colorspace, __shmit->first,
00262                 __camera->buffer(), __shmit->second->buffer(),
00263                 __width, __height);
00264         try {
00265           __shmit->second->set_capture_time(__camera->capture_time());
00266         } catch (NotImplementedException &e) {
00267           // ignored
00268         }
00269         __tt->ping_end(__ttc_convert);
00270         __tt->ping_start(__ttc_unlock);
00271         __shmit->second->unlock();
00272         __tt->ping_end(__ttc_unlock);
00273       }
00274     }
00275   } catch (Exception &e) {
00276     __logger->log_error(name(), "Cannot convert image data");
00277     __logger->log_error(name(), e);
00278   }
00279   __tt->ping_start(__ttc_dispose);
00280   __camera->dispose_buffer();
00281   __tt->ping_end(__ttc_dispose);
00282 
00283   if ( (++__loop_count % FVBASE_TT_PRINT_INT) == 0 ) {
00284     __tt->print_to_stdout();
00285   }
00286 
00287 #else // no time tracking
00288   try {
00289     __camera->capture();
00290     if ( __enabled ) {
00291       for (__shmit = __shm.begin(); __shmit != __shm.end(); ++__shmit) {
00292         if (__shmit->first == CS_UNKNOWN)  continue;
00293         __shmit->second->lock_for_write();
00294         convert(__colorspace, __shmit->first,
00295                 __camera->buffer(), __shmit->second->buffer(),
00296                 __width, __height);
00297         try {
00298           __shmit->second->set_capture_time(__camera->capture_time());
00299         } catch (NotImplementedException &e) {
00300           // ignored
00301         }
00302         __shmit->second->unlock();
00303       }
00304     }
00305   } catch (Exception &e) {
00306     __logger->log_error(name(), e);
00307   }
00308   __camera->dispose_buffer();
00309 #endif
00310 
00311   if ( __mode == AqtCyclic ) {
00312     vision_threads->wakeup_and_wait_cyclic_threads();
00313   }
00314 
00315   // reset to the original cancel state, cancelling is now safe
00316   set_cancel_state(old_cancel_state);
00317 }