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