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 
00037 /** @class FvRetrieverThread "retriever_thread.h"
00038  * FireVision retriever thread.
00039  * This implements the functionality of the FvRetrieverPlugin.
00040  * @author Tim Niemueller
00041  */
00042 
00043 /** Constructor.
00044  * @param camera_string camera argument string for camera to open
00045  * @param id ID used to form thread name (FvRetrieverThread_[ID]) and shared
00046  * memory image segment ID (retriever_[ID]).
00047  */
00048 FvRetrieverThread::FvRetrieverThread(const char *camera_string, const char *id)
00049   : Thread("FvRetrieverThread", Thread::OPMODE_WAITFORWAKEUP),
00050     VisionAspect(VisionAspect::CYCLIC)
00051 {
00052   __id = strdup(id);
00053   __camera_string = strdup(camera_string);
00054   set_name("FvRetrieverThread_%s", id);
00055   seq_writer = NULL;
00056 }
00057
00058 
00059 /** Destructor. */
00060 FvRetrieverThread::~FvRetrieverThread()
00061 {
00062   free(__camera_string);
00063   free(__id);
00064 }
00065
00066
00067 void
00068 FvRetrieverThread::init()
00069 {
00070   try {
00071     logger->log_debug(name(), "Registering for camera '%s'", __camera_string);
00072     cam = vision_master->register_for_camera(__camera_string, this);
00073   } catch (Exception &e) {
00074     e.append("FvRetrieverThread::init() failed");
00075     throw;
00076   }
00077   try {
00078     char *imgbufname;
00079     if ( asprintf(&imgbufname, "retriever_%s", __id) == -1 ) {
00080       throw Exception("Cannot allocate buffer name");
00081     }
00082     shm = new SharedMemoryImageBuffer(imgbufname, cam->colorspace(),
00083                                       cam->pixel_width(), cam->pixel_height());
00084
00085     free(imgbufname);
00086     if ( ! shm->is_valid() ) {
00087       throw Exception("Shared memory segment not valid");
00088     }
00089   } catch (Exception &e) {
00090     delete cam;
00091     cam = NULL;
00092     throw;
00093   }
00094
00095   seq_writer = NULL;
00096   try {
00097     if ( config->get_bool("/firevision/retriever/save_images") ) {
00098       logger->log_info(name(), "Writing images to disk");
00099       Writer* writer = new JpegWriter();
00100       seq_writer = new SeqWriter(writer);
00101       std::string save_path;
00102        try {
00103         save_path = config->get_string("/firevision/retriever/save_path");
00104        } catch (Exception &e) {
00105          save_path = ("recorded_images");
00106          logger->log_info(name(), "No save path specified. Using './%s'", save_path.c_str());
00107        }
00108       seq_writer->set_path( save_path.c_str() );
00109       seq_writer->set_dimensions( cam->pixel_width(), cam->pixel_height() );
00110       seq_writer->set_colorspace( cam->colorspace() );
00111     }
00112   } catch (Exception &e) {
00113     // ignored, not critical
00114   }
00115
00116   __tt = NULL;
00117   try {
00118     if ( config->get_bool("/firevision/retriever/use_time_tracker") ) {
00119       __tt = new TimeTracker();
00120       __ttc_capture = __tt->add_class("Capture");
00121       __ttc_memcpy  = __tt->add_class("Memcpy");
00122       __ttc_dispose = __tt->add_class("Dispose");
00123       __loop_count  = 0;
00124     }
00125   } catch (Exception &e) {
00126     // ignored, not critical
00127   }
00128
00129   __cm = new ColorModelLookupTable(1, "retriever-colormap", true);
00130   YuvColormap *ycm = __cm->get_colormap();
00131   for (unsigned int u = 100; u < 150; ++u) {
00132     for (unsigned int v = 100; v < 150; ++v) {
00133       ycm->set(128, u, v, C_ORANGE);
00134     }
00135   }
00136 }
00137
00138
00139 void
00140 FvRetrieverThread::finalize()
00141 {
00142   logger->log_debug(name(), "Unregistering from vision master");
00143   vision_master->unregister_thread(this);
00144   delete cam;
00145   delete shm;
00146   delete seq_writer;
00147   delete __tt;
00148   delete __cm;
00149 }
00150
00151 
00152 /** Thread loop. */
00153 void
00154 FvRetrieverThread::loop()
00155 {
00156   if (__tt) {
00157     // use time tracker
00158     __tt->ping_start(__ttc_capture);
00159     cam->capture();
00160     __tt->ping_end(__ttc_capture);
00161     __tt->ping_start(__ttc_memcpy);
00162     memcpy(shm->buffer(), cam->buffer(), cam->buffer_size()-1);
00163     __tt->ping_end(__ttc_memcpy);
00164     __tt->ping_start(__ttc_dispose);
00165     cam->dispose_buffer();
00166     __tt->ping_end(__ttc_dispose);
00167     if ( (++__loop_count % 200) == 0 ) {
00168       // output results every 200 loops
00169       __tt->print_to_stdout();
00170     }
00171   } else {
00172     // no time tracker
00173     cam->capture();
00174     memcpy(shm->buffer(), cam->buffer(), cam->buffer_size());
00175     cam->dispose_buffer();
00176   }
00177
00178   if (seq_writer) {
00179     seq_writer->write( shm->buffer() );
00180   }
00181 }