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 "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 }