/home/fwph/code/wurde/modules/visionModule/Operators/DataCollector.cpp

Go to the documentation of this file.

#include "DataCollector.H"
#include <Logger.H>
#include <string>
#include <opencv/cv.h>
#include <opencv/highgui.h>

using namespace std;
using namespace WURDE;
using namespace WURDEVision;

DataCollector::DataCollector(std::string name) : VisionOperator(name), 
                                                 m_myRF(STRAT_ASSIGNED, "DataRangeFinderClient"), 
                                                 m_myPTU(STRAT_ASSIGNED, "DataPTUClient") {
        
        registerConsumer(&m_myRF);
        registerConsumer(&m_myPTU);
        
}

void DataCollector::initOperator() {
        // Potentially add two image saving threads here so that this operator doesn't
        // hold things up too badly.
        
        t1state=STATE_RUN;
       t2state=STATE_RUN;
       t3state=STATE_RUN;
       
       m_newt1image=false;
       m_newt2image=false;
       m_newt3image=false;
       
       m_t1image=cvCreateImage(cvSize(320, 240), IPL_DEPTH_8U, 3);
       m_t2image=cvCreateImage(cvSize(320, 240), IPL_DEPTH_8U, 3);
       m_t3image=cvCreateImage(cvSize(320, 240), IPL_DEPTH_8U, 3);
       
       pthread_create(&thread1,NULL,runDataCollectorThread1,(void *)this);
       pthread_create(&thread2,NULL,runDataCollectorThread2,(void *)this);
       pthread_create(&thread3,NULL,runDataCollectorThread3,(void *)this);
       
       if(g_globalConfiguration.haveModuleOption(m_name,"imageDir")){
               m_imageDir=g_globalConfiguration.getModuleOption(m_name,"imageDir");
       }else{
               m_imageDir=g_globalConfiguration.getDataDirectory();
       }
       
       if(g_globalConfiguration.haveModuleOption(m_name,"imagePrefix")){
              m_imagePrefix=g_globalConfiguration.getModuleOption(m_name,"imagePrefix");
       }else{
               m_imagePrefix=m_name;
       }
       
       if(g_globalConfiguration.haveModuleOption(m_name,"laserOutputFile")){
               m_outfilename=g_globalConfiguration.getModuleOption(m_name,"laserOutputFile");
               if(m_outfilename[0]!='/'){
                       m_outfilename=g_globalConfiguration.getDataDirectory()+"/"+m_outfilename;
               }
       }else{
               m_outfilename=g_globalConfiguration.getDataDirectory()+"/"+"laseroutdata.txt";
       }
       
       m_outfile.open(m_outfilename.c_str());
       
       m_state=STATE_RUN;
       m_format=FORMAT_CHAR_111_RGB;
       
       next=1;
}

void DataCollector::runOperator(WURDEImage mImage) {
        
        if(m_myRF.newData()){
                
                if(next==1){
                        if(!m_newt1image){              
                                if(m_t1image->width!=mImage.second->width||
                                   m_t1image->height!=mImage.second->height){
                                        m_t1image=NULL;
                                        m_t1image=cvCreateImage(cvSize(mImage.second->width, mImage.second->height), IPL_DEPTH_8U, 3);
                                }
                                
                                t1time=mImage.first;
                                // memcpy(m_t1image->imageData,mImage.second->imageData,3*mImage.second->height*mImage.second->width);
                                cvConvertImage(mImage.second,m_t1image,CV_CVTIMG_SWAP_RB);
                                next=2;
                                
                                m_newt1image=true;
                        }
                }else if (next==2){
                        if(!m_newt2image){
                                if(m_t2image->width!=mImage.second->width||
                                   m_t2image->height!=mImage.second->height){
                                        //                          cvReleaseImage(&m_t2image);
                                        m_t2image=NULL;
                                        m_t2image=cvCreateImage(cvSize(mImage.second->width, mImage.second->height), IPL_DEPTH_8U, 3);
                                }
                                t2time=mImage.first;
                                //memcpy(m_t2image->imageData,mImage.second->imageData,3*mImage.second->height*mImage.second->width);
                                cvConvertImage(mImage.second,m_t2image,CV_CVTIMG_SWAP_RB);
                                next=3;
                                m_newt2image=true;
                        }
                }else{
                        if(!m_newt3image){
                                if(m_t3image->width!=mImage.second->width||
                                   m_t3image->height!=mImage.second->height){
                                        //                          cvReleaseImage(&m_t3image);
                                        m_t3image=NULL;
                                        m_t3image=cvCreateImage(cvSize(mImage.second->width, mImage.second->height), IPL_DEPTH_8U, 3);
                                }
                                
                                t3time=mImage.first;
                                cvConvertImage(mImage.second,m_t3image,CV_CVTIMG_SWAP_RB);
                                //                   memcpy(m_t3image->imageData,mImage.second->imageData,3*mImage.second->height*mImage.second->width);
                                next=1;
                                m_newt3image=true;
                        }
                }
                
                m_myRF.getNextData();
                
                vector<double>::iterator rangeiter;
                
                for(rangeiter=m_myRF.data.ranges.begin();
                    rangeiter!=m_myRF.data.ranges.end();
                    rangeiter++){
                        
                        m_outfile << (*rangeiter) << " ";
                }
                
                if(m_myPTU.newInfo()){
                        m_myPTU.getNextInfo();
                }
                
                m_outfile << m_myPTU.info.pan.getValue() << " " << m_myPTU.info.tilt.getValue();
                
                m_outfile << endl << flush;
                
        }
        
}

void DataCollector::quitOperator() {
        m_outfile.close();
        t1state=STATE_QUIT;
        t2state=STATE_QUIT;
        t3state=STATE_QUIT;
        
        pthread_join(thread1,NULL);
        pthread_join(thread2,NULL);
        pthread_join(thread3,NULL);
        
        
        cvReleaseImage(&m_t1image);
        cvReleaseImage(&m_t2image);
        cvReleaseImage(&m_t3image);
        
        m_t1image=NULL;
        m_t2image=NULL;
        m_t3image=NULL;
}

void DataCollector::fileWriterThread1(){
        while(t1state==STATE_RUN){
                if(m_newt1image){
                        std::stringstream filename;
                        filename.setf(ios_base::showpoint);
                        filename.setf(ios_base::fixed,ios_base::floatfield);
                        double stamp;
                        stamp=t1time.getSeconds()+t1time.getUSeconds()/1000000.0;
                        
                        filename << m_imageDir << "/" << m_imagePrefix << "-" << stamp << ".ppm";
                        g_debug("Thread 1 writing..");
                        cvSaveImage((filename.str()).c_str(),m_t1image);
                        
                        m_newt1image=false;
                }
                usleep(50000);
        }
}

void DataCollector::fileWriterThread2(){
        while(t1state==STATE_RUN){
                if(m_newt2image){
                        std::stringstream filename;
                        filename.setf(ios_base::showpoint);
                        filename.setf(ios_base::fixed,ios_base::floatfield);
                        double stamp;
                        stamp=t2time.getSeconds()+t2time.getUSeconds()/1000000.0;
                        filename << m_imageDir << "/" << m_imagePrefix << "-" << stamp << ".ppm";
                        g_debug("Thread 2 writing..");
                        cvSaveImage((filename.str()).c_str(),m_t2image);
                        
                        m_newt2image=false;
                }
                usleep(50000);
        }
}

void DataCollector::fileWriterThread3(){
        while(t1state==STATE_RUN){
                if(m_newt3image){
                        std::stringstream filename;
                        filename.setf(ios_base::showpoint);
                        filename.setf(ios_base::fixed,ios_base::floatfield);
                        double stamp;
                        stamp=t3time.getSeconds()+t3time.getUSeconds()/1000000.0;
                        
                        filename << m_imageDir << "/" << m_imagePrefix << "-" << stamp << ".ppm";
                        g_debug("Thread 3 writing..");
                        cvSaveImage((filename.str()).c_str(),m_t3image) ;
                        
                        m_newt3image=false;
                        
                }             usleep(50000);
        }
}

extern "C" {
        void *runDataCollectorThread1(void *ptr){
                DataCollector *theObject=(DataCollector *)ptr;
                
                theObject->fileWriterThread1();
                pthread_exit(NULL);
        }
        
        void *runDataCollectorThread2(void *ptr){
                DataCollector *theObject=(DataCollector *)ptr;
                
                theObject->fileWriterThread2();
                pthread_exit(NULL);
        }
        
        void *runDataCollectorThread3(void *ptr){
                DataCollector *theObject=(DataCollector *)ptr;
                
                theObject->fileWriterThread3();
                pthread_exit(NULL);
        }
        
        VisionOperator *DataCollector_factory(std::string name) {
                return new DataCollector(name);
        }
        
        class DataCollectorProxy { 
        public:
                DataCollectorProxy(){
                        operatorFactory["DataCollector"] = DataCollector_factory;
                }
        };
        
        DataCollectorProxy p_imagewriter;
}

---