ros_openni_view.cpp

Go to the documentation of this file.

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <sensor_msgs/image_encodings.h>

//#include "IronPalette.h"

#include <QtGui/QApplication>
#include <QtGui/QLabel>
#include <QtCore/QBasicTimer>
#include <QtGui/QPainter>
#include <QtGui/QDialog>
#include <QtGui/QMainWindow>
//#include <QtGui/QMessageBox>
#include <QtGui/QKeyEvent>

#include <stdio.h>
#include "openni_view.h"
#include "openni_cam.h"

class myApp : public QApplication {
public:
    QView view;
    openni_cam::openni_cam cam;
public:
    myApp(int & argc, char **argv):QApplication(argc,argv){
        startTimer(1000/100);
    };
    //
    ~myApp() {
    };
    //
    void imageCallback1(const sensor_msgs::ImageConstPtr & msg) {//rgb
        //
        ++view.count;
        cam.rgb.resize(exttype::mint3(3,msg->width,msg->height));
        memcpy(cam.rgb.begin(),&msg->data[0],cam.rgb.byte_size());
    };
    void imageCallback2(const sensor_msgs::ImageConstPtr & msg) {//depth
        //
        cam.depth.resize(exttype::mint2(msg->width,msg->height));
        memcpy(cam.depth.begin(),&msg->data[0],cam.depth.byte_size());
    };
    //
    void timerEvent(QTimerEvent * event) {
        if(!ros::ok()) {
            //ros::shutdown();
            printf("openni_node quitting");
            fflush(stdout);
            exit(0);
        };
        if(!cam.depth.is_empty() && !cam.rgb.is_empty()){
            view.show(cam);
        };
        ros::spinOnce();
    };
    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_Space) {
                return true;
            } else if (e->key() == Qt::Key_Escape) {
                exit(0);
                return true;
            };
        };
        return QApplication::notify(receiver,ev);
    };
};

myApp *a;

void imageCallback1(const sensor_msgs::ImageConstPtr & msg) {
    //fprintf(stderr,"!");
    a->imageCallback1(msg);
};

void imageCallback2(const sensor_msgs::ImageConstPtr & msg) {
    //fprintf(stderr,"!");
    a->imageCallback2(msg);
};

int main(int argc, char **argv) {
    ros::init(argc, argv, "openni_view");
    ros::NodeHandle nh;
    //
    image_transport::ImageTransport it1(nh);
    image_transport::Subscriber sub1 = it1.subscribe("openni_camera/rgb", 1,imageCallback1);
    image_transport::Subscriber sub2 = it1.subscribe("openni_camera/depth", 1,imageCallback2);
    //
    a = new myApp(argc, argv);
    a->exec();
    delete a;
};