Fawkes API
Fawkes Development Version
|
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