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 "retriever_thread.h" 00024 00025 #include <fvcams/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 <fvmodels/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 cfg_name ID used to form thread name (FvRetrieverThread_[ID]) and shared 00047 * memory image segment ID (retriever_[ID]). 00048 * @param cfg_prefix configuration prefix path 00049 */ 00050 FvRetrieverThread::FvRetrieverThread(std::string camera_string, 00051 std::string cfg_name, std::string cfg_prefix) 00052 : Thread("FvRetrieverThread", Thread::OPMODE_WAITFORWAKEUP), 00053 VisionAspect(VisionAspect::CYCLIC) 00054 { 00055 cfg_name_ = cfg_name; 00056 cfg_prefix_ = cfg_prefix; 00057 camera_string_ = camera_string; 00058 set_name("FvRetrieverThread_%s", cfg_name_.c_str()); 00059 seq_writer = NULL; 00060 } 00061 00062 00063 /** Destructor. */ 00064 FvRetrieverThread::~FvRetrieverThread() 00065 { 00066 } 00067 00068 00069 void 00070 FvRetrieverThread::init() 00071 { 00072 try { 00073 logger->log_debug(name(), "Registering for camera '%s'", camera_string_.c_str()); 00074 cam = vision_master->register_for_camera(camera_string_.c_str(), this); 00075 } catch (Exception &e) { 00076 e.append("FvRetrieverThread::init() failed"); 00077 throw; 00078 } 00079 try { 00080 char *imgbufname; 00081 if ( asprintf(&imgbufname, "retriever_%s", cfg_name_.c_str()) == -1 ) { 00082 throw Exception("Cannot allocate buffer name"); 00083 } 00084 shm = new SharedMemoryImageBuffer(imgbufname, cam->colorspace(), 00085 cam->pixel_width(), cam->pixel_height()); 00086 00087 free(imgbufname); 00088 if ( ! shm->is_valid() ) { 00089 throw Exception("Shared memory segment not valid"); 00090 } 00091 } catch (Exception &e) { 00092 delete cam; 00093 cam = NULL; 00094 throw; 00095 } 00096 00097 try { 00098 std::string frame_id = config->get_string((cfg_prefix_ + "frame").c_str()); 00099 shm->set_frame_id(frame_id.c_str()); 00100 } catch (Exception &e) {/* ignored, not critical */} 00101 00102 seq_writer = NULL; 00103 try { 00104 if ( config->get_bool("/firevision/retriever/save_images") ) { 00105 logger->log_info(name(), "Writing images to disk"); 00106 Writer* writer = new JpegWriter(); 00107 seq_writer = new SeqWriter(writer); 00108 std::string save_path; 00109 try { 00110 save_path = config->get_string("/firevision/retriever/save_path"); 00111 } catch (Exception &e) { 00112 save_path = ("recorded_images"); 00113 logger->log_info(name(), "No save path specified. Using './%s'", save_path.c_str()); 00114 } 00115 seq_writer->set_path( save_path.c_str() ); 00116 seq_writer->set_dimensions( cam->pixel_width(), cam->pixel_height() ); 00117 seq_writer->set_colorspace( cam->colorspace() ); 00118 } 00119 } catch (Exception &e) { 00120 // ignored, not critical 00121 } 00122 00123 __tt = NULL; 00124 try { 00125 if ( config->get_bool("/firevision/retriever/use_time_tracker") ) { 00126 __tt = new TimeTracker(); 00127 __ttc_capture = __tt->add_class("Capture"); 00128 __ttc_memcpy = __tt->add_class("Memcpy"); 00129 __ttc_dispose = __tt->add_class("Dispose"); 00130 __loop_count = 0; 00131 } 00132 } catch (Exception &e) { 00133 // ignored, not critical 00134 } 00135 00136 __cm = new ColorModelLookupTable(1, "retriever-colormap", true); 00137 YuvColormap *ycm = __cm->get_colormap(); 00138 for (unsigned int u = 100; u < 150; ++u) { 00139 for (unsigned int v = 100; v < 150; ++v) { 00140 ycm->set(128, u, v, C_ORANGE); 00141 } 00142 } 00143 00144 __cam_has_timestamp_support = true; 00145 try { 00146 fawkes::Time *t = cam->capture_time(); 00147 if (t->is_zero()) { 00148 throw NotImplementedException(""); 00149 } 00150 cap_time_ = NULL; 00151 } 00152 catch (NotImplementedException &e) 00153 { 00154 __cam_has_timestamp_support = false; 00155 cap_time_ = new Time(clock); 00156 } 00157 } 00158 00159 00160 void 00161 FvRetrieverThread::finalize() 00162 { 00163 logger->log_debug(name(), "Unregistering from vision master"); 00164 vision_master->unregister_thread(this); 00165 delete cam; 00166 delete shm; 00167 delete seq_writer; 00168 delete __tt; 00169 delete __cm; 00170 delete cap_time_; 00171 } 00172 00173 00174 /** Thread loop. */ 00175 void 00176 FvRetrieverThread::loop() 00177 { 00178 if (__tt) { 00179 // use time tracker 00180 __tt->ping_start(__ttc_capture); 00181 cam->capture(); 00182 __tt->ping_end(__ttc_capture); 00183 __tt->ping_start(__ttc_memcpy); 00184 memcpy(shm->buffer(), cam->buffer(), cam->buffer_size()-1); 00185 __tt->ping_end(__ttc_memcpy); 00186 if (__cam_has_timestamp_support) shm->set_capture_time(cam->capture_time()); 00187 __tt->ping_start(__ttc_dispose); 00188 cam->dispose_buffer(); 00189 __tt->ping_end(__ttc_dispose); 00190 if ( (++__loop_count % 200) == 0 ) { 00191 // output results every 200 loops 00192 __tt->print_to_stdout(); 00193 } 00194 } else { 00195 // no time tracker 00196 cam->capture(); 00197 memcpy(shm->buffer(), cam->buffer(), cam->buffer_size()); 00198 if (__cam_has_timestamp_support) { 00199 shm->set_capture_time(cam->capture_time()); 00200 } else { 00201 cap_time_->stamp(); 00202 shm->set_capture_time(cap_time_); 00203 } 00204 cam->dispose_buffer(); 00205 } 00206 00207 if (seq_writer) { 00208 seq_writer->write( shm->buffer() ); 00209 } 00210 }