Fawkes API
Fawkes Development Version
|
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 }