extractor.hpp

Go to the documentation of this file.

#ifndef _THERMALVIS_EXTRACTOR_H_
#define _THERMALVIS_EXTRACTOR_H_
        
#include "ros_resources.hpp"
#include "general_resources.hpp"
#include "opencv_resources.hpp"
#include "tools.hpp"
#include "improc.hpp"

#include <dynamic_reconfigure/server.h>
#include "extractorConfig.h"

#include <rosbag/bag.h>
#include <rosbag/view.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

#include <opencv/cv.h>
#include <opencv/highgui.h>

#include <cv_bridge/cv_bridge.h> 
#include <image_transport/image_transport.h>

#include <boost/foreach.hpp>
#include "boost/filesystem.hpp"  
#include <time.h>
#include <math.h>

#include "ros/ros.h"
#include "sensor_msgs/Image.h"
#include "sensor_msgs/CameraInfo.h"

#include <iostream>
#include <fstream>

#define SAVEPLY false
#define FILTER_DISTANCE 1
#define SIGMA_P 15
#define SIGMA_R .05

#define MAX_BAGS 200

#define DEFAULT_MAX_TIMESTAMP_DIFF      0.01

const char __PROGRAM__[] = "EXTRACTOR";
using namespace cv;
struct extractorData {
        
        string bagSource, outputFolder;
        
        bool debugMode, verboseMode;
        
        bool internalTimestamp, thermistorReading, recordDuplicates, undistortThermal;
        
        bool setupForMapping;
        
        bool splitBags;
        
        string thermalIntrinsics;
        
        int maxFrames;
        
        string rgbTopic;
        string depthTopic;
        string thermalTopic;
        
        bool rawTimeFormat;
        
        // Intra-determined variables
        
        bool wantsThermal, wantsRgb, wantsDepth;
        
        string rgbInfoTopic;
        string depthInfoTopic;
        string thermalInfoTopic;
        
        string read_addr;
        
        bool obtainStartingData(ros::NodeHandle& nh);  
        
};

class extractorNode {
private:

        ros::NodeHandle *ref;
        
        // cam-calib
        Mat imageSize, cameraMatrix, distCoeffs, newCamMat;
        Size cameraSize;
        Mat map1, map2;

        
        Mat oldThermal;
        
        rosbag::Bag bag[MAX_BAGS];
        
        vector<string> splitBagNames;
        
        float *writeData;
        
        extractorData configData;
        
        bool stillListening;
        
        ofstream ofs_thermistor_log;
        string thermistorLogFile;
        
        ros::NodeHandle private_node_handle;
        
        char nodeName[256];
        
        dynamic_reconfigure::Server<thermalvis::extractorConfig> server;
        dynamic_reconfigure::Server<thermalvis::extractorConfig>::CallbackType f;
        
        float** ApplyBilateralFilter(float** data, const int width, const int height, float sigmaP, float sigmaR);
        
        FILE *dataFile, *timeFile, *depthtimeFile, *depthInternaltimeFile, *imgtimeFile, *imgInternaltimeFile, *thermaltimeFile, *thermalInternaltimeFile, *thermalDuplicatesFile;
        
        int count;
        int filenum;
        double startTime;
        int skipFrames;
        int maxFrames;

        float d_fx;
        float d_fy;
        float d_cx;
        float d_cy;
        
  char *outputFilename;
  char *outputFilenameThermal;
  char *outputdepth, *outputcloud ;
  
  stringstream imgPath;
    stringstream timePath;
    stringstream imgtimePath, imgInternaltimePath, depthtimePath, depthInternaltimePath, thermaltimePath, thermalInternaltimePath, thermalDuplicatesPath;
    
    // Image topics to load
  std::vector<std::string> topics;
  
  
  
  int count_rgb; 
  int count_depth;
  int count_rgbinfo; 
  int count_depthinfo;
  int count_thermal;
  int count_thermalinfo;
  
  sensor_msgs::Image::ConstPtr rgbImg;
  sensor_msgs::CameraInfo::ConstPtr rgbInfo;
  sensor_msgs::Image::ConstPtr depthImg;
  sensor_msgs::CameraInfo::ConstPtr depthInfo;
  sensor_msgs::Image::ConstPtr thermalImg;
  sensor_msgs::CameraInfo::ConstPtr thermalInfo;
  
        
public:

        rosbag::View *view;

        extractorNode(ros::NodeHandle& nh, extractorData startupData);
        ~extractorNode();
        
        void makeDirectories();
        
        bool isStillListening();
        
        void serverCallback(thermalvis::extractorConfig &config, uint32_t level);
        
        
        
        void extractBagFile();
        void cameraCallback(const sensor_msgs::Image::ConstPtr& rgbImgMsg,
                                          const sensor_msgs::Image::ConstPtr& depthImgMsg,
                                          const sensor_msgs::Image::ConstPtr& thermalImgMsg,
                                          const sensor_msgs::CameraInfo::ConstPtr& rgbInfoMsg,
                                          const sensor_msgs::CameraInfo::ConstPtr& depthInfoMsg,
                                          const sensor_msgs::CameraInfo::ConstPtr& thermalInfoMsg
                                          );
};

typedef message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo > SyncPolicy;

template <class M>
class BagSubscriber : public message_filters::SimpleFilter<M>
{
public:
  void newMessage(const boost::shared_ptr<M const> &msg)
  {
    signalMessage(msg);
  }
};

#endif