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
50using namespace fawkes;
51using 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
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
121void
123{
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
136void
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 */
176Camera *
177FvAcquisitionThread::camera_instance(colorspace_t cspace, bool deep_copy)
178{
179 const char *img_id = NULL;
180
181 if (cspace == CS_UNKNOWN) {
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 */
213Camera *
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 */
224void
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 */
243void
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 */
280void
282{
283 try {
285 } catch (Exception &e) {
287 "At least one thread was being finalized while prepfin hold "
288 "was about to be acquired");
289 throw;
290 }
291}
292
293void
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
396bool
397FvAcquisitionThread::bb_interface_message_received(Interface *interface, Message *message) noexcept
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}
firevision::Camera * get_camera()
Get the Camera of this acquisition thread.
FvAcquisitionThread(const char *id, firevision::Camera *camera, fawkes::Logger *logger, fawkes::Clock *clock)
Constructor.
virtual ~FvAcquisitionThread()
Destructor.
virtual void finalize()
Finalize the thread.
void set_aqtmode(AqtMode mode)
Set acquisition thread mode.
FvAqtVisionThreads * vision_threads
Vision threads assigned to this acquisition thread.
virtual void loop()
Code to execute in the thread.
void set_enabled(bool enabled)
Enable or disable image retrieval.
AqtMode aqtmode()
Get acquisition thread mode.
void set_vt_prepfin_hold(bool hold)
Set prepfin hold status for vision threads.
virtual void init()
Initialize the thread.
AqtMode
Acquisition thread mode.
@ AqtContinuous
continuous mode, use if there are only continuous threads for this acquisition thread.
@ AqtCyclic
cyclic mode, use if there is at least one cyclic thread for this acquisition thread.
fawkes::Thread * raw_subscriber_thread
Vision thread registered for raw camera access on this camera.
firevision::Camera * camera_instance(firevision::colorspace_t cspace, bool deep_copy)
Get a camera instance.
Aquisition-dependant threads.
void wakeup_and_wait_cyclic_threads()
Wakeup and wait for all cyclic threads.
void set_prepfin_hold(bool hold)
Set prepfin hold fo cyclic threads.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
BlackBoard interface listener.
void bbil_add_message_interface(Interface *interface)
Add an interface to the message received watch list.
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
Definition: blackboard.cpp:212
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
Definition: blackboard.cpp:185
virtual void close(Interface *interface)=0
Close interface.
This is supposed to be the central clock in Fawkes.
Definition: clock.h:35
Base class for exceptions in Fawkes.
Definition: exception.h:36
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:80
bool msgq_first_is()
Check if first message has desired type.
Definition: interface.h:351
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1215
Message * msgq_first()
Get the first message from the message queue.
Definition: interface.cpp:1200
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:501
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1062
Interface for logging.
Definition: logger.h:42
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:44
const char * type() const
Get message type.
Definition: message.cpp:381
virtual void log_info(const char *component, const char *format,...)
Log informational message.
Definition: multi.cpp:195
Mutex locking helper.
Definition: mutex_locker.h:34
Mutex mutual exclusion lock.
Definition: mutex.h:33
Called method has not been implemented.
Definition: software.h:105
System ran out of memory and desired operation could not be fulfilled.
Definition: system.h:32
DisableSwitchMessage Fawkes BlackBoard Interface Message.
EnableSwitchMessage Fawkes BlackBoard Interface Message.
SwitchInterface Fawkes BlackBoard Interface.
void set_enabled(const bool new_enabled)
Set enabled value.
Thread class encapsulation of pthreads.
Definition: thread.h:46
void set_prepfin_conc_loop(bool concurrent=true)
Set concurrent execution of prepare_finalize() and loop().
Definition: thread.cpp:716
const char * name() const
Get name of thread.
Definition: thread.h:100
CancelState
Cancel state.
Definition: thread.h:64
static void set_cancel_state(CancelState new_state, CancelState *old_state=0)
Set the cancel state of the current thread.
Definition: thread.cpp:1396
void set_opmode(OpMode op_mode)
Set operation mode.
Definition: thread.cpp:682
void set_name(const char *format,...)
Set name of thread.
Definition: thread.cpp:748
Time tracking utility.
Definition: tracker.h:37
Wait until a given condition holds.
void wait()
Wait for the condition forever.
void wake_all()
Wake up all waiting threads.
Camera interface for image aquiring devices in FireVision.
Definition: camera.h:33
virtual void stop()=0
Stop image transfer from the camera.
virtual fawkes::Time * capture_time()
Get the Time of the last successfully captured image.
Definition: camera.cpp:137
virtual void close()=0
Close camera.
virtual unsigned int pixel_height()=0
Height of image in pixels.
virtual void dispose_buffer()=0
Dispose current buffer.
virtual unsigned int pixel_width()=0
Width of image in pixels.
virtual void capture()=0
Capture an image.
virtual colorspace_t colorspace()=0
Colorspace of returned image.
virtual unsigned char * buffer()=0
Get access to current image buffer.
virtual void start()=0
Start image transfer from the camera.
Shared memory camera.
Definition: shmem.h:36
Shared memory image buffer.
Definition: shm_image.h:184
Fawkes library namespace.