Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
acquisition_thread.cpp
1 
2 /***************************************************************************
3  * acquisition_thread.h - FireVision Acquisition Thread
4  *
5  * Created: Wed Jun 06 19:01:10 2007
6  * Copyright 2006-2009 Tim Niemueller [www.niemueller.de]
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "acquisition_thread.h"
24 #include "aqt_vision_threads.h"
25 
26 #include <core/exceptions/system.h>
27 #include <core/exceptions/software.h>
28 #ifdef FVBASE_TIMETRACKER
29 #include <utils/time/clock.h>
30 #include <utils/time/tracker.h>
31 #endif
32 #include <logging/logger.h>
33 
34 #include <fvcams/shmem.h>
35 #include <fvutils/color/conversions.h>
36 
37 #ifndef _GNU_SOURCE
38 #define _GNU_SOURCE
39 #endif
40 #include <cstdio>
41 #include <string>
42 #include <algorithm>
43 
44 using namespace fawkes;
45 using namespace firevision;
46 
47 /** @class FvAcquisitionThread "acquisition_thread.h"
48  * FireVision base application acquisition thread.
49  * This thread is used by the base application to acquire images from a camera
50  * and call dependant threads when new images are available so that these
51  * threads can start processing the images.
52  * @author Tim Niemueller
53  */
54 
55 /** Constructor.
56  * @param logger logger
57  * @param id id to be used for the shared memory segment and to announce changes
58  * to the base thread
59  * @param camera camera to manage
60  * @param clock clock to use for timeout measurement (system time)
61  */
63  Logger *logger, Clock *clock)
64  : Thread((std::string("FvAcquisitionThread::") + id).c_str())
65 {
66  __logger = logger;
67  __image_id = strdup(id);
68 
70  raw_subscriber_thread = NULL;
71 
72  __camera = camera;
73  __width = __camera->pixel_width();
74  __height = __camera->pixel_height();
75  __colorspace = __camera->colorspace();
76  logger->log_debug(name(), "Camera opened, w=%u h=%u c=%s", __width, __height,
77  colorspace_to_string(__colorspace));
78 
79  __mode = AqtContinuous;
80  __enabled = false;
81 
82 #ifdef FVBASE_TIMETRACKER
83  __tt = new TimeTracker();
84  __loop_count = 0;
85  __ttc_capture = __tt->add_class("Capture");
86  __ttc_lock = __tt->add_class("Lock");
87  __ttc_convert = __tt->add_class("Convert");
88  __ttc_unlock = __tt->add_class("Unlock");
89  __ttc_dispose = __tt->add_class("Dispose");
90 #endif
91 }
92 
93 
94 /** Destructor. */
96 {
97  __camera->close();
98 
99  for (__shmit = __shm.begin(); __shmit != __shm.end(); ++__shmit) {
100  delete __shmit->second;
101  }
102  __shm.clear();
103 
104  delete vision_threads;
105  delete __camera;
106  free(__image_id);
107 }
108 
109 
110 /** Get a camera instance.
111  * This will return a camera instance suitable for accessing the image
112  * buffer. Note, that this is not the camera provided to the constructor,
113  * but rather a SharedMemoryCamera instance accessing a shared memory buffer
114  * where the image is copied to (or a conversion result is posted to).
115  * The returned instance has to bee freed using delete when done with it.
116  *
117  * You can decide whether you want to get access to the raw camera image
118  * that has not been modified in any way or to the YUV422_PLANAR image buffer
119  * (a conversion is done if needed). Use the raw parameter to decide whether
120  * to get the raw image (true) or the YUV422_PLANAR image (false).
121  *
122  * When a thread is added it is internally put into a waiting queue. Since
123  * at the time when it is added the thread is not yet started, and its
124  * initialization may even fail. For this reason the acquisition thread
125  * registers itself to receive status notifications of the thread. If the
126  * thread signals successful startup it is moved to the running queue and
127  * from then on woken up when new image material can be processed. If the
128  * thread fails for whatever reason it is dropped.
129  *
130  * The acquisition thread has a timeout. If no thread is in the running or
131  * waiting queue for this number of seconds, the base thread is signalled
132  * to shut down this acquisition thread (which the base thread may do or
133  * deny). This is done so that if a plugin is just unloaded shortly and
134  * then quickly loaded again the overhead of closing the camera and then
135  * opening it again is avoided.
136  *
137  * @param cspace the desired colorspace the image should be converted to.
138  * See general notes in VisionMaster::register_for_camera().
139  * @param deep_copy given to the shared memory camera.
140  * @return camera instance
141  * @see SharedMemoryCamera
142  */
143 Camera *
144 FvAcquisitionThread::camera_instance(colorspace_t cspace, bool deep_copy)
145 {
146  const char *img_id = NULL;
147 
148  if (cspace == CS_UNKNOWN) {
149  if (raw_subscriber_thread) {
150  // There may be only one
151  throw Exception("Only one vision thread may access the raw camera.");
152  } else {
153  return __camera;
154  }
155  } else {
156  char *tmp = NULL;
157  if (__shm.find(cspace) == __shm.end()) {
158  if ( asprintf(&tmp, "%s.%zu", __image_id, __shm.size()) == -1) {
159  throw OutOfMemoryException("FvAcqThread::camera_instance(): Could not create image ID");
160  }
161  img_id = tmp;
162  __shm[cspace] = new SharedMemoryImageBuffer(img_id, cspace, __width, __height);
163  } else {
164  img_id = __shm[cspace]->image_id();
165  }
166 
167  SharedMemoryCamera *c = new SharedMemoryCamera(img_id, deep_copy);
168 
169  if (tmp) free(tmp);
170  return c;
171  }
172 }
173 
174 
175 /** Get the Camera of this acquisition thread.
176  * This is just used for the camera controls, if you want to access the camera,
177  * use camera_instance()
178  * @return a pointer to the Camera
179  */
180 Camera *
182 {
183  return __camera;
184 }
185 
186 
187 /** Set acquisition thread mode.
188  * Note that this may only be called on a stopped thread or an
189  * exception will be thrown by Thread::set_opmode()!
190  * @param mode new acquisition thread mode
191  */
192 void
194 {
195  if ( mode == AqtCyclic ) {
196  //__logger->log_info(name(), "Setting WAITFORWAKEUPMODE");
197  set_opmode(Thread::OPMODE_WAITFORWAKEUP);
198  } else if ( mode == AqtContinuous ) {
199  //__logger->log_info(name(), "Setting CONTINUOUS");
200  set_opmode(Thread::OPMODE_CONTINUOUS);
201  }
202  __mode = mode;
203 }
204 
205 
206 /** Enable or disable image retrieval.
207  * When the acquisition thread is enabled image data will be converted or copied
208  * to the shared memory buffer, otherwise only the capture/dispose cycle is
209  * executed.
210  * @param enabled true to enable acquisition thread, false to disable
211  */
212 void
214 {
215  __enabled = enabled;
216 }
217 
218 
219 /** Get acquisition thread mode.
220  * @return acquisition thread mode.
221  */
224 {
225  return __mode;
226 }
227 
228 
229 /** Set prepfin hold status for vision threads.
230  * @param hold prepfin hold status
231  * @see Thread::set_prepfin_hold()
232  */
233 void
235 {
236  try {
238  } catch (Exception &e) {
239  __logger->log_warn(name(), "At least one thread was being finalized while prepfin hold "
240  "was about to be acquired");
241  throw;
242  }
243 }
244 
245 
246 void
248 {
249  // We disable cancelling here to avoid problems with the write lock
250  Thread::CancelState old_cancel_state;
251  set_cancel_state(Thread::CANCEL_DISABLED, &old_cancel_state);
252 
253 #ifdef FVBASE_TIMETRACKER
254  try {
255  __tt->ping_start(__ttc_capture);
256  __camera->capture();
257  __tt->ping_end(__ttc_capture);
258 
259  if ( __enabled ) {
260  for (__shmit = __shm.begin(); __shmit != __shm.end(); ++__shmit) {
261  if (__shmit->first == CS_UNKNOWN) continue;
262  __tt->ping_start(__ttc_lock);
263  __shmit->second->lock_for_write();
264  __tt->ping_end(__ttc_lock);
265  __tt->ping_start(__ttc_convert);
266  convert(__colorspace, __shmit->first,
267  __camera->buffer(), __shmit->second->buffer(),
268  __width, __height);
269  try {
270  __shmit->second->set_capture_time(__camera->capture_time());
271  } catch (NotImplementedException &e) {
272  // ignored
273  }
274  __tt->ping_end(__ttc_convert);
275  __tt->ping_start(__ttc_unlock);
276  __shmit->second->unlock();
277  __tt->ping_end(__ttc_unlock);
278  }
279  }
280  } catch (Exception &e) {
281  __logger->log_error(name(), "Cannot convert image data");
282  __logger->log_error(name(), e);
283  }
284  __tt->ping_start(__ttc_dispose);
285  __camera->dispose_buffer();
286  __tt->ping_end(__ttc_dispose);
287 
288  if ( (++__loop_count % FVBASE_TT_PRINT_INT) == 0 ) {
289  __tt->print_to_stdout();
290  }
291 
292 #else // no time tracking
293  try {
294  __camera->capture();
295  if ( __enabled ) {
296  for (__shmit = __shm.begin(); __shmit != __shm.end(); ++__shmit) {
297  if (__shmit->first == CS_UNKNOWN) continue;
298  __shmit->second->lock_for_write();
299  convert(__colorspace, __shmit->first,
300  __camera->buffer(), __shmit->second->buffer(),
301  __width, __height);
302  try {
303  __shmit->second->set_capture_time(__camera->capture_time());
304  } catch (NotImplementedException &e) {
305  // ignored
306  }
307  __shmit->second->unlock();
308  }
309  }
310  } catch (Exception &e) {
311  __logger->log_error(name(), e);
312  }
313  __camera->dispose_buffer();
314 #endif
315 
316  if ( __mode == AqtCyclic ) {
318  }
319 
320  // reset to the original cancel state, cancelling is now safe
321  set_cancel_state(old_cancel_state);
322 }