Fawkes API  Fawkes Development Version
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 #include <core/threading/mutex_locker.h>
29 #include <core/threading/mutex.h>
30 #include <core/threading/wait_condition.h>
31 #ifdef FVBASE_TIMETRACKER
32 #include <utils/time/clock.h>
33 #include <utils/time/tracker.h>
34 #endif
35 #include <logging/logger.h>
36 
37 #include <fvcams/shmem.h>
38 #include <fvutils/color/conversions.h>
39 #include <interfaces/SwitchInterface.h>
40 
41 #ifndef _GNU_SOURCE
42 #define _GNU_SOURCE
43 #endif
44 #include <cstdio>
45 #include <cstdlib>
46 #include <string>
47 #include <algorithm>
48 
49 using namespace fawkes;
50 using namespace firevision;
51 
52 /** @class FvAcquisitionThread "acquisition_thread.h"
53  * FireVision base application acquisition thread.
54  * This thread is used by the base application to acquire images from a camera
55  * and call dependant threads when new images are available so that these
56  * threads can start processing the images.
57  * @author Tim Niemueller
58  */
59 
60 /** Constructor.
61  * @param logger logger
62  * @param id id to be used for the shared memory segment and to announce changes
63  * to the base thread
64  * @param camera camera to manage
65  * @param clock clock to use for timeout measurement (system time)
66  */
68  Logger *logger, Clock *clock)
69  : Thread("FvAcquisitionThread"),
70  BlackBoardInterfaceListener("FvAcquisitionThread::%s", id)
71 {
73  set_name("FvAcquisitionThread::%s", id);
74 
75  __image_id = strdup(id);
76 
78  raw_subscriber_thread = NULL;
79 
80  __enabled_mutex = new Mutex(Mutex::RECURSIVE);
81  __enabled_waitcond = new WaitCondition(__enabled_mutex);
82 
83  __camera = camera;
84  __width = __camera->pixel_width();
85  __height = __camera->pixel_height();
86  __colorspace = __camera->colorspace();
87 
88  __mode = AqtContinuous;
89  __enabled = false;
90 
91 #ifdef FVBASE_TIMETRACKER
92  __tt = new TimeTracker();
93  __loop_count = 0;
94  __ttc_capture = __tt->add_class("Capture");
95  __ttc_lock = __tt->add_class("Lock");
96  __ttc_convert = __tt->add_class("Convert");
97  __ttc_unlock = __tt->add_class("Unlock");
98  __ttc_dispose = __tt->add_class("Dispose");
99 #endif
100 }
101 
102 
103 /** Destructor. */
105 {
106  __camera->close();
107 
108  for (__shmit = __shm.begin(); __shmit != __shm.end(); ++__shmit) {
109  delete __shmit->second;
110  }
111  __shm.clear();
112 
113  delete vision_threads;
114  delete __camera;
115  free(__image_id);
116  delete __enabled_waitcond;
117  delete __enabled_mutex;
118 }
119 
120 void
122 {
123  logger->log_debug(name(), "Camera opened, w=%u h=%u c=%s", __width, __height,
124  colorspace_to_string(__colorspace));
125 
126  std::string if_id = std::string("Camera ") + __image_id;
127  __enabled_if = blackboard->open_for_writing<SwitchInterface>(if_id.c_str());
128  __enabled_if->set_enabled(__enabled);
129  __enabled_if->write();
130 
131  bbil_add_message_interface(__enabled_if);
132  blackboard->register_listener(this, BlackBoard::BBIL_FLAG_MESSAGES);
133 }
134 
135 
136 void
138 {
140  blackboard->close(__enabled_if);
141 }
142 
143 
144 /** Get a camera instance.
145  * This will return a camera instance suitable for accessing the image
146  * buffer. Note, that this is not the camera provided to the constructor,
147  * but rather a SharedMemoryCamera instance accessing a shared memory buffer
148  * where the image is copied to (or a conversion result is posted to).
149  * The returned instance has to bee freed using delete when done with it.
150  *
151  * You can decide whether you want to get access to the raw camera image
152  * that has not been modified in any way or to the YUV422_PLANAR image buffer
153  * (a conversion is done if needed). Use the raw parameter to decide whether
154  * to get the raw image (true) or the YUV422_PLANAR image (false).
155  *
156  * When a thread is added it is internally put into a waiting queue. Since
157  * at the time when it is added the thread is not yet started, and its
158  * initialization may even fail. For this reason the acquisition thread
159  * registers itself to receive status notifications of the thread. If the
160  * thread signals successful startup it is moved to the running queue and
161  * from then on woken up when new image material can be processed. If the
162  * thread fails for whatever reason it is dropped.
163  *
164  * The acquisition thread has a timeout. If no thread is in the running or
165  * waiting queue for this number of seconds, the base thread is signalled
166  * to shut down this acquisition thread (which the base thread may do or
167  * deny). This is done so that if a plugin is just unloaded shortly and
168  * then quickly loaded again the overhead of closing the camera and then
169  * opening it again is avoided.
170  *
171  * @param cspace the desired colorspace the image should be converted to.
172  * See general notes in VisionMaster::register_for_camera().
173  * @param deep_copy given to the shared memory camera.
174  * @return camera instance
175  * @see SharedMemoryCamera
176  */
177 Camera *
178 FvAcquisitionThread::camera_instance(colorspace_t cspace, bool deep_copy)
179 {
180  const char *img_id = NULL;
181 
182  if (cspace == CS_UNKNOWN) {
183  if (raw_subscriber_thread) {
184  // There may be only one
185  throw Exception("Only one vision thread may access the raw camera.");
186  } else {
187  return __camera;
188  }
189  } else {
190  char *tmp = NULL;
191  if (__shm.find(cspace) == __shm.end()) {
192  if ( asprintf(&tmp, "%s.%zu", __image_id, __shm.size()) == -1) {
193  throw OutOfMemoryException("FvAcqThread::camera_instance(): Could not create image ID");
194  }
195  img_id = tmp;
196  __shm[cspace] = new SharedMemoryImageBuffer(img_id, cspace, __width, __height);
197  } else {
198  img_id = __shm[cspace]->image_id();
199  }
200 
201  SharedMemoryCamera *c = new SharedMemoryCamera(img_id, deep_copy);
202 
203  if (tmp) free(tmp);
204  return c;
205  }
206 }
207 
208 
209 /** Get the Camera of this acquisition thread.
210  * This is just used for the camera controls, if you want to access the camera,
211  * use camera_instance()
212  * @return a pointer to the Camera
213  */
214 Camera *
216 {
217  return __camera;
218 }
219 
220 
221 /** Set acquisition thread mode.
222  * Note that this may only be called on a stopped thread or an
223  * exception will be thrown by Thread::set_opmode()!
224  * @param mode new acquisition thread mode
225  */
226 void
228 {
229  if ( mode == AqtCyclic ) {
230  //logger->log_info(name(), "Setting WAITFORWAKEUPMODE");
231  set_opmode(Thread::OPMODE_WAITFORWAKEUP);
232  } else if ( mode == AqtContinuous ) {
233  //logger->log_info(name(), "Setting CONTINUOUS");
234  set_opmode(Thread::OPMODE_CONTINUOUS);
235  }
236  __mode = mode;
237 }
238 
239 
240 /** Enable or disable image retrieval.
241  * When the acquisition thread is enabled image data will be converted or copied
242  * to the shared memory buffer, otherwise only the capture/dispose cycle is
243  * executed.
244  * @param enabled true to enable acquisition thread, false to disable
245  */
246 void
248 {
249  MutexLocker lock(__enabled_mutex);
250 
251  if (__enabled && ! enabled) {
252  // disabling thread
253  __camera->stop();
254  __enabled_if->set_enabled(false);
255  __enabled_if->write();
256 
257  } else if (! __enabled && enabled) {
258  // enabling thread
259  __camera->start();
260  __enabled_if->set_enabled(true);
261  __enabled_if->write();
262 
263  __enabled_waitcond->wake_all();
264  } // else not state change
265 
266  // we can safely do this every time...
267  __enabled = enabled;
268 }
269 
270 
271 /** Get acquisition thread mode.
272  * @return acquisition thread mode.
273  */
276 {
277  return __mode;
278 }
279 
280 
281 /** Set prepfin hold status for vision threads.
282  * @param hold prepfin hold status
283  * @see Thread::set_prepfin_hold()
284  */
285 void
287 {
288  try {
290  } catch (Exception &e) {
291  logger->log_warn(name(), "At least one thread was being finalized while prepfin hold "
292  "was about to be acquired");
293  throw;
294  }
295 }
296 
297 
298 void
300 {
301  MutexLocker lock(__enabled_mutex);
302 
303  while (! __enabled_if->msgq_empty() ) {
305  // must be re-established
306  logger->log_info(name(), "Enabling on blackboard request");
307  set_enabled(true);
308  } else if (__enabled_if->msgq_first_is<SwitchInterface::DisableSwitchMessage>()) {
309  logger->log_info(name(), "Disabling on blackboard request");
310  set_enabled(false);
311  } else {
312  logger->log_warn(name(), "Unhandled message %s ignored",
313  __enabled_if->msgq_first()->type());
314  }
315  __enabled_if->msgq_pop();
316  }
317 
318  // We disable cancelling here to avoid problems with the write lock
319  Thread::CancelState old_cancel_state;
320  set_cancel_state(Thread::CANCEL_DISABLED, &old_cancel_state);
321 
322 #ifdef FVBASE_TIMETRACKER
323  try {
324  if ( __enabled ) {
325  __tt->ping_start(__ttc_capture);
326  __camera->capture();
327  __tt->ping_end(__ttc_capture);
328 
329  for (__shmit = __shm.begin(); __shmit != __shm.end(); ++__shmit) {
330  if (__shmit->first == CS_UNKNOWN) continue;
331  __tt->ping_start(__ttc_lock);
332  __shmit->second->lock_for_write();
333  __tt->ping_end(__ttc_lock);
334  __tt->ping_start(__ttc_convert);
335  convert(__colorspace, __shmit->first,
336  __camera->buffer(), __shmit->second->buffer(),
337  __width, __height);
338  try {
339  __shmit->second->set_capture_time(__camera->capture_time());
340  } catch (NotImplementedException &e) {
341  // ignored
342  }
343  __tt->ping_end(__ttc_convert);
344  __tt->ping_start(__ttc_unlock);
345  __shmit->second->unlock();
346  __tt->ping_end(__ttc_unlock);
347  }
348  }
349  } catch (Exception &e) {
350  logger->log_error(name(), "Cannot convert image data");
351  logger->log_error(name(), e);
352  }
353  if ( __enabled ) {
354  __tt->ping_start(__ttc_dispose);
355  __camera->dispose_buffer();
356  __tt->ping_end(__ttc_dispose);
357  }
358 
359  if ( (++__loop_count % FVBASE_TT_PRINT_INT) == 0 ) {
360  __tt->print_to_stdout();
361  }
362 
363 #else // no time tracking
364  try {
365  if ( __enabled ) {
366  __camera->capture();
367  for (__shmit = __shm.begin(); __shmit != __shm.end(); ++__shmit) {
368  if (__shmit->first == CS_UNKNOWN) continue;
369  __shmit->second->lock_for_write();
370  convert(__colorspace, __shmit->first,
371  __camera->buffer(), __shmit->second->buffer(),
372  __width, __height);
373  try {
374  __shmit->second->set_capture_time(__camera->capture_time());
375  } catch (NotImplementedException &e) {
376  // ignored
377  }
378  __shmit->second->unlock();
379  }
380  }
381  } catch (Exception &e) {
382  logger->log_error(name(), e);
383  }
384  if ( __enabled ) {
385  __camera->dispose_buffer();
386  }
387 #endif
388 
389  if ( __mode == AqtCyclic && __enabled ) {
391  }
392 
393  // reset to the original cancel state, cancelling is now safe
394  set_cancel_state(old_cancel_state);
395 
396  // in continuous mode wait for signal if disabled
397  while ( __mode == AqtContinuous && ! __enabled ) {
398  __enabled_waitcond->wait();
399  }
400 }
401 
402 bool
403 FvAcquisitionThread::bb_interface_message_received(Interface *interface,
404  Message *message) throw()
405 {
406  // in continuous mode wait for signal if disabled
407  MutexLocker lock(__enabled_mutex);
408  if (__mode == AqtContinuous && ! __enabled) {
410  logger->log_info(name(), "Enabling on blackboard request");
411  set_enabled(true);
412  return false;
413  }
414  }
415 
416  return true;
417 }
void set_aqtmode(AqtMode mode)
Set acquisition thread mode.
void wakeup_and_wait_cyclic_threads()
Wakeup and wait for all cyclic threads.
Wait until a given condition holds.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:44
AqtMode
Acquisition thread mode.
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1048
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Camera interface for image aquiring devices in FireVision.
Definition: camera.h:35
virtual void stop()=0
Stop image transfer from the camera.
Fawkes library namespace.
fawkes::Thread * raw_subscriber_thread
Vision thread registered for raw camera access on this camera.
Called method has not been implemented.
Definition: software.h:107
void wake_all()
Wake up all waiting threads.
Mutex locking helper.
Definition: mutex_locker.h:33
This is supposed to be the central clock in Fawkes.
Definition: clock.h:34
virtual unsigned int pixel_width()=0
Width of image in pixels.
FvAqtVisionThreads * vision_threads
Vision threads assigned to this acquisition thread.
Aquisition-dependant threads.
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
Definition: blackboard.cpp:218
Thread class encapsulation of pthreads.
Definition: thread.h:42
void set_prepfin_conc_loop(bool concurrent=true)
Set concurrent execution of prepare_finalize() and loop().
Definition: thread.cpp:727
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:500
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:79
void set_prepfin_hold(bool hold)
Set prepfin hold fo cyclic threads.
virtual colorspace_t colorspace()=0
Colorspace of returned image.
static void set_cancel_state(CancelState new_state, CancelState *old_state=0)
Set the cancel state of the current thread.
Definition: thread.cpp:1350
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
virtual ~FvAcquisitionThread()
Destructor.
virtual void finalize()
Finalize the thread.
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
Definition: blackboard.cpp:190
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1193
SwitchInterface Fawkes BlackBoard Interface.
void set_name(const char *format,...)
Set name of thread.
Definition: thread.cpp:761
Base class for exceptions in Fawkes.
Definition: exception.h:36
Message * msgq_first()
Get the first message from the message queue.
Definition: interface.cpp:1180
virtual void capture()=0
Capture an image.
virtual fawkes::Time * capture_time()
Get the Time of the last successfully captured image.
Definition: camera.cpp:141
void set_enabled(const bool new_enabled)
Set enabled value.
virtual void init()
Initialize the thread.
void set_opmode(OpMode op_mode)
Set operation mode.
Definition: thread.cpp:690
Shared memory image buffer.
Definition: shm_image.h:181
Time tracking utility.
Definition: tracker.h:38
Shared memory camera.
Definition: shmem.h:38
firevision::Camera * get_camera()
Get the Camera of this acquisition thread.
DisableSwitchMessage Fawkes BlackBoard Interface Message.
const char * name() const
Get name of thread.
Definition: thread.h:95
void set_vt_prepfin_hold(bool hold)
Set prepfin hold status for vision threads.
bool msgq_first_is()
Check if first message has desired type.
Definition: interface.h:314
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
firevision::Camera * camera_instance(firevision::colorspace_t cspace, bool deep_copy)
Get a camera instance.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
virtual void loop()
Code to execute in the thread.
bool is_of_type()
Check if message has desired type.
Definition: message.h:138
void wait()
Wait for the condition forever.
cyclic mode, use if there is at least one cyclic thread for this acquisition thread.
EnableSwitchMessage Fawkes BlackBoard Interface Message.
void set_enabled(bool enabled)
Enable or disable image retrieval.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void close()=0
Close camera.
virtual unsigned char * buffer()=0
Get access to current image buffer.
FvAcquisitionThread(const char *id, firevision::Camera *camera, fawkes::Logger *logger, fawkes::Clock *clock)
Constructor.
virtual unsigned int pixel_height()=0
Height of image in pixels.
virtual void start()=0
Start image transfer from the camera.
Mutex mutual exclusion lock.
Definition: mutex.h:32
void bbil_add_message_interface(Interface *interface)
Add an interface to the message received watch list.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
const char * type() const
Get message type.
Definition: message.cpp:378
continuous mode, use if there are only continuous threads for this acquisition thread.
System ran out of memory and desired operation could not be fulfilled.
Definition: system.h:32
BlackBoard interface listener.
AqtMode aqtmode()
Get acquisition thread mode.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
virtual void dispose_buffer()=0
Dispose current buffer.
CancelState
Cancel state.
Definition: thread.h:60
virtual void close(Interface *interface)=0
Close interface.
Interface for logging.
Definition: logger.h:34