00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
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
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
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
00095 FvAcquisitionThread::~FvAcquisitionThread()
00096 {
00097 __camera->close();
00098
00099 delete vision_threads;
00100 delete __camera;
00101 free(__image_id);
00102 }
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133
00134
00135
00136
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
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
00171
00172
00173
00174
00175 Camera *
00176 FvAcquisitionThread::get_camera()
00177 {
00178 return __camera;
00179 }
00180
00181
00182
00183
00184
00185
00186
00187 void
00188 FvAcquisitionThread::set_aqtmode(AqtMode mode)
00189 {
00190 if ( mode == AqtCyclic ) {
00191
00192 set_opmode(Thread::OPMODE_WAITFORWAKEUP);
00193 } else if ( mode == AqtContinuous ) {
00194
00195 set_opmode(Thread::OPMODE_CONTINUOUS);
00196 }
00197 __mode = mode;
00198 }
00199
00200
00201
00202
00203
00204
00205
00206
00207 void
00208 FvAcquisitionThread::set_enabled(bool enabled)
00209 {
00210 __enabled = enabled;
00211 }
00212
00213
00214
00215
00216
00217 FvAcquisitionThread::AqtMode
00218 FvAcquisitionThread::aqtmode()
00219 {
00220 return __mode;
00221 }
00222
00223
00224
00225
00226
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
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
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
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
00316 set_cancel_state(old_cancel_state);
00317 }