pipeline_thread.cpp

00001 
00002 /***************************************************************************
00003  *  pipeline_thread.cpp - SwissRanger Save Pipeline Thread
00004  *
00005  *  Created: Fri Jan 22 10:50:13 2010
00006  *  Copyright  2005-2010  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 "pipeline_thread.h"
00024 
00025 #include <cams/camera.h>
00026 
00027 #include <sys/time.h>
00028 #include <stdlib.h>
00029 #include <cstdio>
00030 
00031 using namespace fawkes;
00032 
00033 /** @class FvSrSavePipelineThread "pipeline_thread.h"
00034  * SrSave vision image processing pipeline.
00035  * This thread implements an image processing pipeline that uses a colormodel and
00036  * classifier to determine regions of interest (ROI) which contain a significant
00037  * amount with "pixels of ball color". The best ROI is then filtered for edge detection.
00038  * On the edges a circle shape detection is carried out to confirm the result and to
00039  * get the required data to calculate the relative and global position of the ball.
00040  *
00041  * @author Tim Niemueller
00042  */
00043 
00044 
00045 /** Constructor. */
00046 FvSrSavePipelineThread::FvSrSavePipelineThread()
00047   : Thread("FvSrSavePipelineThread", Thread::OPMODE_WAITFORWAKEUP),
00048     VisionAspect(VisionAspect::CYCLIC)
00049 {
00050 }
00051 
00052 
00053 /** Destructor. */
00054 FvSrSavePipelineThread::~FvSrSavePipelineThread()
00055 {
00056 }
00057 
00058 
00059 /** Initialize the pipeline thread.
00060  * Camera is requested, config parameters are obtained from the config db, and
00061  * other miscellaneous init stuff is done here.
00062  */
00063 void
00064 FvSrSavePipelineThread::init()
00065 {
00066   try {
00067     __cam = vision_master->register_for_raw_camera("swissranger:any:mode=CARTESIAN_FLOAT", this );
00068   } catch (Exception& e) {
00069     e.append("FvSrSavePipelineThread::init() failed since no camera is specified");
00070     throw;
00071   }
00072 }
00073 
00074 
00075 /** Thread finalization. */
00076 void
00077 FvSrSavePipelineThread::finalize()
00078 {
00079   vision_master->unregister_thread(this);
00080 }
00081 
00082 /** A new image is retrieved from the camera and the classifier looks for a ball
00083  * in the image */
00084 void
00085 FvSrSavePipelineThread::loop()
00086 {
00087   __cam->capture();
00088 
00089   const unsigned int width  = __cam->pixel_width();
00090   const unsigned int height = __cam->pixel_height();
00091 
00092   float *fbuf = (float *)__cam->buffer();
00093   float *x = fbuf;
00094   float *y = x + width * height;
00095   float *z = y + width * height;
00096 
00097   char *filename;
00098   if (asprintf(&filename, "swissranger-%05u.pts", __frame_i++) != -1) {
00099     FILE *f = fopen(filename, "w");
00100 
00101     for (unsigned int h = 0; h < height; ++h) {
00102       for (unsigned int w = 0; w < width; ++w) {
00103         fprintf(f, "%f %f %f 128 128 128\n",
00104                 *x++ * 2000., *y++ * 2000., *z++ * 2000.);
00105       }
00106     }
00107 
00108     fclose(f);
00109     free(filename);
00110   } else {
00111     logger->log_warn(name(), "Failed to allocate filename");
00112   }
00113 
00114 
00115   __cam->dispose_buffer();
00116 }
00117 

Generated on Tue Feb 22 13:32:14 2011 for Fawkes API by  doxygen 1.4.7