Subversion Repositories Code-Repo

#include "MainWindow.h"
#include "ImageWidget.h"
#include "OpenNI.h"
#include "DepthProcessor.h"
#include "RobotControl.h"

#include <QtWidgets>

MainWindow::MainWindow(QWidget *parent)
: QWidget(parent) {

        // Create the widgets that go on the GUI
        statusLabel = new QLabel();
        statusLabel->setFixedHeight(statusLabel->sizeHint().height());
        statusLabel->setText("Initializing OpenNI...");

        frameLabel = new QLabel();
        frameLabel->setFixedHeight(frameLabel->sizeHint().height());
        frameLabel->setText("Frame: ??");

        fpsLabel = new QLabel();
        fpsLabel->setFixedHeight(fpsLabel->sizeHint().height());
        fpsLabel->setText("FPS: ??");

        topLeftImage = new ImageWidget();
        topLeftImage->setSize(X_RES, Y_RES);
        topLeftImage->setText("Initializing OpenNI...");
        topRightImage = new ImageWidget();
        topRightImage->setSize(X_RES, Y_RES);
        topRightImage->setText("Initializing OpenNI...");
        botLeftImage = new ImageWidget();
        botLeftImage->setSize(X_RES, Y_RES);
        botLeftImage->setText("Initializing OpenNI...");
        botRightImage = new ImageWidget();
        botRightImage->setSize(X_RES, Y_RES);
        botRightImage->setText("Initializing OpenNI...");

        topLeftSelect = new QComboBox();
        topLeftSelect->setFocusPolicy(Qt::NoFocus);
        topRightSelect = new QComboBox();
        topRightSelect->setFocusPolicy(Qt::NoFocus);
        botLeftSelect = new QComboBox();
        botLeftSelect->setFocusPolicy(Qt::NoFocus);
        botRightSelect = new QComboBox();
        botRightSelect->setFocusPolicy(Qt::NoFocus);
        QStringList imageList;
        imageList.append("Raw Depth");
        imageList.append("Last Valid Depth");
        imageList.append("Movement Mask Raw Depth");
        imageList.append("Movement Mask Average Depth");
        imageList.append("Processed Depth");
        imageList.append("Movement Map");
        imageList.append("Raw Depth Horizon");
        imageList.append("Last Valid Horizon");
        imageList.append("Overlay Horizon");
        updateSelectionList(imageList);

        // Initialize the widgets for controlling the robot
        robot = new RobotControl();

        // Add all GUI widgets to a layout
        QGridLayout *mainLayout = new QGridLayout();
        mainLayout->addWidget(statusLabel, 0, 0, 1, 3);
        mainLayout->addWidget(frameLabel, 0, 5, 1, 1, Qt::AlignRight);
        mainLayout->addWidget(fpsLabel, 0, 6, 1, 1, Qt::AlignRight);

        mainLayout->addWidget(topLeftSelect, 1, 0, 1, 3);
        mainLayout->addWidget(topLeftImage, 2, 0, 1, 3);

        mainLayout->addWidget(topRightSelect, 1, 4, 1, 3);
        mainLayout->addWidget(topRightImage, 2, 4, 1, 3);

        mainLayout->addWidget(botLeftSelect, 3, 0, 1, 3);
        mainLayout->addWidget(botLeftImage, 4, 0, 1, 3);

        mainLayout->addWidget(botRightSelect, 3, 4, 1, 3);
        mainLayout->addWidget(botRightImage, 4, 4, 1, 3);


        mainLayout->addWidget(robot, 5, 0, 1, 7);

        setLayout(mainLayout);

        setWindowTitle("RGBD Depth Map");

        // Initialize the threads for pulling and processing depth data
        openNIThread = new OpenNI();
        openNIProcessor = new DepthProcessor();

        connect(topLeftSelect, SIGNAL(activated(QString)), this, SLOT(imageSelectionChanged(QString)));
        connect(topRightSelect, SIGNAL(activated(QString)), this, SLOT(imageSelectionChanged(QString)));
        connect(botLeftSelect, SIGNAL(activated(QString)), this, SLOT(imageSelectionChanged(QString)));
        connect(botRightSelect, SIGNAL(activated(QString)), this, SLOT(imageSelectionChanged(QString)));

        connect(openNIThread, SIGNAL(setStatusString(QString)), this, SLOT(updateStatusText(QString)));
        connect(openNIThread, SIGNAL(setFrameString(QString)), this, SLOT(updateFrameText(QString)));
        connect(openNIThread, SIGNAL(setFPSString(QString)), this, SLOT(updateFPSText(QString)));
        connect(openNIThread, SIGNAL(processDepthData(cv::Mat)), openNIProcessor, SLOT(processDepthData(cv::Mat)));
        connect(openNIThread, SIGNAL(sensorConnected()), this, SLOT(sensorConnected()));
        connect(openNIProcessor, SIGNAL(setImageTopLeft(QImage)), topLeftImage, SLOT(updateImage(QImage)));
        connect(openNIProcessor, SIGNAL(setImageTopRight(QImage)), topRightImage, SLOT(updateImage(QImage)));
        connect(openNIProcessor, SIGNAL(setImageBotLeft(QImage)), botLeftImage, SLOT(updateImage(QImage)));
        connect(openNIProcessor, SIGNAL(setImageBotRight(QImage)), botRightImage, SLOT(updateImage(QImage)));
        connect(openNIThread, SIGNAL(setFOV(float, float)), openNIProcessor, SLOT(setFOV(float, float)));
        connect(this, SIGNAL(changeDisplayImage(int, QString)), openNIProcessor, SLOT(setDisplayImage(int, QString)));

        // Start the thread for processing depth data from the sensor
        openNIProcessor->start(QThread::HighPriority);
        // Start the thread for reading depth data from the sensor
        openNIThread->start();

        setFixedSize(this->sizeHint());
}

MainWindow::~MainWindow() {
        delete(topLeftImage);
        delete(topRightImage);
        delete(botLeftImage);
        delete(botRightImage);

        delete(topLeftSelect);
        delete(topRightSelect);
        delete(botLeftSelect);
        delete(botRightSelect);

        delete(robot);

        delete(statusLabel);
        delete(frameLabel);
        delete(fpsLabel);

        openNIThread->terminate();
        openNIProcessor->terminate();
}

void MainWindow::updateStatusText(const QString &string) {
        statusLabel->setText(string);
}

void MainWindow::updateFrameText(const QString &string) {
        frameLabel->setText("Frame: " + string);
}

void MainWindow::updateFPSText(const QString &string) {
        fpsLabel->setText("FPS: " + string);
}

void MainWindow::updateSelectionList(const QStringList &list) {
        topLeftSelect->addItems(list);
        topRightSelect->addItems(list);
        botLeftSelect->addItems(list);
        botRightSelect->addItems(list);
}

void MainWindow::imageSelectionChanged(const QString &str) {
        if (sender() == topLeftSelect) {
                emit changeDisplayImage(0, str);
        } else if (sender() == topRightSelect) {
                emit changeDisplayImage(1, str);
        } else if (sender() == botLeftSelect) {
                emit changeDisplayImage(2, str);
        } else if (sender() == botRightSelect) {
                emit changeDisplayImage(3, str);
        }
}

void MainWindow::sensorConnected() {
        emit changeDisplayImage(0, "Raw Depth");
        emit changeDisplayImage(1, "Raw Depth Horizon");
        emit changeDisplayImage(2, "Processed Depth");
        emit changeDisplayImage(3, "Overlay Horizon");
        topLeftSelect->setCurrentIndex(0);
        topRightSelect->setCurrentIndex(6);
        botLeftSelect->setCurrentIndex(4);
        botRightSelect->setCurrentIndex(8);
}

void MainWindow::keyPressEvent(QKeyEvent *event) {
        // Send keypress events to the robot handing code
        robot->handleKeyPress(event);
}

void MainWindow::keyReleaseEvent(QKeyEvent *event) {
        // Send keypress events to the robot handing code
        robot->handleKeyRelease(event);
}