Fawkes API Fawkes Development Version

retriever_thread.cpp

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 }
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends