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