00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
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
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046 FvSrSavePipelineThread::FvSrSavePipelineThread()
00047 : Thread("FvSrSavePipelineThread", Thread::OPMODE_WAITFORWAKEUP),
00048 VisionAspect(VisionAspect::CYCLIC)
00049 {
00050 }
00051
00052
00053
00054 FvSrSavePipelineThread::~FvSrSavePipelineThread()
00055 {
00056 }
00057
00058
00059
00060
00061
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
00076 void
00077 FvSrSavePipelineThread::finalize()
00078 {
00079 vision_master->unregister_thread(this);
00080 }
00081
00082
00083
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