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 <logging/logger.h>
00033 
00034 #include <fvcams/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   for (__shmit = __shm.begin(); __shmit != __shm.end(); ++__shmit) {
00100     delete __shmit->second;
00101   }
00102   __shm.clear();
00103 
00104   delete vision_threads;
00105   delete __camera;
00106   free(__image_id);
00107 }
00108 
00109 
00110 /** Get a camera instance.
00111  * This will return a camera instance suitable for accessing the image
00112  * buffer. Note, that this is not the camera provided to the constructor,
00113  * but rather a SharedMemoryCamera instance accessing a shared memory buffer
00114  * where the image is copied to (or a conversion result is posted to).
00115  * The returned instance has to bee freed using delete when done with it.
00116  *
00117  * You can decide whether you want to get access to the raw camera image
00118  * that has not been modified in any way or to the YUV422_PLANAR image buffer
00119  * (a conversion is done if needed). Use the raw parameter to decide whether
00120  * to get the raw image (true) or the YUV422_PLANAR image (false).
00121  *
00122  * When a thread is added it is internally put into a waiting queue. Since
00123  * at the time when it is added the thread is not yet started, and its
00124  * initialization may even fail. For this reason the acquisition thread
00125  * registers itself to receive status notifications of the thread. If the
00126  * thread signals successful startup it is moved to the running queue and
00127  * from then on woken up when new image material can be processed. If the
00128  * thread fails for whatever reason it is dropped.
00129  *
00130  * The acquisition thread has a timeout. If no thread is in the running or
00131  * waiting queue for this number of seconds, the base thread is signalled
00132  * to shut down this acquisition thread (which the base thread may do or
00133  * deny). This is done so that if a plugin is just unloaded shortly and
00134  * then quickly loaded again the overhead of closing the camera and then
00135  * opening it again is avoided.
00136  *
00137  * @param cspace the desired colorspace the image should be converted to.
00138  * See general notes in VisionMaster::register_for_camera().
00139  * @param deep_copy given to the shared memory camera.
00140  * @return camera instance
00141  * @see SharedMemoryCamera
00142  */
00143 Camera *
00144 FvAcquisitionThread::camera_instance(colorspace_t cspace, bool deep_copy)
00145 {
00146   const char *img_id = NULL;
00147 
00148   if (cspace == CS_UNKNOWN) {
00149     if (raw_subscriber_thread) {
00150       // There may be only one
00151       throw Exception("Only one vision thread may access the raw camera.");
00152     } else {
00153       return __camera;
00154     }
00155   } else {
00156     char *tmp =  NULL;
00157     if (__shm.find(cspace) == __shm.end()) {
00158       if ( asprintf(&tmp, "%s.%zu", __image_id, __shm.size()) == -1) {
00159         throw OutOfMemoryException("FvAcqThread::camera_instance(): Could not create image ID");
00160       }
00161       img_id = tmp;
00162       __shm[cspace] = new SharedMemoryImageBuffer(img_id, cspace, __width, __height);
00163     } else {
00164       img_id = __shm[cspace]->image_id();
00165     }
00166 
00167     SharedMemoryCamera *c = new SharedMemoryCamera(img_id, deep_copy);
00168 
00169     if (tmp)  free(tmp);
00170     return c;
00171   }
00172 }
00173 
00174 
00175 /** Get the Camera of this acquisition thread.
00176  * This is just used for the camera controls, if you want to access the camera,
00177  * use camera_instance()
00178  * @return a pointer to the Camera
00179  */
00180 Camera *
00181 FvAcquisitionThread::get_camera()
00182 {
00183   return __camera;
00184 }
00185 
00186 
00187 /** Set acquisition thread mode.
00188  * Note that this may only be called on a stopped thread or an
00189  * exception will be thrown by Thread::set_opmode()!
00190  * @param mode new acquisition thread mode
00191  */
00192 void
00193 FvAcquisitionThread::set_aqtmode(AqtMode mode)
00194 {
00195   if ( mode == AqtCyclic ) {
00196     //__logger->log_info(name(), "Setting WAITFORWAKEUPMODE");
00197     set_opmode(Thread::OPMODE_WAITFORWAKEUP);
00198   } else if ( mode == AqtContinuous ) {
00199     //__logger->log_info(name(), "Setting CONTINUOUS");
00200     set_opmode(Thread::OPMODE_CONTINUOUS);
00201   }
00202   __mode = mode;
00203 }
00204 
00205 
00206 /** Enable or disable image retrieval.
00207  * When the acquisition thread is enabled image data will be converted or copied
00208  * to the shared memory buffer, otherwise only the capture/dispose cycle is
00209  * executed.
00210  * @param enabled true to enable acquisition thread, false to disable
00211  */
00212 void
00213 FvAcquisitionThread::set_enabled(bool enabled)
00214 {
00215   __enabled = enabled;
00216 }
00217 
00218 
00219 /** Get acquisition thread mode.
00220  * @return acquisition thread mode.
00221  */
00222 FvAcquisitionThread::AqtMode
00223 FvAcquisitionThread::aqtmode()
00224 {
00225   return __mode;
00226 }
00227 
00228 
00229 /** Set prepfin hold status for vision threads.
00230  * @param hold prepfin hold status
00231  * @see Thread::set_prepfin_hold()
00232  */
00233 void
00234 FvAcquisitionThread::set_vt_prepfin_hold(bool hold)
00235 {
00236   try {
00237     vision_threads->set_prepfin_hold(hold);
00238   } catch (Exception &e) {
00239     __logger->log_warn(name(), "At least one thread was being finalized while prepfin hold "
00240                       "was about to be acquired");
00241     throw;
00242   }
00243 }
00244 
00245 
00246 void
00247 FvAcquisitionThread::loop()
00248 {
00249   // We disable cancelling here to avoid problems with the write lock
00250   Thread::CancelState old_cancel_state;
00251   set_cancel_state(Thread::CANCEL_DISABLED, &old_cancel_state);
00252 
00253 #ifdef FVBASE_TIMETRACKER
00254   try {
00255     __tt->ping_start(__ttc_capture);
00256     __camera->capture();
00257     __tt->ping_end(__ttc_capture);
00258 
00259     if ( __enabled ) {
00260       for (__shmit = __shm.begin(); __shmit != __shm.end(); ++__shmit) {
00261         if (__shmit->first == CS_UNKNOWN)  continue;
00262         __tt->ping_start(__ttc_lock);
00263         __shmit->second->lock_for_write();
00264         __tt->ping_end(__ttc_lock);
00265         __tt->ping_start(__ttc_convert);
00266         convert(__colorspace, __shmit->first,
00267                 __camera->buffer(), __shmit->second->buffer(),
00268                 __width, __height);
00269         try {
00270           __shmit->second->set_capture_time(__camera->capture_time());
00271         } catch (NotImplementedException &e) {
00272           // ignored
00273         }
00274         __tt->ping_end(__ttc_convert);
00275         __tt->ping_start(__ttc_unlock);
00276         __shmit->second->unlock();
00277         __tt->ping_end(__ttc_unlock);
00278       }
00279     }
00280   } catch (Exception &e) {
00281     __logger->log_error(name(), "Cannot convert image data");
00282     __logger->log_error(name(), e);
00283   }
00284   __tt->ping_start(__ttc_dispose);
00285   __camera->dispose_buffer();
00286   __tt->ping_end(__ttc_dispose);
00287 
00288   if ( (++__loop_count % FVBASE_TT_PRINT_INT) == 0 ) {
00289     __tt->print_to_stdout();
00290   }
00291 
00292 #else // no time tracking
00293   try {
00294     __camera->capture();
00295     if ( __enabled ) {
00296       for (__shmit = __shm.begin(); __shmit != __shm.end(); ++__shmit) {
00297         if (__shmit->first == CS_UNKNOWN)  continue;
00298         __shmit->second->lock_for_write();
00299         convert(__colorspace, __shmit->first,
00300                 __camera->buffer(), __shmit->second->buffer(),
00301                 __width, __height);
00302         try {
00303           __shmit->second->set_capture_time(__camera->capture_time());
00304         } catch (NotImplementedException &e) {
00305           // ignored
00306         }
00307         __shmit->second->unlock();
00308       }
00309     }
00310   } catch (Exception &e) {
00311     __logger->log_error(name(), e);
00312   }
00313   __camera->dispose_buffer();
00314 #endif
00315 
00316   if ( __mode == AqtCyclic ) {
00317     vision_threads->wakeup_and_wait_cyclic_threads();
00318   }
00319 
00320   // reset to the original cancel state, cancelling is now safe
00321   set_cancel_state(old_cancel_state);
00322 }