23 #include "base_thread.h"
24 #include "acquisition_thread.h"
25 #include "aqt_vision_threads.h"
27 #include <core/threading/thread.h>
28 #include <core/threading/mutex.h>
29 #include <core/threading/mutex_locker.h>
30 #include <core/threading/barrier.h>
31 #include <logging/logger.h>
33 #include <fvutils/system/camargp.h>
34 #include <fvutils/ipc/shm_image.h>
35 #include <fvutils/ipc/shm_lut.h>
36 #include <fvcams/factory.h>
37 #include <fvcams/cam_exceptions.h>
38 #include <fvcams/control/factory.h>
39 #include <core/exceptions/software.h>
41 #include <aspect/vision.h>
46 using namespace fawkes;
47 using namespace firevision;
79 SharedMemoryImageBuffer::cleanup(
false);
80 SharedMemoryLookupTable::cleanup(
false);
88 for (__ait = __aqts.begin(); __ait != __aqts.end(); ++__ait) {
94 __owned_controls.
lock();
96 for (i = __owned_controls.begin(); i != __owned_controls.end(); ++i) {
99 __owned_controls.clear();
100 __owned_controls.
unlock();
111 for (__ait = __aqts.begin(); __ait != __aqts.end(); ++__ait) {
112 __ait->second->set_vt_prepfin_hold(
true);
116 for (__ait = __aqts.begin(); __ait != __aqts.end(); ++__ait) {
117 __ait->second->set_vt_prepfin_hold(
false);
124 for (__ait = __aqts.begin(); __ait != __aqts.end(); ++__ait) {
127 __ait->second->wakeup(__aqt_barrier);
131 __aqt_barrier->
wait();
134 for (__ait = __aqts.begin(); __ait != __aqts.end();) {
135 if ( __ait->second->vision_threads->empty() &&
136 (__ait->second->vision_threads->empty_time() > __aqt_timeout) ) {
139 __ait->second->name());
143 delete __ait->second;
144 __aqts.erase(__ait++);
150 __started_threads.
lock();
152 while (stit != __started_threads.end()) {
155 stit->second->name(), __started_threads.size());
158 stit->second->vision_threads->set_thread_running(stit->first);
160 if ( stit->second->vision_threads->has_cyclic_thread() ) {
163 stit->second->name());
165 stit->second->prepare_finalize();
166 stit->second->cancel();
167 stit->second->join();
169 stit->second->start();
170 stit->second->cancel_finalize();
174 stit->second->name());
175 stit->second->prepare_finalize();
176 stit->second->cancel();
177 stit->second->join();
179 stit->second->start();
180 stit->second->cancel_finalize();
184 stit->second->set_enabled(
true);
188 __started_threads.erase( stittmp );
190 __started_threads.
unlock();
193 unsigned int num_cyclic_threads = 0;
194 for (__ait = __aqts.begin(); __ait != __aqts.end(); ++__ait) {
195 if ( __ait->second->vision_threads->has_cyclic_thread() ) {
196 ++num_cyclic_threads;
199 cond_recreate_barrier(num_cyclic_threads);
201 for (__ait = __aqts.begin(); __ait != __aqts.end(); ++__ait) {
202 __ait->second->set_vt_prepfin_hold(
false);
229 if ( vision_thread == NULL ) {
236 if ( __aqts.find(
id) != __aqts.end() ) {
238 c = __aqts[id]->camera_instance(cspace,
240 VisionAspect::CONTINUOUS));
242 __aqts[id]->vision_threads->add_waiting_thread(thread);
247 cam = CameraFactory::instance(cap);
252 e.
append(
"Could not open or start camera");
259 VisionAspect::CONTINUOUS));
268 logger->
log_info(
name(),
"Acquisition thread '%s' started for thread '%s' and camera '%s'",
269 aqt->
name(), thread->
name(),
id.c_str());
277 e.
append(
"FvBaseVisionMaster: could not instantiate camera");
282 e.
append(
"FvBaseVisionMaster: could not open or start camera");
302 if ( __aqts.find(
id) != __aqts.end() ) {
303 __aqts[id]->raw_subscriber_thread = thread;
314 FvBaseThread::create_camctrl(
const char *camera_string)
316 CameraControl *cc = CameraControlFactory::instance(camera_string);
318 __owned_controls.
lock();
319 __owned_controls.push_back(cc);
320 __owned_controls.sort();
321 __owned_controls.unique();
322 __owned_controls.
unlock();
325 throw Exception(
"Cannot create camera control of desired type");
337 if (__aqts.find(
id) != __aqts.end()) {
338 return CameraControlFactory::instance(__aqts[
id]->get_camera());
340 return create_camctrl(cam_string);
347 const std::type_info &typeinf)
354 if (__aqts.find(
id) != __aqts.end()) {
355 return CameraControlFactory::instance(typeinf, __aqts[
id]->get_camera());
357 return create_camctrl(cam_string);
365 __owned_controls.
lock();
367 if ((f = std::find(__owned_controls.begin(), __owned_controls.end(), cc)) != __owned_controls.end()) {
369 __owned_controls.erase(f);
371 __owned_controls.
unlock();
380 FvBaseThread::cond_recreate_barrier(
unsigned int num_cyclic_threads)
382 if ( (num_cyclic_threads + 1) != __aqt_barrier->
count() ) {
383 delete __aqt_barrier;
384 __aqt_barrier =
new Barrier( num_cyclic_threads + 1 );
393 unsigned int num_cyclic_threads = 0;
395 for (__ait = __aqts.begin(); __ait != __aqts.end(); ++__ait) {
398 __ait->second->vision_threads->remove_thread(thread);
400 if (__ait->second->raw_subscriber_thread == thread) {
401 __ait->second->raw_subscriber_thread = NULL;
404 if ( __ait->second->vision_threads->has_cyclic_thread() ) {
405 ++num_cyclic_threads;
409 "on unregister", __ait->second->name());
411 __ait->second->prepare_finalize();
412 __ait->second->cancel();
413 __ait->second->join();
415 __ait->second->start();
416 __ait->second->cancel_finalize();
420 cond_recreate_barrier(num_cyclic_threads);
430 for (__ait = __aqts.begin(); __ait != __aqts.end(); ++__ait) {
431 if (__ait->second->vision_threads->has_waiting_thread(thread)) {
432 __started_threads.lock();
433 __started_threads[thread] = __ait->second;
434 __started_threads.unlock();
447 for (__ait = __aqts.begin(); __ait != __aqts.end(); ++__ait) {
448 __ait->second->vision_threads->remove_waiting_thread(thread);