CVS2/code/hmi3/src/romeomainwindow.cpp

Go to the documentation of this file.

// Qt headers 
#include <stdio.h>
#include <QTimer>
#include <QDebug>
#include <QFile>
#include <QColorGroup>
#include <QImage>
#include "romeomainwindow.h"
#include <QTimeLine>

#include <iostream> // cout and endl functions 

// Trajectory headers
#include "../../common/trajectory/Trajectory.h"
#include "../../common/trajectory/LinearTrajectoryInterpolator.h"
#include "../../common/trajectory/TrajectoryUtil.h"

// Signals and Sparser headers
#include <sparser/all.h>
#include "signal.h"

// Yarp Header
#include <yarp/os/all.h>

// OpenCV Includes
#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <opencv/cxcore.h>  

#define X_INITIAL_PIXEL 38
#define Y_INITIAL_PIXEL 642

static const double Pi = 3.14159265358979323846264338327950288419717;
using namespace std;


RomeoMainWindow::RomeoMainWindow(QWidget* parent, Qt::WFlags fl)
: QMainWindow( parent, fl ), Ui::MainWindow()
{
        setupUi(this);
        initialValues();
        createActions();
        createGraphicsView();
        check_YARP_network();
        startYarpCommunications();
        statusbar->showMessage(tr("(INFO) - Ready"));
}

RomeoMainWindow::~RomeoMainWindow()
{
}


void RomeoMainWindow::initialValues()
{
        // Set SplashScreen
        QPixmap pixmap(":/images/GRVClogo2-1.jpg");
        splash = new QSplashScreen(pixmap);
        splash->show();

        // Set variables initial values

        x_ref=0;        // From EKFLOC
        y_ref=0;        // From EKFLOC
        
        orient_ref=0;   // From EKFLOC
        previous_orient_ref=0;

        speed_ref=0;    // From DCX
        curv_ref=0;     // From DCX

        yaw_ref=0;
        pitch_ref=0;
        roll_ref=0;
        
        x_ini_local=0;
        y_ini_local=0;
        
        x_ini_global=0;
        y_ini_global=0;

        purpurState=0;
        previousPurpurState=0;

        zoomPreviousValue=0;

        trajectory_points_number=0;
        trajectory_edit_mode=false;
        theta_edit_mode=false;
        trajectory_executing_mode=false;
        setting_initial_point=false;
        
        trajectoriesBYinterface=false;

        theta=0;
        previous_theta=0;
        theta_offset=0;
        
        map_x_axis_length=100;
        map_y_axis_length=100;

        yarpRunning=true;
        
        // Read Sensor Images FLAGS

        readLaser2DImage=false;
        
        readElevationMapImage=false;
        elevationMapMainView=false;
        
        readTransMapImage=false;
        transMapMainView=false;

        // Setting Buttons Initial Status
        
        elevationMap_SWITCH_VIEW_toolButton->setEnabled(false);
        transMap_SWITCH_VIEW_toolButton->setEnabled(false);

        laser2D_STOP_toolButton->setEnabled(false);
        elevationMap_STOP_toolButton->setEnabled(false);
        transMap_STOP_toolButton->setEnabled(false);
        
        launcher_STOP_toolButton->setEnabled(false);

        // CONTROL PROCESS

//      LAUNCHER_program="./launcher";
//      LAUNCHER_Process = new QProcess();
//      LAUNCHER_Process->setWorkingDirectory("../launcher/");
}


void RomeoMainWindow::createActions()
{
        // Time SLOTS

        QTimer *timer = new QTimer();
        connect(timer, SIGNAL(timeout()), this, SLOT(read_odometry_values()));
        timer->start(1000);
        
        QTimer *timer2 = new QTimer();
        connect(timer2, SIGNAL(timeout()), this, SLOT(read_system_status()));
        timer2->start(2000);

        // Construct a Time Line for Splash Screen and showing the Main Window
        QTimeLine *timeLine= new QTimeLine(100,this);
        connect(timeLine, SIGNAL(finished()),splash, SLOT(close()));
        connect(timeLine, SIGNAL(finished()),this, SLOT(show()));
        timeLine->start();

        // connect message signal from scene to showMessage slot
        connect(this, SIGNAL(message(QString)), this, SLOT(showMessage(QString)) );
        connect(&scene, SIGNAL(messagemap(QString)), this, SLOT(showMessage(QString)) );
        connect(&SelectTrajectory, SIGNAL(preloadTrajectory(QString)), this, SLOT(showTrajectory(QString)) );


        connect(&scene, SIGNAL(changeRobot(QPoint)), this, SLOT(changeRobotPosition(QPoint)) );
        connect(&scene,SIGNAL(ungrabRobot()),this,SLOT(ungrabRomeo()));
        connect(&scene,SIGNAL(mapScenePressEvent(QPointF)),this,SLOT(mapPressEvent(QPointF)));

        // SLOT and SIGNALS Connections for BUTTONS

        connect(aboutQt_toolButton, SIGNAL(clicked()), this, SLOT(aboutQt()));
        connect(about_RomeoHMI_toolButton, SIGNAL(clicked()), this, SLOT(about()));
        connect(EXIT_toolButton, SIGNAL(clicked()), this, SLOT(exitApplication()));
        connect(Zoom_verticalSlider, SIGNAL(valueChanged(int)), this, SLOT(zooming(int)));
        
        // Process Buttons
//      connect(launcher_START_toolButton, SIGNAL(clicked()), this, SLOT(launch_LAUNCHER_Process()));
//      connect(launcher_STOP_toolButton, SIGNAL(clicked()), this, SLOT(stop_LAUNCHER_Process()));
        
        // Sensor Buttons

        connect(laser2D_START_toolButton, SIGNAL(clicked()), this, SLOT(connect_LASER2D_sensor()));
        connect(laser2D_STOP_toolButton, SIGNAL(clicked()), this, SLOT(disconnect_LASER2D_sensor()));
        
        connect(elevationMap_START_toolButton, SIGNAL(clicked()), this, SLOT(connect_ELEVATIONMAP_sensor()));
        connect(elevationMap_STOP_toolButton, SIGNAL(clicked()), this, SLOT(disconnect_ELEVATIONMAP_sensor()));
        connect(elevationMap_SWITCH_VIEW_toolButton, SIGNAL(clicked()), this, SLOT(switchElevationView()));

        connect(transMap_START_toolButton, SIGNAL(clicked()), this, SLOT(connect_TRANSMAP_sensor()));
        connect(transMap_STOP_toolButton, SIGNAL(clicked()), this, SLOT(disconnect_TRANSMAP_sensor()));
        connect(transMap_SWITCH_VIEW_toolButton, SIGNAL(clicked()), this, SLOT(switchTransView()));

        // Control Buttons

        connect(INIT_toolButton, SIGNAL(clicked()), this, SLOT(initialtrajectorypoint()));
        connect(NEXT_toolButton, SIGNAL(clicked()), this, SLOT(nexttrajectorypoint()));
        connect(thetaInit_ToolButton, SIGNAL(clicked()), this, SLOT(changeThetaInit()));
        connect(loadMap_toolButton, SIGNAL(clicked()), this, SLOT(openMap()));
        connect(loadTrajectory_toolButton, SIGNAL(clicked()), this, SLOT(openTraject()));
        connect(trashTrajectory_toolButton, SIGNAL(clicked()), this, SLOT(trashtrajectory()));
        connect(launchTrajectorytoolButton, SIGNAL(clicked()), this, SLOT(launchEvent()));
        connect(PANIC_toolButton, SIGNAL(clicked()), this, SLOT(panicEvent())); 

        connect(LEFT_toolButton, SIGNAL(clicked()), this, SLOT(leftArrow()));
        connect(RIGHT_toolButton, SIGNAL(clicked()), this, SLOT(rightArrow()));
        connect(UP_toolButton, SIGNAL(clicked()), this, SLOT(upArrow()));
        connect(DOWN_toolButton, SIGNAL(clicked()), this, SLOT(downArrow()));


        // Creating and opening test_trajectory_file

        file.setFileName("trajectory.dat");
        if (file.open(QFile::WriteOnly)) 
        {
                test_trajectory_line.setDevice(&file);
        }
}

void RomeoMainWindow::check_YARP_network()
{
        statusbar->showMessage(tr("(INFO) - Checking Yarp Network ..."));

//      yarpRunning=yarp::os::Network::checkNetwork();
        yarpRunning=true;
        if(yarpRunning==true)
        {
                statusbar->showMessage(tr("(INFO) - Yarp OK ..."));
        }
        else
        {
                statusbar->showMessage(tr("(INFO) - Yarp Network DOWN..."));
        }
}


void RomeoMainWindow::startYarpCommunications()
{
        if(yarpRunning==true)
        {
                // Configure communications first of all
                yarp::os::Network::init();
        
                // open Odometry ports
//              locInput.open("/romeo/hmi/ekfloc6d/data");
                locInput.open("/romeo/hmi/ekfloc/data");
                odomInput.open("/romeo/hmi/dcx/data");

                // open Sensor ports
                laser2DImageInput.open("/romeo/hmi/laser2D/sensorimage");
                elevationMapImageInput.open("/romeo/hmi/sensormap/sensorimage");
                transMapImageInput.open("/romeo/hmi/transmap/sensorimage");

                // and make connections:
                yarp::os::Network::connect("/romeo/dcx/data","/romeo/hmi/dcx/data");
//              yarp::os::Network::connect("/romeo/ekfloc6d/data","/romeo/hmi/ekfloc6d/data");
                yarp::os::Network::connect("/romeo/ekfloc/data","/romeo/hmi/ekfloc/data");

                //purpur
                purpurStatePort.open("/romeo/hmi/purpur/state");
                yarp::os::Network::connect("/romeo/purpur/state","/romeo/hmi/purpur/state");
        }
}

void RomeoMainWindow::aboutQt()
{
        QMessageBox::aboutQt( this, "Qt About" );
}

void RomeoMainWindow::about()
{
        QMessageBox::about(this, tr("About Romeo Human Machine Interface 2.0"),
        tr("The Romeo HMI v 1.0 was developed by Daniel Perez Rodriguez "
        "member of <b>The Robotics, Vision and Control Group</b>, "
        "at the Engeneering High School of Seville."
        "<p>"
        "September 2009"));
}

void RomeoMainWindow::exitApplication()
{
        this->close();
}

void RomeoMainWindow::read_odometry_values()
{
        if( yarpRunning == true )       // Reading DATA form YARP Network or Not
        {       
//              if( pyarpPurpur=purpurStatePort.read(false) )   // Checking if purpur port is alive
//              {
//                      previousPurpurState=purpurState;
//                      purpurState = pyarpPurpur->getState();
//                      purpurState = 1;                        //Simulation
//              }
//              else
//              {
//                      yarp::os::Network::connect("/romeo/purpur/state","/romeo/hmi/purpur/state");
//              //      return;
//              }
// 
//              if ( purpurState == 0 ) // purpurState is IDLE
//              {
//                      x_ini_local=x_ref;      // Saving Last X coordinate ( with Romeo Stopped )
//                      y_ini_local=y_ref;      // Saving Last Y coordinate ( with Romeo Stopped )
//                      //theta=orient_ref*(-180/Pi);   // Saving Theta ( with Romeo Stopped )
//                      theta=(orient_ref*(-180/Pi))+theta_offset;      // Saving Theta ( with Romeo Stopped )
// //                   qDebug() << "X Ini FINAL: " << x_ini_local;
// //                   qDebug() << "Y Ini FINAL: " << y_ini_local;
// //                   qDebug() << "Theta FINAL: " << theta;
//                      initial_trajectory_point=romeo->scenePos();
//                      last_trajectory_point=romeo->scenePos();
// 
//                      if ( previousPurpurState == 1 )         // Trajectory has ben finalized
//                      {
//                              trajetoryFinalized();
//                      }
//              }

//              else            // purpurState is RUNNING
//              {       
                        pyarpDcx=odomInput.read(false); // Read from Yarp Buffers
                        pyarpEkf=locInput.read(false);
                
                        if(pyarpDcx!=NULL)      // Updating DCX values
                        {
                                speed_ref=pyarpDcx->dcxSpeed;
                                curv_ref=pyarpDcx->dcxCurv;
                        }
                        else
                        {
                                yarp::os::Network::connect("/romeo/dcx/data", "/romeo/hmi/dcx/data");
                        }
                
                        if(pyarpEkf!=NULL) // Updating Ekflok values
                        {
                                x_ref=pyarpEkf->x;              // Usink ekfloc6D or ekfloc
                                y_ref=pyarpEkf->y;              // Usink ekfloc6D or ekfloc
                                orient_ref=pyarpEkf->theta;     // Using ekfloc (not ekfloc6d)
//                              orient_ref=pyarpEkf->yaw;       // Usink ekfloc6D
//                              yaw_ref=pyarpEkf->yaw;          // Usink ekfloc6D
//                              pitch_ref=pyarpEkf->pitch;      // Usink ekfloc6D
//                              roll_ref=pyarpEkf->roll;        // Usink ekfloc6D
                                theta=orient_ref*(-180/Pi);
                        }
                        else
                        {
//                              yarp::os::Network::connect("/romeo/ekfloc6d/data", "/romeo/hmi/ekfloc6d/data");
                                yarp::os::Network::connect("/romeo/ekfloc/data", "/romeo/hmi/ekfloc/data");
                        }

                        XcoordinatelcdNumber->display(x_ref);   // Updating Displays
                        YcoordinatelcdNumber->display(y_ref);
                        YAWlcdNumber-> display(yaw_ref);
                        PITCHlcdNumber-> display(pitch_ref);
                        ROLLlcdNumber-> display(roll_ref);
                        speedlcdNumber->display(speed_ref);
                        curvatureDial->setProperty("value", (-100)*curv_ref);
        
                        // Updating Romeo position based in EKFLOC lectures 
                        
                        if (trajectory_edit_mode==false)
                        {
                                romeo->setPos(global_init_point.x()+(x_ref*map_X_scale),global_init_point.y()-(y_ref*map_Y_scale));
                                //romeo->setPos(romeo->pos().x()+(x_ref*map_X_scale),romeo->pos().y()-(y_ref*map_Y_scale));

                                romeo->rotate((orient_ref-previous_orient_ref)*(-180/Pi));
                        }
                
                        previous_orient_ref=orient_ref;
                        
        
                        // Reading LASER 2D - SENSOR Images
                
                        if( readLaser2DImage==true )
                        {
                                laser2DImage=laser2DImageInput.read(false);
                                
                                if(laser2DImage!=NULL)  // load image  
                                {
                                        if( (laserRange->scene()!=&laser2Dscene) && (romeoImage->scene()!=&laser2Dscene))
                                        {
                                                laserRange=laser2Dscene.addEllipse(QRectF(0,0,170,170));
                                                laserRange->setSpanAngle(2880);
                                                laserRange->setBrush(QColor(225,225,255,127));  // Semitransparent Laser
                                                laserRange->setPen(QPen(Qt::blue, 1, Qt::DotLine, Qt::FlatCap, Qt::RoundJoin));
                                                laserRange->setPos(0,65);
                                                                        
                                                romeoImage=laser2Dscene.addPixmap(QPixmap(":/images/romeo3.png"));
                                                romeoImage->setPos(90,150);
                                                romeoImage->setZValue(10);
                                        }
                                        
                                        cvLaser2DImage=(IplImage *)laser2DImage->getIplImage();
                                        IplImage2QImage(cvLaser2DImage,laser2DPixmap, &laser2Dscene);
                        
                                        laser2D_x_axis_length=laser2DPixmap->pixmap().width();
                                        laser2D_y_axis_length=laser2DPixmap->pixmap().height();
        
//                                      laser2DGraphicsView->fitInView (laser2DPixmap,Qt::IgnoreAspectRatio );
                                }
                                else
                                {
//                                      laser2Dscene.clear();           // Valid in Qt 4.4
                
                                        QList<QGraphicsItem *> list1 =laser2Dscene.items();
                                        
                                        foreach (QGraphicsItem *list, list1)    // Cleaning Actual Map
                                        {
                                                laser2Dscene.removeItem(list);
                                        }
                                
                                        laser2DInfoText=laser2Dscene.addText("Port Open\nBUT No Image\nReceived", infoFont);
                                        laser2DGraphicsView->resetMatrix();
                                        laser2DGraphicsView->centerOn(elevationMapInfoText);
                                        laser2Dscene.update();
                                }
                        }
                        else
                        {
                                //laser2Dscene.clear();         // Valid in Qt 4.4
        
                                QList<QGraphicsItem *> list1 =laser2Dscene.items();
                                
                                foreach (QGraphicsItem *list, list1)    // Cleaning Actual Map
                                {
                                        laser2Dscene.removeItem(list);
                                }
        
                                laser2DInfoText=laser2Dscene.addText(" - DISABLED -", infoFont);
                                laser2DGraphicsView->resetMatrix();
                                laser2DGraphicsView->centerOn(laser2DInfoText);
                                laser2Dscene.update();
                        }
                
                        // Reading ELEVATION MAP
        
                        if( readElevationMapImage==true )
                        {
                                elevationMapImage=elevationMapImageInput.read(false);
                                
                                if(elevationMapImage!=NULL)     // load image  
                                {
                                        cvelevationMapImage=(IplImage *)elevationMapImage->getIplImage();
                                        IplImage2QImage(cvelevationMapImage,elevationMapPixmap, &elevationMapscene);
                                        elevationMap_SWITCH_VIEW_toolButton->setEnabled(true);
        
                                        elevationMap_x_axis_length=elevationMapPixmap->pixmap().width();
                                        elevationMap_y_axis_length=elevationMapPixmap->pixmap().height();
        
                                        if( elevationMapMainView==false )
                                        {       elevationMapGraphicsView->fitInView (elevationMapPixmap,Qt::IgnoreAspectRatio );
                                        }
                                }
                                else
                                {
        //                              elevationMapscene.clear();      // Valid in Qt 4.4
                
                                        QList<QGraphicsItem *> list2 =elevationMapscene.items();
                                        
                                        foreach (QGraphicsItem *list, list2)    // Cleaning Actual Map
                                        {
                                                elevationMapscene.removeItem(list);
                                        }
        
                                        elevationMapInfoText=elevationMapscene.addText("Port Open\nBUT No Image\nReceived", infoFont);
                                        elevationMapGraphicsView->resetMatrix();
                                        elevationMapGraphicsView->centerOn(elevationMapInfoText);
                                        elevationMapscene.update();
                                        elevationMapMainView=false;
                                        elevationMap_SWITCH_VIEW_toolButton->setEnabled(false);
                                }
                        }
                        else
                        {
        //                      elevationMapscene.clear();      // Valid in Qt 4.4
        
                                QList<QGraphicsItem *> list2 =elevationMapscene.items();
                                
                                foreach (QGraphicsItem *list, list2)    // Cleaning Actual Map
                                {
                                        elevationMapscene.removeItem(list);
                                }
        
                                elevationMapInfoText=elevationMapscene.addText(" - DISABLED -", infoFont);
                                elevationMapGraphicsView->resetMatrix();
                                elevationMapGraphicsView->centerOn(elevationMapInfoText);
                                elevationMapMainView=false;                     
                                elevationMap_SWITCH_VIEW_toolButton->setEnabled(false);
                                elevationMapscene.update();
                        }
        
                        // Reading TRANSVERSABILITY MAP
                        
                        if( readTransMapImage==true )
                        {
                                transMapImage=transMapImageInput.read(false);
                                
                                if(transMapImage!=NULL) // load image  
                                {
                                        cvTransMapImage=(IplImage *)transMapImage->getIplImage();
                                        IplImage2QImage(cvTransMapImage,transMapPixmap, &transMapscene);
                                        transMap_SWITCH_VIEW_toolButton->setEnabled(true);
        
                                        transMap_x_axis_length=elevationMapPixmap->pixmap().width();
                                        transMap_y_axis_length=elevationMapPixmap->pixmap().height();
                                        
                                        if( transMapMainView==false )
                                        {       transMapGraphicsView->fitInView (transMapPixmap,Qt::IgnoreAspectRatio );
                                        }
        
                                }
                                else
                                {
        //                              transMapscene.clear();
        
                                        QList<QGraphicsItem *> list3 =transMapscene.items();
        
                                        foreach (QGraphicsItem *list, list3)    
                                        {
                                                transMapscene.removeItem(list);
                                        }
        
                                        transMapInfoText=transMapscene.addText("Port Open\nBUT No Image\nReceived", infoFont);
                                        transMapGraphicsView->resetMatrix();
                                        transMapGraphicsView->centerOn(transMapInfoText);
                                        transMapscene.update();
                                        transMapMainView=false;
                                        transMap_SWITCH_VIEW_toolButton->setEnabled(false);
                                }
                        }
                        else
                        {
        //                      transMapscene.clear();  // Valid in Qt 4.4
                                
                                QList<QGraphicsItem *> list3 =transMapscene.items();
        
                                foreach (QGraphicsItem *list, list3)    
                                {
                                        transMapscene.removeItem(list);
                                }
        
                                transMapInfoText=transMapscene.addText(" - DISABLED -", infoFont);
                                transMapGraphicsView->resetMatrix();
                                transMapGraphicsView->centerOn(transMapInfoText);
                                transMapMainView=false;
                                transMap_SWITCH_VIEW_toolButton->setEnabled(false);
                                transMapscene.update();
                        }

//              }       // if ( purpurState == IDLE ) -> ELSE

        }       // if( yarpRunning == true )
}

void RomeoMainWindow::closeEvent(QCloseEvent *event)
{
        int ret = QMessageBox::question(this, tr("ROMEO HMI v 2.0"),
                                tr("Do you really want to exit ?\n"),
                                QMessageBox::Yes | QMessageBox::No );

        if (ret == QMessageBox::Yes)
        {
                emit message(QString("Closing application ..."));
                stop_ALL_CONTROL_Process();
                yarp::os::Network::fini();
                event->accept();
        }
        else if (ret == QMessageBox::No)
        {       
                event->ignore();
        }
}

void RomeoMainWindow::openMap()
{
        QString mapName = QFileDialog::getOpenFileName(this, tr("Open Map"), "../src/images/maps", tr("Map Files (*.png *.jpg *.bmp)"));

        if (!mapName.isEmpty())
        {
                loadMap(mapName);
                statusbar->showMessage(tr("(INFO) - New map loaded"));

                // Set MAP Real Scale
                if(MapScale.exec() == QDialog::Accepted)
                {
                        map_x_axis_length=MapScale.spinBox_X_Dimension->value();
                        map_y_axis_length=MapScale.spinBox_Y_Dimension->value();
                }
        }
}
void RomeoMainWindow::loadMap(QString mapName)
{
        foreach (QGraphicsPixmapItem *actualMap, maps)  // Cleaning Actual Map
        {
                scene.removeItem(actualMap);
        }
        
        map=QPixmap(QString("%1").arg(mapName));
        map.setMask(map.createMaskFromColor(Qt::black, Qt::MaskOutColor));
        
        
        QGraphicsPixmapItem *mapPoint=scene.addPixmap(map);
        scene.update();
        maps.append(mapPoint);
        mapPoint->setTransformationMode(Qt::SmoothTransformation);

        map_width=map.width();
        map_height=map.height();
        
        map_X_scale=map.width()/map_x_axis_length;   // SCALE in  pixeles / m 
        map_Y_scale=map.width()/map_y_axis_length;   // SCALE in pixeles / m 

        MapGraphicsView->setSceneRect(0,0, map_width, map_height);
        zoomFit();
        trashtrajectory();

        loadSemanticSpots(mapName);
}



void RomeoMainWindow::openTraject()
{
        if( trajectoriesBYinterface == true )
        {
                QDir dir;
                QString trajectoryName;
                QString path=dir.currentPath();

                SelectTrajectory.move(0,0);
                
                int Result = SelectTrajectory.exec();

                if(Result == QDialog::Accepted)
                {
                        if(!trajectoryName.isEmpty())
                        {
                                trajectoryName=SelectTrajectory.trajectory;                     
                                trajectoryName.prepend("/src/images/trajectories/UPC/");
                                trajectoryName.prepend(path);
                                loadTrajectory(trajectoryName);
                                statusbar->showMessage(tr("(INFO) - New trajectory loaded: %1").arg(trajectoryName) );
                        }
                }       

                if(Result == QDialog::Rejected)
                {
                        trashtrajectory();
                        trajectoryName.clear();
                        statusbar->showMessage(tr("(WARN) - NO trajectory loaded.") );
                }

        }
        else
        {
                QString trajectoryName = QFileDialog::getOpenFileName(this, tr("Open Trajectory"), "../src/images/trajectories", tr("Map Files (*.dat)"));
                
                if (!trajectoryName.isEmpty())
                {
                        loadTrajectory(trajectoryName);
                        statusbar->showMessage(tr("(INFO) - New trajectory loaded"));
                }
        }
}

void RomeoMainWindow::zooming(int value)
{
        double dif;

        dif=value-zoomPreviousValue;

        if(dif>0)
        {
                MapGraphicsView->scale(1.2,1.2);
                statusbar->showMessage(tr("(INFO) - Zoom IN 10%"));
        }
        else if(dif<0)
        {
                MapGraphicsView->scale(0.8,0.8);
                statusbar->showMessage(tr("(INFO) - Zoom OUT 10%"));
        }
        
        zoomPreviousValue=value;
        
        update();
}

void RomeoMainWindow::zoomFit()
{
        zoomPreviousValue=0;
        Zoom_verticalSlider->setValue(0);
        MapGraphicsView->resetMatrix();
        MapGraphicsView->scale(0.5,0.5);
        scene.update();
}


void RomeoMainWindow::read_system_status()
{
        // READING CPU and MEMORY STATUS

        QFile file_stats("/proc/stat", this);
        if ( !file_stats.open(QIODevice::ReadOnly | QIODevice::Text) ) 
        {
                qDebug() << "File /proc/stat could not be opened";
                return;
        }

        QString cpu_line = file_stats.readLine();
        QStringList list1 = cpu_line.split(" ");

        for ( int i=0; i < 4; i++)
        {
                 m_stats_new[i]=list1[i+1].toDouble();
        }       

        double user = m_stats_new[0]- m_stats[0];
        double nice = m_stats_new[1]- m_stats[1];
        double system = m_stats_new[2]- m_stats[2];

        double cpu_usage = (user + system + nice);

        for (int  i=0; i < 4; i++)
        {
                m_stats[i] = m_stats_new[i];
        }
        
        CPUprogressBar->setValue(cpu_usage);
        file_stats.close();
        
        // READING MEMORY STATUS - QProgressBar *memoryProgressBar;

        QFile mem_stats("/proc/meminfo", this);
        if ( !mem_stats.open(QIODevice::ReadOnly | QIODevice::Text) )
        {
                qDebug() << "File /proc/stat could not be opened";
                return;
        }
        
        QString mem_line = mem_stats.readLine();
        QStringList list2 = mem_line.split(QRegExp("\\W+"), QString::SkipEmptyParts);

        double MemTotal=list2[1].toDouble();

        mem_stats.seek(26);     // Positioning at second line
        mem_line = mem_stats.readLine();
        
        list2 = mem_line.split(QRegExp("\\W+"), QString::SkipEmptyParts);

        double MemFree=list2[1].toDouble();
        
        double mem_usage=((MemTotal-MemFree)/MemTotal)*100;

        memoryProgressBar->setValue(mem_usage);
        mem_stats.close();
        
        // READING NETWORK STATUS
        
        double tx_stats;
        double rx_stats;

        QFile net_stats("/proc/net/dev", this);
        if ( !net_stats.open(QIODevice::ReadOnly | QIODevice::Text) )
        {
                qDebug() << "File /proc/net/dev could not be opened";
                return;
        }

        for (int i=0;i<4;i++)
        {
                QString net_line = net_stats.readLine();
        
                QStringList list3 = net_line.split(QRegExp("\\W+"), QString::SkipEmptyParts);
                
                if(list3[0]=="eth0" )
                {
                        tx_net_stats_new=list3[9].toDouble();
                        rx_net_stats_new=list3[1].toDouble();

                        tx_stats = (tx_net_stats_new - tx_net_stats)/2048;
                        rx_stats = (rx_net_stats_new - rx_net_stats)/2048;
        
                        tx_net_stats=tx_net_stats_new;
                        rx_net_stats=rx_net_stats_new;
                }
        }
        
        networkTX_progressBar->setValue(tx_stats);
        networkRX_progressBar->setValue(rx_stats);
        
        net_stats.close();
}



void RomeoMainWindow::createGraphicsView()
{
        // Defining Info Font

        infoFont.setStyle(QFont::StyleItalic);
        infoFont.setStyleHint(QFont::System);
        infoFont.setWeight(QFont::Bold);
        infoFont.setPointSize(10);

        // GRAPHIC VIEW
        
        MapGraphicsView->setInteractive(true);  
        MapGraphicsView->setRenderHint(QPainter::Antialiasing);
        MapGraphicsView->setDragMode(QGraphicsView::ScrollHandDrag);
        MapGraphicsView->setCacheMode(QGraphicsView::CacheBackground);

        MapGraphicsView->setEnabled(true);
        MapGraphicsView->setScene(&scene);
        
        romeo=&romeo_local;
        romeo->setPixmap(QPixmap(":/images/romeo3.png"));
        romeo->setTransformationMode(Qt::SmoothTransformation);
        romeo->setAcceptedMouseButtons(Qt::LeftButton);
        romeo->setAcceptDrops(true);
        romeo->setEnabled(true);

        romeo->setPos(X_INITIAL_PIXEL,Y_INITIAL_PIXEL);

        // SCALING ROMEO
        //TODO

        romeo->setZValue(10);   // Representing Romeo at TOP

        last_trajectory_point=romeo->scenePos();
        initial_trajectory_point=romeo->scenePos();

        qDebug() << "Initial Trajectory Point: " << initial_trajectory_point;

        global_init_point=romeo->scenePos();

        loadMap(QString("src/images/maps/upc2.png"));
        
        x_ini_global=romeo->pos().x();
        y_ini_global=romeo->pos().y();

        qDebug() << "X ini Global: " << x_ini_global << " [pixels]";
        qDebug() << "Y ini Global: " << y_ini_global << " [pixels]";

        graphicview2map(&x_ini_global,&y_ini_global);

        qDebug() << "X ini Global: " << x_ini_global << " [meters]";
        qDebug() << "Y ini Global: " << y_ini_global << " [meters]";

        scene.addItem(romeo);
        zoomFit();
        scene.update();

        // Initializing Laser 2D Graphics View
        
        laser2DGraphicsView->setScene(&laser2Dscene);
        laser2DGraphicsView->setRenderHint(QPainter::Antialiasing);
        laser2DGraphicsView->setCacheMode(QGraphicsView::CacheBackground);
        laser2DGraphicsView->setVerticalScrollBarPolicy(Qt::ScrollBarAlwaysOff);
        laser2DGraphicsView->setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOff);
        laser2DGraphicsView->setDragMode(QGraphicsView::NoDrag);

        laser2DPixmap=&laser2DPixmap_local;
        
        laser2DInfoText=laser2Dscene.addText(" - DISABLED -",infoFont);
        
        laser2Dscene.update();

        // Initializing Elevation Map Graphics View

        elevationMapGraphicsView->setScene(&elevationMapscene);
        elevationMapGraphicsView->setRenderHint(QPainter::Antialiasing);
        elevationMapGraphicsView->setCacheMode(QGraphicsView::CacheBackground);
        elevationMapGraphicsView->setVerticalScrollBarPolicy(Qt::ScrollBarAlwaysOff);
        elevationMapGraphicsView->setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOff);
        elevationMapGraphicsView->setDragMode(QGraphicsView::NoDrag);

        elevationMapPixmap=&elevationMapPixmap_local;
        elevationMapInfoText=elevationMapscene.addText(" - DISABLED -",infoFont);
        elevationMapscene.update();

        // Initializing Transversability Map Graphics View

        transMapGraphicsView->setScene(&transMapscene);
        transMapGraphicsView->setRenderHint(QPainter::Antialiasing);
        transMapGraphicsView->setCacheMode(QGraphicsView::CacheBackground);
        transMapGraphicsView->setVerticalScrollBarPolicy(Qt::ScrollBarAlwaysOff);
        transMapGraphicsView->setHorizontalScrollBarPolicy(Qt::ScrollBarAlwaysOff);
        transMapGraphicsView->setDragMode(QGraphicsView::NoDrag);

        transMapPixmap=&transMapPixmap_local;
        transMapInfoText=transMapscene.addText(" - DISABLED -",infoFont);
        transMapscene.update();
}

void RomeoMainWindow::loadTrajectory(QString trajectoryName)
{
        QFile file;

        double x;
        double y;
        double speed;
        char endline;

        QPointF nextPoint;
        
        file.setFileName(trajectoryName);
        
        if ( file.open(QFile::ReadOnly) )
        {       
                trashtrajectory();
                QTextStream in(&file);
                
                while ( !in.atEnd() )
                {
                        in >> x;
                        in >> y;
                        in >> speed;
                        in >> endline;

                        romeo2graphicview(&x,&y);
        
                        qDebug() << "X: " << x;
                        qDebug() << "Y: " << y;
        
                        nextPoint.setX(x);
                        nextPoint.setY(y);
                
                        trajectory_edit_mode=true;
                        emit mapPressEvent( nextPoint );        // Adding the Point     
                }
                file.close();
                trajectory_edit_mode=false;
        }
}


void RomeoMainWindow::loadSemanticSpots(QString mapName)
{
        foreach (QGraphicsItem *list, semanticSpotsList)        // Cleaning Semantic Spots List
        {
                scene.removeItem(list);
        }

        foreach (QGraphicsItem *list, semanticSpotsText)        // Cleaning Semantic Spots Text
        {
                scene.removeItem(list);
        }

        foreach (QGraphicsItem *list, semanticInfo)     // Cleaning Semantic Info
        {
                scene.removeItem(list);
        }
        
        char type[50];
        char name[50];
        double x;
        double y;
        char endline;
        int nspots=0;

        QGraphicsTextItem *info[15];
        QGraphicsTextItem *spotNames[15];
        QGraphicsEllipseItem *semanticSpots[15];
        
        QFont coordinates_font("Arial", 8, QFont::Bold);
        QFont info_font1("Arial", 20, QFont::Bold);
        
        QFile fileSemanticPoints;
        fileSemanticPoints.setFileName(QString("%1.semanticMapList.txt").arg(mapName));

        QFile fileSemanticInfo;
        fileSemanticInfo.setFileName(QString("%1.semanticInfoList.txt").arg(mapName));

        
        if ( fileSemanticInfo.open(QFile::ReadOnly) )
        {       
                QTextStream in(&fileSemanticInfo);
                
                while ( !in.atEnd() )
                {
                        in >> type;
                        in >> name;
                        in >> x;
                        in >> y;
                        in >> endline;
                
                        info[nspots]=scene.addText(QString("%1").arg(name),info_font1);
                        info[nspots]->setDefaultTextColor(Qt::lightGray);
                        info[nspots]->setPos(x,y);
                        semanticInfo.append(info[nspots]);
                        nspots++;
                        
                }
                fileSemanticInfo.close();
        }


        if ( fileSemanticPoints.open(QFile::ReadOnly) )
        {       
                nspots=0;

                QTextStream in(&fileSemanticPoints);
                
                while ( !in.atEnd() )
                {
                        in >> name;
                        in >> x;
                        in >> y;
                        in >> endline;
                
                        map2graphicview(&x,&y);

                        semanticSpots[nspots]=scene.addEllipse(QRectF(0,0,10,10));
                        semanticSpots[nspots]->setPos(x,y-10);
                        semanticSpots[nspots]->setBrush(Qt::darkGreen);
                        semanticSpotsList.append(semanticSpots[nspots]);

                        spotNames[nspots]=scene.addText(QString("%1").arg(name),coordinates_font);
                        spotNames[nspots]->setDefaultTextColor(Qt::darkGray);
                        spotNames[nspots]->setPos(x-15,y);
                        semanticSpotsText.append(spotNames[nspots]);

                        nspots++;
                }
                fileSemanticPoints.close();
        }
}




void RomeoMainWindow::trashtrajectory()
{
        trajectory_edit_mode=false;
        trajectory_executing_mode=false;        // Just in case
        romeo->setAcceptedMouseButtons(0);
        
        setCursor(Qt::ArrowCursor);
        MapGraphicsView->setCursor(Qt::ArrowCursor);
        romeo->setCursor(Qt::ArrowCursor);
        romeo->setAcceptedMouseButtons(Qt::LeftButton);
        emit message("Trajectory Trashed");

        trajectory_points_number=0;

        // Closing and deleting trajectory File 
        file.remove();
        
        // Creating a new trajectory file
        file.setFileName("trajectory.dat"); 

        if (file.open(QFile::WriteOnly)) 
        {
                test_trajectory_line.setDevice(&file);
        }

        // Cleaning the SCREEN of SPOTS and LINES

//      last_trajectory_point=QPointF(romeo->scenePos().x()+12,romeo->scenePos().y() );
//      initial_trajectory_point=QPointF(romeo->scenePos().x()+12,romeo->scenePos().y() );

        last_trajectory_point=QPointF(romeo->scenePos().x(),romeo->scenePos().y() );
        initial_trajectory_point=QPointF(romeo->scenePos().x(),romeo->scenePos().y() );

        trajectory_edit_mode=false;

        foreach (QGraphicsEllipseItem *spot, spots)     // Cleaning SPOTS
        {
                scene.removeItem(spot);
        }
        
        foreach (QGraphicsLineItem *line, lines)        // Cleaning Lines
        {
                scene.removeItem(line);
        }

        foreach (QGraphicsLineItem *line, absolutLines) // Cleaning Lines
        {
                scene.removeItem(line);
        }

        foreach (QGraphicsTextItem *text, waypoints)    // Cleaning trajectory coordinates
        {
                scene.removeItem(text);
        }

        foreach (QGraphicsTextItem *text, absolutWaypoints)     // Cleaning trajectory coordinates
        {
                scene.removeItem(text);
        }

        trajectory_lines.clear();       // Cleaning the list of trajectory points

        statusbar->showMessage(tr("(INFO) - Trajectory Crashed."));
}




void RomeoMainWindow::graphicview2romeo( double *x_graph,double *y_graph ) // Pixels to Meters
{

        graphicview2map(x_graph, y_graph);

        (*x_graph)-=x_ini_global;
        (*y_graph)-=y_ini_global;
        
        
        
        


// //   Traslation  ( with Y inversion )
// 
//      (*x_graph)=  (*x_graph) - romeo->pos().x();
//      (*y_graph)=  romeo->pos().y() - (*y_graph);
// 
//      double x_graph_ini = *x_graph;
//      double y_graph_ini = *y_graph;
// 
// //   Rotation        
// 
// 
//      (*x_graph)= (x_graph_ini)*cos(theta*(Pi/180))+(y_graph_ini)*sin(theta*(Pi/180));
//      (*y_graph)= (y_graph_ini)*cos(theta*(Pi/180))-(x_graph_ini)*sin(theta*(Pi/180));
// //   (*x_graph)= (x_graph_ini)*cos(theta*(Pi/180))-(y_graph_ini)*sin(theta*(Pi/180));
// //   (*y_graph)= (x_graph_ini)*cos(theta*(Pi/180))+(y_graph_ini)*sin(theta*(Pi/180));
// //   (*x_graph)= (x_graph_ini)*cos((Pi/2)-theta*(Pi/180))+(y_graph_ini)*sin((Pi/2)-theta*(Pi/180));
// //   (*y_graph)= (y_graph_ini)*cos((Pi/2)-theta*(Pi/180))-(x_graph_ini)*sin((Pi/2)-theta*(Pi/180));
// 
// 
// //   Scale
// 
//      (*x_graph)=(*x_graph)/map_X_scale;
//      (*y_graph)=(*y_graph)/map_Y_scale;
}

void RomeoMainWindow::map2graphicview(double *x,double *y)
{
        (*x)= (*x);
        (*y)= map_y_axis_length-(*y);

//      Scale in Pixels for Graphic View
        (*x)=(*x)*map_X_scale;
        (*y)=(*y)*map_Y_scale;

}


void RomeoMainWindow::graphicview2map(double *x, double *y)
{
        (*x)=(*x)/map_X_scale;
        (*y)=(map_height-(*y))/map_Y_scale; 
}


void RomeoMainWindow::romeo2graphicview( double *x_graph,double *y_graph ) // Meters to Pixels
{
        double x_graph_ini = *x_graph;
        double y_graph_ini = *y_graph;

//      Rotation        

        (*x_graph)= (x_graph_ini)*cos(theta*(Pi/180))+(y_graph_ini)*sin(theta*(Pi/180));
        (*y_graph)= (y_graph_ini)*cos(theta*(Pi/180))-(x_graph_ini)*sin(theta*(Pi/180));

//      Y inversion 
        (*y_graph)=(*y_graph)*(-1);

//      Scale
        (*x_graph)=(*x_graph)*map_X_scale;
        (*y_graph)=(*y_graph)*map_Y_scale;

//      Traslation

        (*x_graph)= (*x_graph) + romeo->pos().x();
        (*y_graph)= (*y_graph) + romeo->pos().y();

}



void RomeoMainWindow:: mapPressEvent(QPointF clickedPoint)
{
        qreal x = clickedPoint.x();
        qreal y = clickedPoint.y();
        
        QPen pen1;
        pen1.setStyle(Qt::DashDotLine);
        pen1.setWidth(2);
        pen1.setBrush(Qt::blue);
        pen1.setCapStyle(Qt::RoundCap);
        pen1.setJoinStyle(Qt::RoundJoin);


        QPen pen2;
        pen2.setStyle(Qt::DotLine);
        pen2.setWidth(1);
        pen2.setBrush(Qt::darkGreen);
        pen2.setCapStyle(Qt::RoundCap);
        pen2.setJoinStyle(Qt::RoundJoin);


        emit message( QString("Mouse MAP PRESS Position at [ %1  -  %2 ]").arg(x).arg(y) );

        if(trajectory_edit_mode==true)
        {
                if(setting_initial_point==true)
                {
//                      romeo->setPos(clickedPoint.x()-12,clickedPoint.y()-17);
                        romeo->setPos(clickedPoint.x(),clickedPoint.y());
                        
                        global_init_point=romeo->scenePos();

                        x_ini_global=romeo->pos().x();
                        y_ini_global=romeo->pos().y();

                        graphicview2map(&x_ini_global,&y_ini_global);
                        qDebug() << "Romeo Globlal Init Pos X: " << x_ini_global << " [meters]";
                        qDebug() << "Romeo Globlal Init Pos Y: " << y_ini_global << " [meters]";

                        trashtrajectory();
                }
                else
                {
                        QGraphicsEllipseItem *spot;
                        QGraphicsTextItem *pointCoordinates;
                        QGraphicsTextItem *absolutPointCoordinates;
                        QFont coordinates_font1("Arial", 8, QFont::Bold);
                        QFont coordinates_font2("Arial", 8, QFont::Bold);
        
                        // Adding the SPOT
                        
                        spot=scene.addEllipse(QRectF(0,0,8,8));
                        spot->setBrush(Qt::red);
        
                        spot->setPos(clickedPoint.x()-4,clickedPoint.y()-4);
                        
                        trajectory_points_number++;
                        spots.append(spot);
                        
                        // Adding the line between last 2 points
        
                        lines.append(scene.addLine(last_trajectory_point.x(),last_trajectory_point.y(),clickedPoint.x(),clickedPoint.y(),pen1));
                        absolutLines.append(scene.addLine(initial_trajectory_point.x(),initial_trajectory_point.y(),clickedPoint.x(),clickedPoint.y(),pen2));

                        // SCALING PiXELS TO METERS

                        double x_meters=x;
                        double y_meters=y;

                        qDebug() << "x meters (real): " << x_meters << "x meters (double): " << x;
                        qDebug() << "y meters (real): " << x_meters << "y meters (double): " << y;
        
                        // Adding Text with Point coordinates in meters to the scene

                        graphicview2romeo(&x_meters,&y_meters);

                        pointCoordinates=scene.addText(QString("[%1 , %2 ]").arg(x_meters-x_ini_local).arg(y_meters-y_ini_local),coordinates_font1);
                        pointCoordinates->setDefaultTextColor(Qt::darkRed);
                        pointCoordinates->setPos(clickedPoint.x(),clickedPoint.y());
                        waypoints.append(pointCoordinates);

                        absolutPointCoordinates=scene.addText(QString("[%1 , %2 ]").arg(x_meters).arg(y_meters),coordinates_font2);
                        absolutPointCoordinates->setDefaultTextColor(Qt::darkGreen);
                        absolutPointCoordinates->setPos(clickedPoint.x(),clickedPoint.y()+10);
                        absolutWaypoints.append(absolutPointCoordinates);

                        // Saving Last Point
                        last_trajectory_point=clickedPoint;
        
                        // Adding Line to Test Trajectory File  
                        trajectory_lines.append(QString("%1\t%2\t1\n").arg(x_meters).arg(y_meters ));
                        qDebug() << "Final Point Coordinate: "<< x_meters << " , " << y_meters;
                }
        }

        MapGraphicsView->setDragMode(QGraphicsView::ScrollHandDrag);
        trajectory_edit_mode=false;
        setting_initial_point=false;
}



void  RomeoMainWindow::showMessage(QString msg)
{
        statusBar()->showMessage(msg);  // display message on main window status bar
}


void  RomeoMainWindow::showTrajectory(QString file)
{
        QDir dir;
        QString trajectoryName;
        QString path=dir.currentPath();

        trajectoryName=file;                    
        trajectoryName.prepend("/src/images/trajectories/UPC/");
        trajectoryName.prepend(path);                           

        loadTrajectory(trajectoryName);
}





void RomeoMainWindow::changeRobotPosition(QPoint position)
{
        romeo->setPos(position.x(),position.y());
}

void RomeoMainWindow::ungrabRomeo()
{
//      romeo->ungrabMouse();           // Valid in Qt 4.4
        last_trajectory_point=romeo->scenePos();
        initial_trajectory_point=romeo->scenePos();
        trajectory_edit_mode=false;
        trashtrajectory();
}


void RomeoMainWindow::launchEvent()
{       
        if (trajectory_points_number==0)
        {
                QMessageBox::warning(this, tr("ROMEO HMI v 2.0"),
                            tr("The trajectory is empty."));
                return;
        }
        else
        {
                // Disabling Buttons
                INIT_toolButton->setEnabled(FALSE);
                NEXT_toolButton->setEnabled(FALSE);
                trashTrajectory_toolButton->setEnabled(FALSE);
                LEFT_toolButton->setEnabled(FALSE);
                RIGHT_toolButton->setEnabled(FALSE);
                UP_toolButton->setEnabled(FALSE);
                DOWN_toolButton->setEnabled(FALSE);
                launchTrajectorytoolButton->setEnabled(FALSE);
                loadTrajectory_toolButton->setEnabled(FALSE);
                loadMap_toolButton->setEnabled(FALSE);
                thetaInit_ToolButton->setEnabled(FALSE);

                // Setting Final Speed 0 m/s
                trajectory_lines.last().replace(QRegExp("\t1\n"), "\t0.0\n");
                
                foreach (QString line, trajectory_lines)
                {
                        test_trajectory_line << line;
                }
                test_trajectory_line.flush();
        
                // Closing trajectory file

                file.close();   // and

                ParseBlock config;
        
                config.load("loader.conf");
        
                TrajectoryLoader *loader = TrajectoryUtil::createLoader(config["loader"]);
                TrajectoryInterpolator *interpolator = TrajectoryUtil::createInterpolator(config["interpolator"]);
        
                std::fstream filestr;
                filestr.open ("trajectory.dat");
        
                Trajectory* trajectory;
                trajectory = loader->load(filestr);
                interpolator->interpolate( *trajectory );
        
                yarp::os::Port trajectoryOutput;

                trajectoryOutput.open("/romeo/hmi/trajectory");
                
                if( yarp::os::Network::connect("/romeo/hmi/trajectory", "/romeo/purpur/trajectory") )
                {
                        trajectoryOutput.write(*trajectory);
                        statusbar->showMessage(tr("(INFO) - Sending Trajectory ..."));
                        theta_offset=0;
                        return;
                }
                else
                {
                        QMessageBox::warning(this, tr("ROMEO HMI v 2.0"),
                            tr("Destiny Port INACTIVE."));

                        INIT_toolButton->setEnabled(TRUE);
                        NEXT_toolButton->setEnabled(TRUE);
                        trashTrajectory_toolButton->setEnabled(TRUE);
                        LEFT_toolButton->setEnabled(TRUE);
                        RIGHT_toolButton->setEnabled(TRUE);
                        UP_toolButton->setEnabled(TRUE);
                        DOWN_toolButton->setEnabled(TRUE);
                        launchTrajectorytoolButton->setEnabled(TRUE);
                        loadTrajectory_toolButton->setEnabled(TRUE);
                        loadMap_toolButton->setEnabled(TRUE);
                        thetaInit_ToolButton->setEnabled(TRUE);

                        return;
                }
        }
}

void RomeoMainWindow::upArrow()
{
        if (trajectory_points_number==0)        // If trajectory is empty we change Romeo's position
        {
                locale_romeo_x_ref=romeo->scenePos().x();
                locale_romeo_y_ref=romeo->scenePos().y();
                romeo->setPos(locale_romeo_x_ref,locale_romeo_y_ref-1);
                last_trajectory_point=romeo->scenePos();
                initial_trajectory_point=romeo->scenePos();
                return;
        }
        else
        {
                // Moving up Last SPot
                spots.last()->setPos(spots.last()->pos().x(),spots.last()->pos().y()-9);
                
                // Redrawing Last Line
                last_trajectory_point.setY(last_trajectory_point.y()-9);
        
                lines.last()->setLine(lines.last()->line().x1(),lines.last()->line().y1(),
                spots.last()->pos().x()+4,spots.last()->pos().y()+4);   

                absolutLines.last()->setLine(absolutLines.last()->line().x1(),absolutLines.last()->line().y1(),
                spots.last()->pos().x()+4,spots.last()->pos().y()+4);   

                
                // Redrawing Waypoint's coordinates
                
                qreal x_meters=( spots.last()->x()- initial_trajectory_point.x() )/map_X_scale;
                qreal y_meters=(initial_trajectory_point.y()-spots.last()->y())/map_Y_scale;
        
                waypoints.last()->setPlainText(QString("[%1 , %2]").arg(x_meters).arg(y_meters));
                waypoints.last()->setPos(last_trajectory_point);
                
                absolutWaypoints.last()->setPlainText(QString("[%1 , %2]").arg(x_meters+x_ini_local).arg(y_meters+y_ini_local));
                absolutWaypoints.last()->setPos(last_trajectory_point.x(),last_trajectory_point.y()+10);
                
                
                // Changing last Line in Test Trajectory File   
        
                trajectory_lines.removeLast();
                trajectory_lines.append(QString("%1\t%2\t1\n").arg(x_meters).arg(y_meters));
        }       
}

void RomeoMainWindow::downArrow()
{
        if (trajectory_points_number==0)
        {
                locale_romeo_x_ref=romeo->scenePos().x();
                locale_romeo_y_ref=romeo->scenePos().y();
                romeo->setPos(locale_romeo_x_ref,locale_romeo_y_ref+1);
                last_trajectory_point=romeo->scenePos();
                initial_trajectory_point=romeo->scenePos();
                return;
        }
        else
        {
                // Moving DOWN Last SPot
                spots.last()->setPos(spots.last()->pos().x(),spots.last()->pos().y()+9);
        
                // Redrawing Last Line
                last_trajectory_point.setY(last_trajectory_point.y()+9);
                
                /*lines.last()->setLine(lines.last()->line().x1(),lines.last()->line().y1(),spots.last()->offset().x()+8,spots.last()->offset().y()+8);*/       
        
                lines.last()->setLine(lines.last()->line().x1(),lines.last()->line().y1(),
                spots.last()->pos().x()+4,spots.last()->pos().y()+4);   
                
                // Redrawing Waypoint's coordinates
        
                qreal x_meters=( spots.last()->x()- initial_trajectory_point.x() )/map_X_scale;
                qreal y_meters=(initial_trajectory_point.y()-spots.last()->y())/map_Y_scale;
        
                waypoints.last()->setPlainText(QString("[%1 , %2]").arg(x_meters).arg(y_meters));
                waypoints.last()->setPos(last_trajectory_point);
        
                // Changing last Line in Test Trajectory File   
        
                trajectory_lines.removeLast();
                trajectory_lines.append(QString("%1\t%2\t1\n").arg(x_meters).arg(y_meters));
        }
}

void RomeoMainWindow::leftArrow()
{
        if( theta_edit_mode==true )
        {
                previous_theta=theta;
                theta_offset-=10;

                theta=(orient_ref*(-180/Pi))+theta_offset;

                romeo->rotate(theta - previous_theta);

                qDebug() << "Theta: " << theta;
                return;
        }


        if (trajectory_points_number==0)
        {
                locale_romeo_x_ref=romeo->scenePos().x();
                locale_romeo_y_ref=romeo->scenePos().y();
                romeo->setPos(locale_romeo_x_ref-1,locale_romeo_y_ref);
                last_trajectory_point=romeo->scenePos();
                initial_trajectory_point=romeo->scenePos();
                return;
        }
        else
        {
                // Moving LEFT Last Spot
                spots.last()->setPos( spots.last()->pos().x()-9, spots.last()->pos().y());

                // Redrawing Last Line
                last_trajectory_point.setX(last_trajectory_point.x()-9);
                lines.last()->setLine(lines.last()->line().x1(),lines.last()->line().y1(),spots.last()->pos().x()+8,spots.last()->pos().y()+8);
        
                // Redrawing Waypoint's coordinates
                qreal x_meters=( spots.last()->x()- initial_trajectory_point.x() )/map_X_scale;
                qreal y_meters=(initial_trajectory_point.y()-spots.last()->y())/map_Y_scale;
                
                waypoints.last()->setPlainText(QString("[%1 , %2]").arg(x_meters).arg(y_meters));
                waypoints.last()->setPos(last_trajectory_point);

                // Changing last Line in Test Trajectory File   
                trajectory_lines.removeLast();
                trajectory_lines.append(QString("%1\t%2\t1\n").arg(x_meters).arg(y_meters));
        }
}

void RomeoMainWindow::rightArrow()
{
        if( theta_edit_mode==true )
        {       
                previous_theta=theta;
                theta_offset+=10;
                theta=(orient_ref*(-180/Pi))+theta_offset;

                romeo->rotate( theta-previous_theta );
                qDebug() << "Theta: " << theta;
                return;
        }

        if (trajectory_points_number==0)
        {
                locale_romeo_x_ref=romeo->scenePos().x();
                locale_romeo_y_ref=romeo->scenePos().y();
                romeo->setPos(locale_romeo_x_ref+1,locale_romeo_y_ref);
                last_trajectory_point=romeo->scenePos();
                initial_trajectory_point=romeo->scenePos();
                return;
        }
        else
        {
                //Moving RIGHT Last SPot
                spots.last()->setPos(spots.last()->pos().x()+9,spots.last()->pos().y());
                
                //Redrawing Last Line
                last_trajectory_point.setX(last_trajectory_point.x()+9);
        
                lines.last()->setLine(lines.last()->line().x1(),lines.last()->line().y1(),spots.last()->pos().x()+8,spots.last()->pos().y()+8);
                
                //Redrawing Waypoint's coordinates
        
                qreal x_meters = ( spots.last()->x() - initial_trajectory_point.x() ) / map_X_scale;
                qreal y_meters = ( initial_trajectory_point.y() - spots.last()->y() ) / map_Y_scale;
                
                waypoints.last()->setPlainText(QString("[%1 , %2]").arg(x_meters).arg(y_meters));
                waypoints.last()->setPos(last_trajectory_point);
        
                //Changing last Line in Test Trajectory File    
        
                trajectory_lines.removeLast();
                trajectory_lines.append(QString("%1\t%2\t1\n").arg(x_meters).arg(y_meters));
        }
}

void RomeoMainWindow::panicEvent()
{
        stop_ALL_CONTROL_Process();

        QMessageBox::critical(this, tr("ROMEO HMI v 3.0"),
                            tr("PANIC BUTTON PRESSED.\nStopping Romeo"));
        // Enabling Buttons

        INIT_toolButton->setEnabled(TRUE);
        NEXT_toolButton->setEnabled(TRUE);
        trashTrajectory_toolButton->setEnabled(TRUE);
        LEFT_toolButton->setEnabled(TRUE);
        RIGHT_toolButton->setEnabled(TRUE);
        UP_toolButton->setEnabled(TRUE);
        DOWN_toolButton->setEnabled(TRUE);
        launchTrajectorytoolButton->setEnabled(TRUE);
        loadTrajectory_toolButton->setEnabled(TRUE);
        loadMap_toolButton->setEnabled(TRUE);
        thetaInit_ToolButton->setEnabled(TRUE);

        x_ini_global=x_ref;     // Saving Last X coordinate ( with Romeo Stopped )
        y_ini_global=y_ref;     // Saving Last Y coordinate ( with Romeo Stopped )

//      x_ini_local=x_ref;
//      y_ini_local=y_ref;

        trashtrajectory();

//      zoomFit();
}


void RomeoMainWindow::trajetoryFinalized()
{
        stop_ALL_CONTROL_Process();

        QMessageBox::information(this, tr("ROMEO HMI v 3.0"),
                            tr("The Trajectory has been concluded.\nStopping Romeo"));
        // Enabling Buttons

        INIT_toolButton->setEnabled(TRUE);
        NEXT_toolButton->setEnabled(TRUE);
        trashTrajectory_toolButton->setEnabled(TRUE);
        LEFT_toolButton->setEnabled(TRUE);
        RIGHT_toolButton->setEnabled(TRUE);
        UP_toolButton->setEnabled(TRUE);
        DOWN_toolButton->setEnabled(TRUE);
        launchTrajectorytoolButton->setEnabled(TRUE);
        loadTrajectory_toolButton->setEnabled(TRUE);
        loadMap_toolButton->setEnabled(TRUE);
        thetaInit_ToolButton->setEnabled(TRUE);

//      x_ini_local=x_ref;      // Saving Last X coordinate ( with Romeo Stopped )
//      y_ini_local=y_ref;      // Saving Last Y coordinate ( with Romeo Stopped )

        trashtrajectory();

//      zoomFit();
}



void RomeoMainWindow::initialtrajectorypoint()
{
        trajectory_edit_mode=true;
        trajectory_executing_mode=false;        // Just in case
        setting_initial_point=true;
        
//      romeo->grabMouse();                     // Valid in Qt 4.4
        MapGraphicsView->setDragMode(QGraphicsView::NoDrag);
        MapGraphicsView->setCursor(Qt::CrossCursor);

        romeo->setAcceptedMouseButtons(Qt::LeftButton);
        emit message("Trajectory Edit Mode ON");
}

void RomeoMainWindow::nexttrajectorypoint()
{
        trajectory_edit_mode=true;
        trajectory_executing_mode=false;        // Just in case

        MapGraphicsView->setDragMode(QGraphicsView::NoDrag);
        MapGraphicsView->setCursor(Qt::CrossCursor);
        scene.clearFocus();
        romeo->setAcceptedMouseButtons(0);
        
        emit message("Insert NEXT trajectory Point");
}


void RomeoMainWindow::changeThetaInit()
{
        theta_edit_mode=not(theta_edit_mode);
        

        if( theta_edit_mode==true )
        {
                emit message("(INFO) - Change Theta Init.");
                UP_toolButton->setEnabled(false);
                DOWN_toolButton->setEnabled(false);
                INIT_toolButton->setEnabled(false);
                NEXT_toolButton->setEnabled(false);
        }
        else
        {

                UP_toolButton->setEnabled(true);
                DOWN_toolButton->setEnabled(true);
                INIT_toolButton->setEnabled(true);
                NEXT_toolButton->setEnabled(true);
        }





}


void RomeoMainWindow::launch_LAUNCHER_Process()
{
//      statusbar->showMessage(tr("(INFO) - Launcher Process Active."));
// 
//      LAUNCHER_Process->start(LAUNCHER_program);
//      launcher_START_toolButton->setEnabled(false);
//      launcher_STOP_toolButton->setEnabled(true);
}

void RomeoMainWindow::stop_LAUNCHER_Process()
{
//      statusbar->showMessage(tr("(INFO) - Launcher Process Inactive."));
//      if ( LAUNCHER_Process->pid()!= 0 )
//      {
//              kill(LAUNCHER_Process->pid(), SIGINT);
//              launcher_START_toolButton->setEnabled(true);
//              launcher_STOP_toolButton->setEnabled(false);
//              
//      }
}


void RomeoMainWindow::stop_ALL_CONTROL_Process()
{
        stop_LAUNCHER_Process();
        sleep(0.5);
}

void RomeoMainWindow::connect_LASER2D_sensor()
{
        if(yarpRunning==true)
        {
                // and make connections:
                if( yarp::os::Network::connect("/romeo/laser2D/imageOut/data", "/romeo/hmi/laser2D/sensorimage") )
                {
                        readLaser2DImage=true;
                        
                        laser2D_START_toolButton->setEnabled(false);
                        laser2D_STOP_toolButton->setEnabled(true);
                        statusbar->showMessage(tr("(INFO) - Laser 2D Sensor Active."));
        
                        laserRange=laser2Dscene.addEllipse(QRectF(0,0,170,170));
                        laserRange->setSpanAngle(2880);
                        laserRange->setBrush(QColor(225,225,255,127));
                        laserRange->setPen(QPen(Qt::blue, 1, Qt::DotLine, Qt::FlatCap, Qt::RoundJoin));
                        laserRange->setPos(0,65);
                
                        romeoImage=laser2Dscene.addPixmap(QPixmap(":/images/romeo3.png"));
                        romeoImage->setPos(90,150);
                        romeoImage->setZValue(10);
                        
                        laser2Dscene.removeItem(laser2DInfoText);
                        laser2Dscene.addItem(laser2DPixmap);
                        laser2Dscene.update();
                }
                else
                {
                        statusbar->showMessage(tr("(ERROR) - Laser 2D Sensor INACTIVE."));
                }
        }
        else
        {
                statusbar->showMessage(tr("(ERROR) - Yarp Network Inactive."));
        }

}

void RomeoMainWindow::disconnect_LASER2D_sensor()
{
        if(yarpRunning==true)
        {
                if(yarp::os::Network::disconnect("/romeo/laser2D/imageOut/data", "/romeo/hmi/laser2D/sensorimage"))             
                {
                        readLaser2DImage=false;
                        
                        laser2D_START_toolButton->setEnabled(true);
                        laser2D_STOP_toolButton->setEnabled(false);

                        laser2Dscene.removeItem(laser2DPixmap);
                        laser2Dscene.removeItem(laser2DInfoText);
                        laser2Dscene.update();
                }
                statusbar->showMessage(tr("(INFO) - Laser 2D Sensor Inactive."));
        }
        else
        {
                statusbar->showMessage(tr("(ERROR) - Yarp Network Inactive."));
        }
}

void RomeoMainWindow::connect_ELEVATIONMAP_sensor()
{
        if(yarpRunning==true)
        {
                // and make connections:
                if(yarp::os::Network::connect("/romeo/elevationmap/imageOut/data", "/romeo/hmi/sensormap/sensorimage"))
                {
                        readElevationMapImage=true;
                        elevationMap_SWITCH_VIEW_toolButton->setEnabled(true);
                        elevationMap_START_toolButton->setEnabled(false);
                        elevationMap_STOP_toolButton->setEnabled(true);
                        statusbar->showMessage(tr("(INFO) - Elevation MAP Sensor Active."));

                        scene.removeItem(elevationMapPixmap);
                        elevationMapscene.removeItem(elevationMapInfoText);
                        elevationMapscene.addItem(elevationMapPixmap);
                        elevationMapscene.update();
                }
                else
                {
                        statusbar->showMessage(tr("(ERROR) - Elevation MAP Sensor INACTIVE."));
                }
        }
        else
        {
                statusbar->showMessage(tr("(ERROR) - Yarp Network Inactive."));
        }
}

void RomeoMainWindow::disconnect_ELEVATIONMAP_sensor()
{
        if(yarpRunning==true)
        {
                if(yarp::os::Network::disconnect("/romeo/elevationmap/imageOut/data", "/romeo/hmi/sensormap/sensorimage"))
                {
                        if(elevationMapMainView==true)
                        {
                                double xScale;
                                double yScale;

                                xScale = elevationMap_x_axis_length / map_width;
                                yScale = elevationMap_y_axis_length / map_height;
                                
                                elevationMapPixmap->scale( xScale, yScale );

                                scene.removeItem(elevationMapPixmap);
                                elevationMapscene.removeItem(elevationMapInfoText);
                                elevationMapscene.update();
                        }

                        readElevationMapImage=false;
                        elevationMap_SWITCH_VIEW_toolButton->setEnabled(false);
                        elevationMap_START_toolButton->setEnabled(true);
                        elevationMap_STOP_toolButton->setEnabled(false);
                }
                statusbar->showMessage(tr("(INFO) - Elevation Map Sensor Inactive."));
        }
        else
        {
                statusbar->showMessage(tr("(ERROR) - Yarp Network Inactive."));
        }
}


void RomeoMainWindow::connect_TRANSMAP_sensor()
{
        if(yarpRunning==true)
        {
                // and make connections:
                if(yarp::os::Network::connect("/romeo/elevationmap/transvOut/data", "/romeo/hmi/transmap/sensorimage"))
                {
                        readTransMapImage=true;
                        transMap_SWITCH_VIEW_toolButton->setEnabled(true);
                        statusbar->showMessage(tr("(INFO) - Transversability MAP Sensor Active."));
                        transMap_START_toolButton->setEnabled(false);
                        transMap_STOP_toolButton->setEnabled(true);

                        scene.removeItem(transMapPixmap);
                        transMapscene.removeItem(transMapInfoText);
                        transMapscene.addItem(transMapPixmap);
                        transMapscene.update();
                }
                else
                {
                        statusbar->showMessage(tr("(ERROR) - Transversability MAP Sensor INACTIVE."));
                }
        }
        else
        {
                statusbar->showMessage(tr("(ERROR) - Yarp Network Inactive."));
        }
}

void RomeoMainWindow::disconnect_TRANSMAP_sensor()
{
        if(yarpRunning==true)
        {
                if(yarp::os::Network::disconnect("/romeo/elevationmap/transvOut/data", "/romeo/transmap/sensorimage"))
                {
                        if(transMapMainView==true)
                        {
                                double xScale;
                                double yScale;

                                xScale = transMap_x_axis_length / map_width;
                                yScale = transMap_y_axis_length / map_height;
                                
                                transMapPixmap->scale( xScale, yScale );

                                scene.removeItem(transMapPixmap);
                                transMapscene.removeItem(transMapInfoText);
                        }
                        
                        readTransMapImage=false;
                        transMap_START_toolButton->setEnabled(true);
                        transMap_STOP_toolButton->setEnabled(false);
                        transMap_SWITCH_VIEW_toolButton->setEnabled(false);
                }
                statusbar->showMessage(tr("(INFO) - Transversability Map Sensor Inactive."));
        }
        else
        {
                statusbar->showMessage(tr("(ERROR) - Yarp Network Inactive."));
        }
}


void RomeoMainWindow::IplImage2QImage(IplImage *src, QGraphicsPixmapItem * pixmapElement, QGraphicsScene *sensorScene)
{
        int width     = src->width;
        int widthStep = src->widthStep;
        int height    = src->height;

        // OpenCV image is stored with 3 byte color pixels (3 channels).
        // We convert it to a 32 bit depth QImage.
        
        uchar *qImageBuffer = (uchar *)malloc(width * height * 4 * sizeof(uchar));
        uchar *QImagePtr = qImageBuffer;
        const uchar *iplImagePtr = (const uchar *)src->imageData;
        
        for (int y = 0; y < height; ++y)
        {
                for (int x = 0; x < width; ++x)
                {
                        // We cannot help but copy manually.
                        QImagePtr[0] = iplImagePtr[0];
                        QImagePtr[1] = iplImagePtr[1];
                        QImagePtr[2] = iplImagePtr[2];
                        QImagePtr[3] = 255;             // Alpha Channel - SOLID


                        if( (QImagePtr[0]==255) && (QImagePtr[1]==255) && (QImagePtr[2]==255) ) // Changes White into GREEN
                        {
                                QImagePtr[0] = 0;
                                QImagePtr[1] = 0;
                                QImagePtr[2] = 255;
                        }

                        if( (QImagePtr[0]==0) && (QImagePtr[1]==0) && (QImagePtr[2]==0) ) // Changes Black into WHITE
                        {
                                QImagePtr[0] = 255;
                                QImagePtr[1] = 255;
                                QImagePtr[2] = 255;
                        }

                        QImagePtr += 4;
                        iplImagePtr += 3;
                }
                iplImagePtr += widthStep - 3 * width;
        }

        QImage tmp_img(qImageBuffer,width,height,QImage::Format_RGB32);
        pixmapElement->setPixmap(QPixmap::fromImage(tmp_img));

        sensorScene->update();
        free(qImageBuffer);
}


void RomeoMainWindow::switchElevationView()
{
        elevationMapMainView=not(elevationMapMainView);
        
        double xScale;
        double yScale;

        if(elevationMapMainView==true)
        {
                xScale =  map_width / elevationMap_x_axis_length ;
                yScale =  map_height / elevationMap_y_axis_length ;

                elevationMapPixmap->scale(  xScale, yScale);
                
                scene.addItem(elevationMapPixmap);
                elevationMapscene.removeItem(elevationMapInfoText);
                elevationMapInfoText=elevationMapscene.addText("Representing Data\non Map View", infoFont);
                elevationMapGraphicsView->centerOn(elevationMapInfoText);
                elevationMapscene.update();
        }
        else
        {       
                xScale = elevationMap_x_axis_length / map_width;
                yScale = elevationMap_y_axis_length / map_height;

                elevationMapPixmap->scale( xScale, yScale );
                
                scene.removeItem(elevationMapPixmap);
                elevationMapscene.removeItem(elevationMapInfoText);
                elevationMapscene.addItem(elevationMapPixmap);
                elevationMapscene.update();
        }
        elevationMapGraphicsView->resetMatrix();
}

void RomeoMainWindow::switchTransView()
{
        transMapMainView=not(transMapMainView);

        double xScale;
        double yScale;

        if(transMapMainView==true)
        {
                xScale =  map_width / transMap_x_axis_length ;
                yScale =  map_height / transMap_y_axis_length ;

                transMapPixmap->scale( xScale, yScale);
                
                scene.addItem(transMapPixmap);
                transMapscene.removeItem(transMapInfoText);
                transMapInfoText=transMapscene.addText("Representing Data\non Map View", infoFont);
                transMapGraphicsView->centerOn(transMapInfoText);
                transMapscene.update();
        }
        else
        {       
                xScale = transMap_x_axis_length / map_width;
                yScale = transMap_y_axis_length / map_height;

                transMapPixmap->scale( xScale, yScale );
                
                scene.removeItem(transMapPixmap);
                transMapscene.removeItem(transMapInfoText);
                transMapscene.addItem(transMapPixmap);
                transMapscene.update();
        }
        transMapGraphicsView->resetMatrix();
}

---

Inicio  |   Acerca de  |   Contactar

DISEÑO E IMPLEMENTACIÓN DE INTERFAZ PERSONA MÁQUINA Y SISTEMA DE ANÁLISIS POST-MISIÓN PARA ROBOT AUTÓNOMO MÓVIL ROMEO-4R

: Pérez Domínguez, Daniel
: Ingeniería Telecomunicación

Contenido del proyecto:

Directorio raíz  >  CODE  >  hmi3  >  doxygen  >  html  >  romeomainwindow_8cpp-source.html