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 <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 }