openni_thermo_node.cpp

Go to the documentation of this file.

#include "openni_cam.h"

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/image_encodings.h>
//
//
//
#include <sensor_msgs/CameraInfo.h>
#include <geometry_msgs/TransformStamped.h>
#include <openni_cam/thermo_info.h>
//#include <tf/tf.h>
//#include <tf/transform_listener.h>
//#include <tf/transform_datatypes.h>
//#include <tf/transform_broadcaster.h>
//
//
//
#include <stdio.h>
//#include "error.h"
#include <cmath>

#include "camera.h"
#include <stdio.h>
#include "error.h"

#include "include/Imager.h"
#include "ImagerUVC.h"

#include <QtGui/QApplication>
#include <QtGui/QKeyEvent>

using namespace optris;
using namespace exttype;

image_transport::Publisher publisher1;
image_transport::Publisher publisher2;
image_transport::Publisher publisher_ir;
//image_transport::Publisher publisher_ir2;
ros::Publisher publisher_rgb_info;
ros::Publisher publisher_ir_info;
ros::Publisher publisher_ir_info1;
//
//
//
//
class thermo_node {
public:
    camera cam;
    Imager * imager;
    unsigned long serial;
    int Width;
    int Height;
    int count;
    int acq_count;
    bool frame_acquired;
    bool frame_processed;
    bool recali;
    unsigned short int bimage[5];
    dynamic::num_array<unsigned short int,2> image;
    dynamic::num_array<float,2> ftemp;
    //dynamic::num_array<unsigned char,2> int8ftemp;
    //dynamic::num_array<unsigned char,2> image_normalized;
    float t_max;
    float t_min;
    float t_med;
    //dynamic::num_array<unsigned short int,2> image;
public:

    unsigned char * buff;

    thermo_node():buff(0) {
    };

    ~thermo_node() {
        if(buff)delete buff;
        delete imager;
    };
    void init(fptrOptrisFrame pOnFrame) {
        cam.find_device();
        cam.init_device();
        Width = cam.width;
        Height = cam.height-1;
        serial = cam.dev_serial;
        int frequency = cam.frequency;

        imager = new Imager(serial, Width, Height+1, frequency, "../../thermo_cam/data/config/12050017.xml");
        count = 0;
        imager->forceRecalibration();
        buff = new unsigned char[Width*(Height+1)*2];
        imager->setFrameCallback(pOnFrame);
        bimage[0] = 0;
        acq_count = 0;
        recali = true;
    };
    void triger() {
        //if(count==0) {
        //    printf("thermo_node %4.4i  ",(count%10000));
        //    fflush(stdout);
        //};
        cam.get_image();
        //const char * psym = "-\\|/";
        //std::cout<<"\b"<<psym[count % 4];fflush(stdout);
        //printf("\b\b\b\b%4.4i",(count%10000));
        //fflush(stdout);
    };

    void process() {
        imager->process((unsigned char *)cam.buffer.start);
    };

    void force_recali() {
        imager->forceRecalibration();
        recali = true;
        for(int i=0;i<5;++i){
            get_image();
        };
    };

    void get_image() {
        frame_acquired = false;
        while(!frame_acquired) {
            triger();
            process();
            const char * psym = "-\\|/";
            std::cout<<"\b"<<psym[count % 4];
            fflush(stdout);
            ++count;
        };
        ++acq_count;
        recali = false;
        //filter_ir();
    };

 /*
    void get_image() {
        frame_acquired = false;
        while(!frame_acquired) {
            triger();
            frame_processed = false;
            process();
            if(0 && !recali) { // normal frame, wait until it is processed
                int i=0;
                for(; i<10 && !frame_processed; ++i) {
                    usleep(500);
                };
                if(i>10) {
                    std::cout<<" timeout \n";
                    fflush(stdout);
                };
            } else { //push more frames
                //usleep(1000);
                // print to indicate this case
                const char * psym = "-\\|/";
                std::cout<<"\b"<<psym[count % 4];
                fflush(stdout);
            };
            ++count;
        };
        ++acq_count;
        recali = false;
        filter_ir();
    };
 */
    void filter_ir() {
        exttype::mint2 sz = image.size();
        dynamic::num_array<float,2> vals; //C
        ftemp.resize(sz);
        vals.resize(sz);
        //image_normalized.resize(sz);
        //int8ftemp.resize(sz);
        for(int i=0; i<image.length(); ++i) {
            vals[i] = ((float)(image[i])-1000.0)*0.1;
        };
        ftemp << 0;
        int Width = sz[0];
        int Height = sz[1];

        float max=-1e6;
        float min=+1e6;
        //median filter
        std::vector<float> a;
        a.resize(9);
        for(int i=1; i<Height-1; ++i) {
            for(int j=1; j<Width-1; ++j) {
                //int ii = i*Width+(Width-1+j);
                int ii = i*Width+j;
                a[0] = vals[ii];
                a[1] = vals[ii+1];
                a[2] = vals[ii-1];
                a[3] = vals[ii-Width];
                a[4] = vals[ii+Width];
                a[5] = vals[ii-1-Width];
                a[6] = vals[ii-1+Width];
                a[7] = vals[ii+1-Width];
                a[8] = vals[ii+1+Width];
                std::nth_element(a.begin(),a.begin()+5,a.end());
                float pixel = a[5];
                //src->ftemp[i * Width+Width-1-j] = pixel;
                ftemp(j,i) = pixel;
                //int8ftemp(j,i) = int(pixel);
                if(pixel > max)max = pixel;
                if(pixel < min)min = pixel;
            };
        };
        t_max = max;
        t_min = min;
        //dynamic::num_array<float,2> a = src->IR;
        int med_pos = vals.length()/2;
        //std::nth_element(vals.begin(),vals.begin()+med_pos,vals.end());
        //t_med = vals[med_pos];
        /*
        for(int i=0; i<image_normalized.length(); ++i) {
            image_normalized[i] = (unsigned char)std::max(0.0,std::min(255.0,((ftemp[i]-t_min)/(t_max-t_min)*255.0)));
        };
        */
    };

    void cbOnFrame(unsigned short* image, unsigned int w, unsigned int h) {
        //return;
        int num_bytes = Width*Height*2;
        //
        int i0 = num_bytes/4;
        if(bimage[0]==0 || bimage[0]==image[i0]) { //same image, skip it, wait for calibration
            bimage[0]=image[i0];
            // triger next immediately
            frame_acquired = false;
            frame_processed = true;
            return;
        };
        //different image, remember
        bimage[0]=image[i0];
        this->image.resize(mint2(w,h));
        memcpy(this->image.begin(),image,num_bytes);
        frame_acquired = true;
        frame_processed = true;
        return;
    };

    /*
        void cbOnFrame(unsigned short* image, unsigned int w, unsigned int h) {
            //return;
            ++count;
            sensor_msgs::ImagePtr msg(new sensor_msgs::Image());
            msg->header.stamp = ros::Time::now();
            int num_bytes = Width*Height*2;
            msg->data.resize(num_bytes);
            memcpy(&msg->data[0],image,num_bytes);
            msg->encoding = sensor_msgs::image_encodings::MONO16;
            msg->step = 2*Width;
            msg->height = Height;
            msg->width = Width;
            msg->header.frame_id = "";
            msg->header.seq = count;
            publisher_ir.publish(msg);
            frame_acquired = true;
        };
    */
};

thermo_node tcam;
openni_cam::openni_cam cam;



//___________________________________________________________

class myApp : public QApplication {
public:
    int count;
    int rcount;
    int recali_time; // recalibrate every 3 seconds
    int loop_rate;
public:
    myApp(int & argc, char ** argv, ros::NodeHandle & nhh):QApplication(argc,argv) {
        count = 0;
        rcount = 0;
        recali_time = 10; // recalibrate every 10 seconds
        //loop_rate = 5;
        nhh.param<int>("loop_rate",loop_rate,5);
        startTimer(1000/loop_rate);
    };
    //
    ~myApp() {
    };
    /*
        void cbOnFrame(unsigned short* image, unsigned int w, unsigned int h) {
            //return;
            int num_bytes = tcam.Width*tcam.Height*2;
            //
            int i0 = num_bytes/4;
            if(tcam.bimage[0]==0 || tcam.bimage[0]==image[i0]){//same image, skip it, wait for calibration
                tcam.bimage[0]=image[i0];
                // triger next immediately
                const char * psym = "-\\|/";
                std::cout<<"\b"<<psym[count % 4];fflush(stdout);
                fflush(stdout);
                tcam.triger();
                tcam.process();
                return;
            };
            //different image, remember
            tcam.bimage[0]=image[i0];
            //frame acqured!
            //
        };
    */

    void timerEvent(QTimerEvent * event) {
        ros::spinOnce();
        if(!ros::ok()) {
            printf("quitting");
            fflush(stdout);
            exit(0);
        };
        ++rcount;
        if(rcount>=recali_time*loop_rate) {
            tcam.force_recali();
            //cam.get_image();
            rcount = 0;
        };
        //tcam.frame_acquired = false;
        tcam.get_image();
        //tcam.ready();
        ros::Time t1 = ros::Time::now();
        cam.get_image();
        tcam.get_image();//better synchronization with this
        tcam.filter_ir();
        printf("\b\b\b\b\b\b\b\b\b\b\b\b%4.4i  %4.4i  ",(count%10000),(count%10000));
        //printf("%4.4i  %4.4i  \n",(count%10000),(tcam.count%10000));
        fflush(stdout);
        ++count;
        //
        {
            // publish temperature image in the float 32 bit format
            sensor_msgs::ImagePtr msg(new sensor_msgs::Image());
            msg->header.stamp = t1;

            int num_bytes = tcam.ftemp.byte_size();
            msg->data.resize(num_bytes);
            memcpy(&msg->data[0],tcam.ftemp.begin(),num_bytes);
            msg->encoding = sensor_msgs::image_encodings::TYPE_32FC1;
            msg->step = tcam.ftemp.byte_stride(0,1);
            msg->height = tcam.ftemp.size()[1];
            msg->width = tcam.ftemp.size()[0];
            //msg->header.frame_id = ss.str().c_str();

            /*
            int num_bytes = tcam.int8ftemp.byte_size();
            //++tcam.count;
            msg->data.resize(num_bytes);
            memcpy(&msg->data[0],tcam.int8ftemp.begin(),num_bytes);
            msg->encoding = sensor_msgs::image_encodings::MONO8;
            msg->step = tcam.int8ftemp.byte_stride(0,1);
            msg->width = tcam.int8ftemp.size()[0];
            msg->height = tcam.int8ftemp.size()[1];
            msg->header.frame_id = "thermo_cam";
            */
            msg->header.seq = count;
            publisher_ir.publish(msg);
            //
            //
            openni_cam::thermo_info info;
            //std::stringstream ss;
            // ss << tcam.serial;
            info.serial = tcam.serial;
            info.header.stamp = t1;
            info.header.seq = count;
            info.header.frame_id = "";
            info.t_min = tcam.t_min;
            info.t_max = tcam.t_max;
            publisher_ir_info1.publish(info);
            {
                //publish filtered normalized temperature in the mono8 format
                /*
                int num_bytes = tcam.image_normalized.byte_size();
                //++tcam.count;
                sensor_msgs::ImagePtr msg(new sensor_msgs::Image());
                msg->header.stamp = t1;
                msg->data.resize(num_bytes);
                memcpy(&msg->data[0],tcam.image_normalized.begin(),num_bytes);
                msg->encoding = sensor_msgs::image_encodings::MONO8;
                msg->height = tcam.image_normalized.size()[1];
                msg->width = tcam.image_normalized.size()[0];
                msg->step = msg->width*sizeof(unsigned char);
                msg->header.frame_id = "thermo_camera";
                msg->header.seq = count;
                publisher_ir2.publish(msg);
                */
                /*
                sensor_msgs::ImagePtr msg(new sensor_msgs::Image());
                msg->header.stamp = t1;
                int num_bytes = cam.rgb.byte_size();
                msg->data.resize(num_bytes);
                memcpy(&msg->data[0],cam.rgb.begin(),num_bytes);
                msg->encoding = sensor_msgs::image_encodings::RGB8;
                msg->step = cam.rgb.stride(0,0,1);
                msg->width = cam.rgb.size()[1];
                msg->height = cam.rgb.size()[2];
                msg->header.frame_id = "thermo_camera";
                msg->header.seq = count;
                publisher_ir2.publish(msg);
                */
                //publish camera_info
                sensor_msgs::CameraInfoPtr ci(new sensor_msgs::CameraInfo());
                ci->header = msg->header;
                ci->height = msg->height;
                ci->width = msg->width;
                geom::matrix3 K;
                K << 0;
                //fow 45 deg
                double fx = 1.0/(msg->width/2);
                double fy = 1.0/(msg->height/2);
                K(0,0) = fx;
                K(1,1) = fy;
                K(0,2) = msg->width/2;
                K(1,2) = msg->height/2;
                K(2,2) = 1;
                for(int i=0; i<3; ++i) {
                    ci->P[i*4 + 3] = 0;
                    for(int j=0; j<3; ++j) {
                        ci->R[i*3+j] = (i==j);
                        ci->P[i*4+j] = K(i,j);
                        ci->K[i*3+j] = K(i,j);
                    };
                }
                ci->distortion_model = "";//"plumb_bob";
                //
                ci->roi.height = msg->height;
                ci->roi.width = msg->width;
                ci->binning_x = 1;
                ci->binning_y = 1;
                publisher_ir_info.publish(ci);
            }
        };
        //
        {
            // publish RGB image
            sensor_msgs::ImagePtr msg(new sensor_msgs::Image());
            msg->header.stamp = t1;
            int num_bytes = cam.rgb.byte_size();
            msg->data.resize(num_bytes);
            memcpy(&msg->data[0],cam.rgb.begin(),num_bytes);
            msg->encoding = sensor_msgs::image_encodings::RGB8;
            msg->step = cam.rgb.byte_stride(0,0,1);
            msg->width = cam.rgb.size()[1];
            msg->height = cam.rgb.size()[2];
            msg->header.frame_id = "openni_camera";
            msg->header.seq = count;
            publisher1.publish(msg);
            //
            //publish camera_info
            sensor_msgs::CameraInfoPtr ci(new sensor_msgs::CameraInfo());
            ci->header = msg->header;
            ci->height = msg->height;
            ci->width = msg->width;
            for(int i=0; i<3; ++i) {
                ci->P[i*4 + 3] = 0;
                for(int j=0; j<3; ++j) {
                    ci->R[i*3+j] = (i==j);
                    ci->P[i*4+j] = cam.K(i,j);
                    ci->K[i*3+j] = cam.K(i,j);
                };
            }
            ci->distortion_model = "";//"plumb_bob";
            //
            ci->roi.height = msg->height;
            ci->roi.width = msg->width;
            ci->binning_x = 1;
            ci->binning_y = 1;
            publisher_rgb_info.publish(ci);
            // publish tf
            //static tf::TransformBroadcaster br;
            //tf::Transform transform;
            //transform.setOrigin( tf::Vector3(0, 0.0, 0) );
            //transform.setRotation( tf::Quaternion(0, 0, 0) );
            //br.sendTransform(tf::StampedTransform(transform, msg->header.stamp, "", "/openni_camera"));
        };
        {
            // publish depth image
            sensor_msgs::ImagePtr msg(new sensor_msgs::Image());
            msg->header.stamp = t1;
            int num_bytes = cam.depth.byte_size();
            msg->data.resize(num_bytes);
            memcpy(&msg->data[0],cam.depth.begin(),num_bytes);
            msg->encoding = sensor_msgs::image_encodings::MONO16;
            msg->step = cam.depth.byte_stride(0,1);
            msg->width = cam.depth.size()[0];
            msg->height = cam.depth.size()[1];
            msg->header.frame_id = "openni_camera";
            msg->header.seq = count;
            publisher2.publish(msg);
        };
    };
    //
    //
    bool notify(QObject * receiver, QEvent * ev) {
        if(ev->type() == QEvent::KeyPress) {
            QKeyEvent * e = static_cast<QKeyEvent *>(ev);
            //QMessageBox* box = new QMessageBox();
            //box->setWindowTitle(QString("Hello"));
            //box->setText(QString("You Pressed: ")+ e->text());
            //box->show();
            if(e->key() == Qt::Key_Return) {
                printf("Return");
                fflush(stdout);
                return true;
            } else if(e->key() == Qt::Key_Tab) {
                printf("Tab");
                fflush(stdout);
                return true;
            } else if(e->key() == Qt::Key_Escape) {
                printf("ESC");
                fflush(stdout);
                exit(0);
                return true;
            };
        };
        return QApplication::notify(receiver,ev);
    };
};

myApp * a;

void cbOnFrame(unsigned short* image, unsigned int w, unsigned int h) {
    tcam.cbOnFrame(image,w,h);
};


int main(int argc, char **argv) {
    ros::init(argc, argv, "openni_node");
    ros::NodeHandle nh;
    ros::NodeHandle nhh("~");
    image_transport::ImageTransport it(nh);
    publisher1 = it.advertise("openni_camera/rgb", 1);
    publisher2 = it.advertise("openni_camera/depth", 1);
    publisher_ir = it.advertise("thermo_camera/ftemp", 1);
    publisher_ir_info = nh.advertise<sensor_msgs::CameraInfo>("/thermo_camera/camera_info", 1);
    publisher_ir_info1 = nh.advertise<openni_cam::thermo_info>("thermo_camera/info", 1);
    //publisher_ir2 = it.advertise("thermo_camera/image_normalized", 1);
    publisher_rgb_info = nh.advertise<sensor_msgs::CameraInfo>("openni_camera/camera_info", 1);
    //
    cam.init();
    tcam.init(cbOnFrame);
    //
    printf("openni_thermo_node %4.4i  %4.4i  ",0,0);
    fflush(stdout);
    //int loop_rate;
    //nhh.param<int>("loop_rate",loop_rate,5);
    a = new myApp(argc, argv, nhh);
    a->exec();
    delete a;
};