Fawkes API Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * retriever_thread.cpp - FireVision Retriever Thread 00004 * 00005 * Created: Tue Jun 26 17:39:11 2007 00006 * Copyright 2006-2008 Tim Niemueller [www.niemueller.de] 00007 * 00008 ****************************************************************************/ 00009 00010 /* This program is free software; you can redistribute it and/or modify 00011 * it under the terms of the GNU General Public License as published by 00012 * the Free Software Foundation; either version 2 of the License, or 00013 * (at your option) any later version. 00014 * 00015 * This program is distributed in the hope that it will be useful, 00016 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00017 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00018 * GNU Library General Public License for more details. 00019 * 00020 * Read the full text in the LICENSE.GPL file in the doc directory. 00021 */ 00022 00023 #include <apps/retriever/retriever_thread.h> 00024 00025 #include <cams/camera.h> 00026 #include <fvutils/ipc/shm_image.h> 00027 #include <utils/time/tracker.h> 00028 #include <fvutils/writers/seq_writer.h> 00029 #include <fvutils/writers/jpeg.h> 00030 #include <models/color/lookuptable.h> 00031 00032 #include <cstring> 00033 #include <cstdlib> 00034 00035 using namespace fawkes; 00036 using namespace firevision; 00037 00038 /** @class FvRetrieverThread "retriever_thread.h" 00039 * FireVision retriever thread. 00040 * This implements the functionality of the FvRetrieverPlugin. 00041 * @author Tim Niemueller 00042 */ 00043 00044 /** Constructor. 00045 * @param camera_string camera argument string for camera to open 00046 * @param id ID used to form thread name (FvRetrieverThread_[ID]) and shared 00047 * memory image segment ID (retriever_[ID]). 00048 */ 00049 FvRetrieverThread::FvRetrieverThread(const char *camera_string, const char *id) 00050 : Thread("FvRetrieverThread", Thread::OPMODE_WAITFORWAKEUP), 00051 VisionAspect(VisionAspect::CYCLIC) 00052 { 00053 __id = strdup(id); 00054 __camera_string = strdup(camera_string); 00055 set_name("FvRetrieverThread_%s", id); 00056 seq_writer = NULL; 00057 } 00058 00059 00060 /** Destructor. */ 00061 FvRetrieverThread::~FvRetrieverThread() 00062 { 00063 free(__camera_string); 00064 free(__id); 00065 } 00066 00067 00068 void 00069 FvRetrieverThread::init() 00070 { 00071 try { 00072 logger->log_debug(name(), "Registering for camera '%s'", __camera_string); 00073 cam = vision_master->register_for_camera(__camera_string, this); 00074 } catch (Exception &e) { 00075 e.append("FvRetrieverThread::init() failed"); 00076 throw; 00077 } 00078 try { 00079 char *imgbufname; 00080 if ( asprintf(&imgbufname, "retriever_%s", __id) == -1 ) { 00081 throw Exception("Cannot allocate buffer name"); 00082 } 00083 shm = new SharedMemoryImageBuffer(imgbufname, cam->colorspace(), 00084 cam->pixel_width(), cam->pixel_height()); 00085 00086 free(imgbufname); 00087 if ( ! shm->is_valid() ) { 00088 throw Exception("Shared memory segment not valid"); 00089 } 00090 } catch (Exception &e) { 00091 delete cam; 00092 cam = NULL; 00093 throw; 00094 } 00095 00096 seq_writer = NULL; 00097 try { 00098 if ( config->get_bool("/firevision/retriever/save_images") ) { 00099 logger->log_info(name(), "Writing images to disk"); 00100 Writer* writer = new JpegWriter(); 00101 seq_writer = new SeqWriter(writer); 00102 std::string save_path; 00103 try { 00104 save_path = config->get_string("/firevision/retriever/save_path"); 00105 } catch (Exception &e) { 00106 save_path = ("recorded_images"); 00107 logger->log_info(name(), "No save path specified. Using './%s'", save_path.c_str()); 00108 } 00109 seq_writer->set_path( save_path.c_str() ); 00110 seq_writer->set_dimensions( cam->pixel_width(), cam->pixel_height() ); 00111 seq_writer->set_colorspace( cam->colorspace() ); 00112 } 00113 } catch (Exception &e) { 00114 // ignored, not critical 00115 } 00116 00117 __tt = NULL; 00118 try { 00119 if ( config->get_bool("/firevision/retriever/use_time_tracker") ) { 00120 __tt = new TimeTracker(); 00121 __ttc_capture = __tt->add_class("Capture"); 00122 __ttc_memcpy = __tt->add_class("Memcpy"); 00123 __ttc_dispose = __tt->add_class("Dispose"); 00124 __loop_count = 0; 00125 } 00126 } catch (Exception &e) { 00127 // ignored, not critical 00128 } 00129 00130 __cm = new ColorModelLookupTable(1, "retriever-colormap", true); 00131 YuvColormap *ycm = __cm->get_colormap(); 00132 for (unsigned int u = 100; u < 150; ++u) { 00133 for (unsigned int v = 100; v < 150; ++v) { 00134 ycm->set(128, u, v, C_ORANGE); 00135 } 00136 } 00137 00138 __cam_has_timestamp_support = true; 00139 try { 00140 cam->capture_time(); 00141 } 00142 catch (NotImplementedException &e) 00143 { 00144 __cam_has_timestamp_support = false; 00145 } 00146 } 00147 00148 00149 void 00150 FvRetrieverThread::finalize() 00151 { 00152 logger->log_debug(name(), "Unregistering from vision master"); 00153 vision_master->unregister_thread(this); 00154 delete cam; 00155 delete shm; 00156 delete seq_writer; 00157 delete __tt; 00158 delete __cm; 00159 } 00160 00161 00162 /** Thread loop. */ 00163 void 00164 FvRetrieverThread::loop() 00165 { 00166 if (__tt) { 00167 // use time tracker 00168 __tt->ping_start(__ttc_capture); 00169 cam->capture(); 00170 __tt->ping_end(__ttc_capture); 00171 __tt->ping_start(__ttc_memcpy); 00172 memcpy(shm->buffer(), cam->buffer(), cam->buffer_size()-1); 00173 __tt->ping_end(__ttc_memcpy); 00174 if (__cam_has_timestamp_support) shm->set_capture_time(cam->capture_time()); 00175 __tt->ping_start(__ttc_dispose); 00176 cam->dispose_buffer(); 00177 __tt->ping_end(__ttc_dispose); 00178 if ( (++__loop_count % 200) == 0 ) { 00179 // output results every 200 loops 00180 __tt->print_to_stdout(); 00181 } 00182 } else { 00183 // no time tracker 00184 cam->capture(); 00185 memcpy(shm->buffer(), cam->buffer(), cam->buffer_size()); 00186 if (__cam_has_timestamp_support) shm->set_capture_time(cam->capture_time()); 00187 cam->dispose_buffer(); 00188 } 00189 00190 if (seq_writer) { 00191 seq_writer->write( shm->buffer() ); 00192 } 00193 }