diff --git a/crazyflie_ws/sandbox/crazypkg/gui/untitled/#mainguiwindow.cpp# b/crazyflie_ws/sandbox/crazypkg/gui/untitled/#mainguiwindow.cpp#
deleted file mode 100644
index fb33ef1f0edc8b024b1b24e8463735b1abc96063..0000000000000000000000000000000000000000
--- a/crazyflie_ws/sandbox/crazypkg/gui/untitled/#mainguiwindow.cpp#
+++ /dev/null
@@ -1,935 +0,0 @@
-#include "mainguiwindow.h"
-#include "ui_mainguiwindow.h"
-#include <QDoubleSpinBox>
-#include <QTextEdit>
-#include <QString>
-
-
-
-#define N_MAX_CRAZYFLIES           20 // protection number
-
-#ifndef DEBUG_GUI
-MainGUIWindow::MainGUIWindow(ros::NodeHandle* nodeHandle, /*ros::CallbackQueue *callbackQueue,
-                             ros::Publisher* publisherMotorCommandsGUI,*/
-                             QWidget *parent) :
-    QMainWindow(parent),
-    ui(new Ui::MainGUIWindow),
-    m_pNodeHandle(nodeHandle)
-{
-    ui->setupUi(this);
-    m_isStopButtonActive=false;
-    m_isCalActive=false;
-    m_trajectoryType=eTrajCustom;
-    _init();
-}
-#else
-MainGUIWindow::MainGUIWindow(QWidget *parent) :
-    QMainWindow(parent),
-    ui(new Ui::MainGUIWindow)
-{
-    ui->setupUi(this);
-    _init();
-}
-#endif
-
-MainGUIWindow::~MainGUIWindow()
-{
-    delete ui;
-}
-
-void MainGUIWindow::_init_tabs()
-{
-    ui->tabWidget->clear();
-    std::string str;
-    for (int i = 0; i < ui->spinBoxNumCrazyflies->value(); i++)
-    {
-        str = "CrazyFly ";
-        str += std::to_string(i+1);
-        QString qstr(str.c_str());
-        ui->tabWidget->addTab(new QWidget(), qstr);
-    }
-}
-
-void MainGUIWindow::_init()
-{
-    ui->spinBoxNumCrazyflies->setMaximum(N_MAX_CRAZYFLIES);
-    _init_tabs();
-}
-
-#ifndef DEBUG_GUI
-void MainGUIWindow::init()
-{
-    m_pNodeHandle->setCallbackQueue(&m_CallbackQueue);
-
-//    m_pPublisherMotorCommandsGUI=new ros::Publisher(m_pNodeHandle->advertise
-//            <crazypkg::MotorCommands>("topicDummyControllerCmd", 1));
-
-    m_pPublisherControllerParam=new ros::Publisher(m_pNodeHandle->advertise
-            <crazypkg::ControllerParam>("topicControllerParam", 100));
-
-    m_pPublisherPositionSetpoint=new ros::Publisher(m_pNodeHandle->advertise
-            <crazypkg::PositionSetpoint>("topicPositionSetpoint", 1));
-
-    m_pPublisherSampleTime=new ros::Publisher(m_pNodeHandle->advertise
-            <crazypkg::SampleTimeParam>("topicSampleTimeParam", 20));
-
-    m_pPublisherControllerType=new ros::Publisher(m_pNodeHandle->advertise
-            <std_msgs::Int32>("topicControllerType", 1));
-
-    m_pPublisherDoSomething=new ros::Publisher(m_pNodeHandle->advertise
-            <std_msgs::Int32>("topicDoSomething", 20));
-
-    m_pPublisherFeedforwardCmd=new ros::Publisher(m_pNodeHandle->advertise
-            <crazypkg::MotorCommands>("topicFeedforwardCmd",1));
-
-
-    // m_pSubscriberControllerOutput=new ros::Subscriber(m_pNodeHandle->subscribe
-    //         ("/FlightControl/topicControllerOutput",1,&MainGUIWindow::callbackControllerOutput,this));
-
-    // m_pSubscriberViconData=new ros::Subscriber(m_pNodeHandle->subscribe
-    //         ("/ViconDataStreamSDK/topicViconData",1,&MainGUIWindow::callbackViconData,this));
-
-    // m_pSubscriberCntViconDataMissed=new ros::Subscriber(m_pNodeHandle->subscribe
-    //         ("/FlightControl/topicCntViconDataMissed",1,&MainGUIWindow::callbackCntViconDataMissed,this));
-    // initPIDParamsTable();
-    // initRateParamsTable();
-
-    readDefaultParameters();
-
-
-    ros::Time::init();
-    ros::Duration(3).sleep();
-
-    m_CallbackQueue.callAvailable(ros::WallDuration(0));
-
-    // setDefaultPIDParameters();
-    // setDefaultRateParameters();
-
-    // initPositionSetpoint();
-    // initSetpointType();
-    // initSampleTime();
-    // initFeedforwardCmd();
-
-    // initSetpointQueues();
-
-    ros::Duration(1).sleep();
-
-    // initControllerType();
-
-    //refreshScreen();
-}
-
-// void MainGUIWindow::refreshScreen()
-// {
-// //    for(int i=0;i<countPIDControllers;i++)
-// //    {
-// //        ((QDoubleSpinBox*)ui->tableWidget->cellWidget(i,eKp))->setValue(m_PIDParams[i].Kp);
-// //        ((QDoubleSpinBox*)ui->tableWidget->cellWidget(i,eKi))->setValue(m_PIDParams[i].Ki);
-// //        ((QDoubleSpinBox*)ui->tableWidget->cellWidget(i,eKd))->setValue(m_PIDParams[i].Kd);
-// //        ((QDoubleSpinBox*)ui->tableWidget->cellWidget(i,eN))->setValue(m_PIDParams[i].N);
-// //    }
-
-// }
-
-
-
-
-// void MainGUIWindow::callbackCntViconDataMissed(const std_msgs::Int32& msg)
-// {
-//     ui->LCDMissedMes->display(msg.data);
-// }
-
-// void MainGUIWindow::callbackControllerOutput(const crazypkg::ControllerOutputPackage& msg)
-// {
-//     ui->LCDMotor1Cmd->display(msg.motorCmd1);
-//     ui->LCDMotor2Cmd->display(msg.motorCmd2);
-//     ui->LCDMotor3Cmd->display(msg.motorCmd3);
-//     ui->LCDMotor4Cmd->display(msg.motorCmd4);
-//     ui->LCDRollCmd->display(msg.roll);
-//     ui->LCDPitchCmd->display(msg.pitch);
-//     ui->LCDYawCmd->display(msg.yaw);
-//     ui->LCDThrustCmd->display(msg.thrust);
-
-//     switch (msg.onboardControllerType)
-//     {
-//     case eOnboardAngleController: {ui->labelControllerOutputMode->setText("Angle"); break;}
-//     case eOnboardRateController: {ui->labelControllerOutputMode->setText("Rate"); break;}
-//     case eOnboardMotorCmdController: {ui->labelControllerOutputMode->setText("MotorCmd"); break;}
-//     default:{ROS_ERROR("unknown onboard controller type in MainGUIWindow::callbackControllerOutput"); break;}
-//     }
-
-
-// }
-
-// void MainGUIWindow::callbackViconData(const crazypkg::ViconData& msg)
-// {
-//     ui->LCDViconDataX->display(msg.x);
-//     ui->LCDViconDataY->display(msg.y);
-//     ui->LCDViconDataZ->display(msg.z);
-//     ui->LCDViconDataYaw->display(msg.yaw*RAD2DEG);
-//     ui->LCDViconDataPitch->display(msg.pitch*RAD2DEG);
-//     ui->LCDViconDataRoll->display(msg.roll*RAD2DEG);
-// }
-
-void MainGUIWindow::runCallbacks()
-{
-    m_CallbackQueue.callAvailable(ros::WallDuration(0));
-
-    // updateSetpoint();
-}
-
-// void MainGUIWindow::setDefaultPIDParameters()
-// {
-//     //memcpy(&m_PIDParams,&m_DefaultPIDParams,sizeof(m_DefaultPIDParams));
-
-//     for(int i=0;i<countPIDControllers;i++)
-//     {
-//         ((QDoubleSpinBox*)ui->PIDParamTable->cellWidget(i,eKp))->setValue(m_DefaultPIDParams[i].Kp);
-//         ((QDoubleSpinBox*)ui->PIDParamTable->cellWidget(i,eKi))->setValue(m_DefaultPIDParams[i].Ki);
-//         ((QDoubleSpinBox*)ui->PIDParamTable->cellWidget(i,eKd))->setValue(m_DefaultPIDParams[i].Kd);
-//         ((QDoubleSpinBox*)ui->PIDParamTable->cellWidget(i,eN))->setValue(m_DefaultPIDParams[i].N);
-//         ((QDoubleSpinBox*)ui->PIDParamTable->cellWidget(i,eMinPIDSat))->setValue(m_DefaultPIDParams[i].MinSat);
-//         ((QDoubleSpinBox*)ui->PIDParamTable->cellWidget(i,eMaxPIDSat))->setValue(m_DefaultPIDParams[i].MaxSat);
-//     }
-// }
-
-// void MainGUIWindow::setDefaultRateParameters()
-// {
-//     //memcpy(&m_PIDParams,&m_DefaultPIDParams,sizeof(m_DefaultPIDParams));
-
-//     for(int i=0;i<countRateControllers;i++)
-//     {
-//         ((QDoubleSpinBox*)ui->RateParamTable->cellWidget(i,eKp))->setValue(m_DefaultRateParams[i].Kp);
-//         ((QDoubleSpinBox*)ui->RateParamTable->cellWidget(i,eKi))->setValue(m_DefaultRateParams[i].Ki);
-//         ((QDoubleSpinBox*)ui->RateParamTable->cellWidget(i,eKd))->setValue(m_DefaultRateParams[i].Kd);
-//         ((QDoubleSpinBox*)ui->RateParamTable->cellWidget(i,eN))->setValue(m_DefaultRateParams[i].N);
-//         ((QDoubleSpinBox*)ui->RateParamTable->cellWidget(i,eMinPIDSat))->setValue(m_DefaultRateParams[i].MinSat);
-//         ((QDoubleSpinBox*)ui->RateParamTable->cellWidget(i,eMaxPIDSat))->setValue(m_DefaultRateParams[i].MaxSat);
-//     }
-// }
-
-void MainGUIWindow::readDefaultParameters()
-{
-    m_pNodeHandle->param<double>("KpX",m_DefaultPIDParams[ePIDX].Kp,0);
-    m_pNodeHandle->param<double>("KiX",m_DefaultPIDParams[ePIDX].Ki,0);
-    m_pNodeHandle->param<double>("KdX",m_DefaultPIDParams[ePIDX].Kd,0);
-    m_pNodeHandle->param<double>("NX",m_DefaultPIDParams[ePIDX].N,60);
-    m_pNodeHandle->param<double>("MinSatPIDX",m_DefaultPIDParams[ePIDX].MinSat,-9876);
-    m_pNodeHandle->param<double>("MaxSatPIDX",m_DefaultPIDParams[ePIDX].MaxSat,98765);
-
-    m_pNodeHandle->param<double>("KpY",m_DefaultPIDParams[ePIDY].Kp,0);
-    m_pNodeHandle->param<double>("KiY",m_DefaultPIDParams[ePIDY].Ki,0);
-    m_pNodeHandle->param<double>("KdY",m_DefaultPIDParams[ePIDY].Kd,0);
-    m_pNodeHandle->param<double>("NY",m_DefaultPIDParams[ePIDY].N,60);
-    m_pNodeHandle->param<double>("MinSatPIDY",m_DefaultPIDParams[ePIDY].MinSat,-9876);
-    m_pNodeHandle->param<double>("MaxSatPIDY",m_DefaultPIDParams[ePIDY].MaxSat,98765);
-
-    m_pNodeHandle->param<double>("KpZ",m_DefaultPIDParams[ePIDZ].Kp,0);
-    m_pNodeHandle->param<double>("KiZ",m_DefaultPIDParams[ePIDZ].Ki,0);
-    m_pNodeHandle->param<double>("KdZ",m_DefaultPIDParams[ePIDZ].Kd,0);
-    m_pNodeHandle->param<double>("NZ",m_DefaultPIDParams[ePIDZ].N,60);
-    m_pNodeHandle->param<double>("MinSatPIDZ",m_DefaultPIDParams[ePIDZ].MinSat,-9876);
-    m_pNodeHandle->param<double>("MaxSatPIDZ",m_DefaultPIDParams[ePIDZ].MaxSat,98765);
-
-    m_pNodeHandle->param<double>("KpYaw",m_DefaultPIDParams[ePIDYaw].Kp,0);
-    m_pNodeHandle->param<double>("KiYaw",m_DefaultPIDParams[ePIDYaw].Ki,0);
-    m_pNodeHandle->param<double>("KdYaw",m_DefaultPIDParams[ePIDYaw].Kd,0);
-    m_pNodeHandle->param<double>("NYaw",m_DefaultPIDParams[ePIDYaw].N,60);
-    m_pNodeHandle->param<double>("MinSatPIDYaw",m_DefaultPIDParams[ePIDYaw].MinSat,-9876);
-    m_pNodeHandle->param<double>("MaxSatPIDYaw",m_DefaultPIDParams[ePIDYaw].MaxSat,98765);
-
-    m_pNodeHandle->param<double>("KpPitch",m_DefaultPIDParams[ePIDPitch].Kp,0);
-    m_pNodeHandle->param<double>("KiPitch",m_DefaultPIDParams[ePIDPitch].Ki,0);
-    m_pNodeHandle->param<double>("KdPitch",m_DefaultPIDParams[ePIDPitch].Kd,0);
-    m_pNodeHandle->param<double>("NPitch",m_DefaultPIDParams[ePIDPitch].N,60);
-    m_pNodeHandle->param<double>("MinSatPIDPitch",m_DefaultPIDParams[ePIDPitch].MinSat,-9876);
-    m_pNodeHandle->param<double>("MaxSatPIDPitch",m_DefaultPIDParams[ePIDPitch].MaxSat,98765);
-
-    m_pNodeHandle->param<double>("KpRoll",m_DefaultPIDParams[ePIDRoll].Kp,0);
-    m_pNodeHandle->param<double>("KiRoll",m_DefaultPIDParams[ePIDRoll].Ki,0);
-    m_pNodeHandle->param<double>("KdRoll",m_DefaultPIDParams[ePIDRoll].Kd,0);
-    m_pNodeHandle->param<double>("NRoll",m_DefaultPIDParams[ePIDRoll].N,60);
-    m_pNodeHandle->param<double>("MinSatPIDRoll",m_DefaultPIDParams[ePIDRoll].MinSat,-9876);
-    m_pNodeHandle->param<double>("MaxSatPIDRoll",m_DefaultPIDParams[ePIDRoll].MaxSat,98765);
-
-
-
-    m_pNodeHandle->param<double>("KpRateYaw",m_DefaultRateParams[ePIDYawRate].Kp,0);
-    m_pNodeHandle->param<double>("KiRateYaw",m_DefaultRateParams[ePIDYawRate].Ki,0);
-    m_pNodeHandle->param<double>("KdRateYaw",m_DefaultRateParams[ePIDYawRate].Kd,0);
-    m_pNodeHandle->param<double>("NRateYaw",m_DefaultRateParams[ePIDYawRate].N,60);
-    m_pNodeHandle->param<double>("MinSatRateYaw",m_DefaultRateParams[ePIDYawRate].MinSat,-9876);
-    m_pNodeHandle->param<double>("MaxSatRateYaw",m_DefaultRateParams[ePIDYawRate].MaxSat,98765);
-
-    m_pNodeHandle->param<double>("KpRatePitch",m_DefaultRateParams[ePIDPitchRate].Kp,0);
-    m_pNodeHandle->param<double>("KiRatePitch",m_DefaultRateParams[ePIDPitchRate].Ki,0);
-    m_pNodeHandle->param<double>("KdRatePitch",m_DefaultRateParams[ePIDPitchRate].Kd,0);
-    m_pNodeHandle->param<double>("NRatePitch",m_DefaultRateParams[ePIDPitchRate].N,60);
-    m_pNodeHandle->param<double>("MinSatRatePitch",m_DefaultRateParams[ePIDPitchRate].MinSat,-9876);
-    m_pNodeHandle->param<double>("MaxSatRatePitch",m_DefaultRateParams[ePIDPitchRate].MaxSat,98765);
-
-    m_pNodeHandle->param<double>("KpRateRoll",m_DefaultRateParams[ePIDRollRate].Kp,0);
-    m_pNodeHandle->param<double>("KiRateRoll",m_DefaultRateParams[ePIDRollRate].Ki,0);
-    m_pNodeHandle->param<double>("KdRateRoll",m_DefaultRateParams[ePIDRollRate].Kd,0);
-    m_pNodeHandle->param<double>("NRateRoll",m_DefaultRateParams[ePIDRollRate].N,60);
-    m_pNodeHandle->param<double>("MinSatRateRoll",m_DefaultRateParams[ePIDRollRate].MinSat,-9876);
-    m_pNodeHandle->param<double>("MaxSatRateRoll",m_DefaultRateParams[ePIDRollRate].MaxSat,98765);
-
-
-
-    m_pNodeHandle->param<double>("SampleTimePID",m_DefaultSampleTime[ePIDTs],0.020);
-    m_pNodeHandle->param<double>("SampleTimeLQRFull",m_DefaultSampleTime[eLQRFullTs],0.020);
-    m_pNodeHandle->param<double>("SampleTimeLQRNested",m_DefaultSampleTime[eLQRNestedTs],0.020);
-    m_pNodeHandle->param<double>("SampleTimeRate",m_DefaultSampleTime[eRateTs],0.020);
-
-
-    m_pNodeHandle->param<float>("FeedforwardMotor1",m_DefaultFeedforwardCmd.cmd1,0.0);
-    m_pNodeHandle->param<float>("FeedforwardMotor2",m_DefaultFeedforwardCmd.cmd2,0.0);
-    m_pNodeHandle->param<float>("FeedforwardMotor3",m_DefaultFeedforwardCmd.cmd3,0.0);
-    m_pNodeHandle->param<float>("FeedforwardMotor4",m_DefaultFeedforwardCmd.cmd4,0.0);
-}
-
-// void MainGUIWindow::initPIDParamsTable()
-// {
-//     for (int i = 0; i < countPIDControllers; ++i)
-//     {
-//         for (int j = 0; j < countPIDParams ; ++j)
-//         {
-//             ui->PIDParamTable->setCellWidget(i,j,new QDoubleSpinBox(ui->PIDParamTable));
-//             ((QDoubleSpinBox*)ui->PIDParamTable->cellWidget(i,j))->setDecimals(3);
-//             ((QDoubleSpinBox*)ui->PIDParamTable->cellWidget(i,j))->setMaximum(999999.0);
-//             ((QDoubleSpinBox*)ui->PIDParamTable->cellWidget(i,j))->setProperty("row",i);
-//             ((QDoubleSpinBox*)ui->PIDParamTable->cellWidget(i,j))->setProperty("column",j);
-//             ((QDoubleSpinBox*)ui->PIDParamTable->cellWidget(i,j))->setValue(1234.321);
-//             if(j==eMinPIDSat)
-//                 ((QDoubleSpinBox*)ui->PIDParamTable->cellWidget(i,j))->setMinimum(-99999.0);
-//             else
-//                 ((QDoubleSpinBox*)ui->PIDParamTable->cellWidget(i,j))->setMinimum(0);
-//             connect(((QDoubleSpinBox*)ui->PIDParamTable->cellWidget(i,j))
-//                     , SIGNAL(valueChanged(double)), this, SLOT(PIDParamTableChanged(double)));
-//         }
-
-
-//     }
-// }
-
-// void MainGUIWindow::initRateParamsTable()
-// {
-//         for (int i = 0; i < countRateControllers; ++i) {
-//             for (int j = 0; j < countPIDParams ; ++j) {
-//                 ui->RateParamTable->setCellWidget(i,j,new QDoubleSpinBox(ui->RateParamTable));
-//                 ((QDoubleSpinBox*)ui->RateParamTable->cellWidget(i,j))->setDecimals(3);
-//                 ((QDoubleSpinBox*)ui->RateParamTable->cellWidget(i,j))->setMaximum(999999.0);
-//                 ((QDoubleSpinBox*)ui->RateParamTable->cellWidget(i,j))->setProperty("row",i);
-//                 ((QDoubleSpinBox*)ui->RateParamTable->cellWidget(i,j))->setProperty("column",j);
-//                 ((QDoubleSpinBox*)ui->RateParamTable->cellWidget(i,j))->setValue(1234.321);
-//                 if(j==eMinPIDSat)
-//                     ((QDoubleSpinBox*)ui->RateParamTable->cellWidget(i,j))->setMinimum(-99999.0);
-//                 else
-//                      ((QDoubleSpinBox*)ui->RateParamTable->cellWidget(i,j))->setMinimum(0);
-//                 connect(((QDoubleSpinBox*)ui->RateParamTable->cellWidget(i,j))
-//                         , SIGNAL(valueChanged(double)), this, SLOT(RateParamTableChanged(double)));
-//             }
-
-
-//         }
-
-// }
-
-// void MainGUIWindow::PIDParamTableChanged(double param)
-// {
-
-//     QDoubleSpinBox* spinBox = qobject_cast<QDoubleSpinBox*>(sender());
-//         if (spinBox)
-//         {
-//             m_controllerParam.crazyControllerType=ePID;
-//             m_controllerParam.basicControllerType=spinBox->property("row").toInt();
-//             m_controllerParam.paramType=spinBox->property("column").toInt();
-//             m_controllerParam.value=param;
-
-//             m_pPublisherControllerParam->publish(m_controllerParam);
-//         }
-// }
-
-// void MainGUIWindow::RateParamTableChanged(double param)
-// {
-
-//     QDoubleSpinBox* spinBox = qobject_cast<QDoubleSpinBox*>(sender());
-//         if (spinBox)
-//         {
-//             m_controllerParam.crazyControllerType=eRate;
-//             m_controllerParam.basicControllerType=spinBox->property("row").toInt();
-//             m_controllerParam.paramType=spinBox->property("column").toInt();
-//             m_controllerParam.value=param;
-
-//             m_pPublisherControllerParam->publish(m_controllerParam);
-//         }
-// }
-
-// void MainGUIWindow::initPositionSetpoint()
-// {
-//     ui->SetpointX->setValue(ui->LCDViconDataX->value());
-//     ui->SetpointY->setValue(ui->LCDViconDataY->value());
-//     ui->SetpointZ->setValue(ui->LCDViconDataZ->value());
-//     ui->SetpointYaw->setValue(ui->LCDViconDataYaw->value());
-
-//     publishSetpoint();
-
-//     connect(ui->SetpointX, SIGNAL(valueChanged(double)), this, SLOT(positionSetpointChanged(double)));
-//     ui->SetpointX->setProperty("param","X");
-//     connect(ui->SetpointY, SIGNAL(valueChanged(double)), this, SLOT(positionSetpointChanged(double)));
-//     ui->SetpointY->setProperty("param","Y");
-//     connect(ui->SetpointZ, SIGNAL(valueChanged(double)), this, SLOT(positionSetpointChanged(double)));
-//     ui->SetpointZ->setProperty("param","Z");
-//     connect(ui->SetpointYaw, SIGNAL(valueChanged(double)), this, SLOT(positionSetpointChanged(double)));
-//     ui->SetpointYaw->setProperty("param","Yaw");
-
-// }
-
-// void MainGUIWindow::positionSetpointChanged(double param)
-// {
-//     //publishSetpoint();
-// }
-
-// void MainGUIWindow::publishSetpoint()
-// {
-//     m_positionSetpoint.x=ui->SetpointX->value()/1000;
-//     m_positionSetpoint.y=ui->SetpointY->value()/1000;
-//     m_positionSetpoint.z=ui->SetpointZ->value()/1000;
-//     m_positionSetpoint.yaw=ui->SetpointYaw->value()*DEG2RAD;
-//     m_pPublisherPositionSetpoint->publish(m_positionSetpoint);
-// }
-
-// void MainGUIWindow::initSampleTime()
-// {
-
-
-
-// //    publishSampleTime(ePIDTs);
-// //    publishSampleTime(eLQRTs);
-// //    publishSampleTime(eLQRInnerTs);
-// //    publishSampleTime(eLQROuterTs);
-
-//     connect(ui->PIDTs, SIGNAL(valueChanged(double)), this, SLOT(sampleTimeChanged(double)));
-//     ui->PIDTs->setProperty("sampleTimeType",ePIDTs);
-//     connect(ui->RateTs, SIGNAL(valueChanged(double)), this, SLOT(sampleTimeChanged(double)));
-//     ui->RateTs->setProperty("sampleTimeType",eRateTs);
-//     connect(ui->LQRFullTs, SIGNAL(valueChanged(double)), this, SLOT(sampleTimeChanged(double)));
-//     ui->LQRFullTs->setProperty("sampleTimeType",eLQRFullTs);
-//     connect(ui->LQRNestedTs, SIGNAL(valueChanged(double)), this, SLOT(sampleTimeChanged(double)));
-//     ui->LQRNestedTs->setProperty("sampleTimeType",eLQRNestedTs);
-
-//     setDefaultSampleTime();
-
-// }
-
-// void MainGUIWindow::setDefaultSampleTime()
-// {
-//     ui->PIDTs->setValue(m_DefaultSampleTime[ePIDTs]*1000);
-//     ui->RateTs->setValue(m_DefaultSampleTime[eRateTs]*1000);
-//     ui->LQRFullTs->setValue(m_DefaultSampleTime[eLQRFullTs]*1000);
-//     ui->LQRNestedTs->setValue(m_DefaultSampleTime[eLQRNestedTs]*1000);
-// }
-
-//void MainGUIWindow::publishSampleTime(ESampleTimeType sampleTimeType)
-//{
-//    m_sampleTimeParam.crazyControllerType=controller;
-//    switch (controller)
-//    {
-//    case ePIDTs:
-//    {
-//        m_sampleTimeParam.value=ui->PIDTs->value();
-//        break;
-//    }
-//    case eLQRTs:
-//    {
-//        m_sampleTimeParam.value=ui->LQRTs->value();
-//        break;
-//    }
-//    case eLQRInnerTs:
-//    {
-//        m_sampleTimeParam.value=ui->LQRInnerTs->value();
-//        break;
-//    }
-//    case eLQROuterTs:
-//    {
-//        m_sampleTimeParam.value=ui->LQROuterTs->value();
-//        break;
-//    }
-//    default: ROS_ERROR("invalid sampleTime type in publish sample time")
-//    }
-
-//    m_pPublisherSampleTime->publish(m_sampleTimeParam);
-//}
-
-// void MainGUIWindow::sampleTimeChanged(double param)
-// {
-//     QDoubleSpinBox* spinBox = qobject_cast<QDoubleSpinBox*>(sender());
-//         if (spinBox)
-//         {
-//             m_sampleTimeParam.value=param/1000;
-//             m_sampleTimeParam.sampleTimeType=spinBox->property("sampleTimeType").toInt();
-//             m_pPublisherSampleTime->publish(m_sampleTimeParam);
-//         }
-// }
-
-// void MainGUIWindow::initControllerType()
-// {
-//     connect(ui->controllerPIDPosition, SIGNAL(toggled(bool)), this, SLOT(controllerTypeChanged(bool)));
-//     ui->controllerPIDPosition->setProperty("controllerType",ePIDPosition);
-//     connect(ui->controllerPIDAngle, SIGNAL(toggled(bool)), this, SLOT(controllerTypeChanged(bool)));
-//     ui->controllerPIDAngle->setProperty("controllerType",ePIDAngle);
-//     connect(ui->controllerPIDFull, SIGNAL(toggled(bool)), this, SLOT(controllerTypeChanged(bool)));
-//     ui->controllerPIDFull->setProperty("controllerType",ePIDFull);
-
-//     connect(ui->controllerLQRFull, SIGNAL(toggled(bool)), this, SLOT(controllerTypeChanged(bool)));
-//     ui->controllerLQRFull->setProperty("controllerType",eLQRFull);
-
-//     connect(ui->controllerLQRNestedOnboard, SIGNAL(toggled(bool)), this, SLOT(controllerTypeChanged(bool)));
-//     ui->controllerLQRNestedOnboard->setProperty("controllerType",eLQRNestedOnboardRate);
-//     connect(ui->controllerLQRNestedOffboard, SIGNAL(toggled(bool)), this, SLOT(controllerTypeChanged(bool)));
-//     ui->controllerLQRNestedOffboard->setProperty("controllerType",eLQRNestedOffboardRate);
-
-//     connect(ui->controllerAngleCmdTest, SIGNAL(toggled(bool)), this, SLOT(controllerTypeChanged(bool)));
-//     ui->controllerAngleCmdTest->setProperty("controllerType",eAngleCmdTest);
-//     connect(ui->controllerRateCmdTest, SIGNAL(toggled(bool)), this, SLOT(controllerTypeChanged(bool)));
-//     ui->controllerRateCmdTest->setProperty("controllerType",eRateCmdTest);
-//     connect(ui->controllerMotorCmdTest, SIGNAL(toggled(bool)), this, SLOT(controllerTypeChanged(bool)));
-//     ui->controllerMotorCmdTest->setProperty("controllerType",eMotorCmdTest);
-
-//     //set default controller:
-//     ui->controllerMotorCmdTest->setChecked(true);
-
-// }
-
-// void MainGUIWindow::initSetpointType()
-// {
-//     connect(ui->trajCustom, SIGNAL(toggled(bool)), this, SLOT(trajectoryTypeChanged(bool)));
-//     ui->trajCustom->setProperty("trajectoryType",eTrajCustom);
-//     connect(ui->trajCircle, SIGNAL(toggled(bool)), this, SLOT(trajectoryTypeChanged(bool)));
-//     ui->trajCircle->setProperty("trajectoryType",eTrajCircle);
-//     connect(ui->trajSquare, SIGNAL(toggled(bool)), this, SLOT(trajectoryTypeChanged(bool)));
-//     ui->trajSquare->setProperty("trajectoryType",eTrajSquare);
-
-//     ui->trajCustom->setChecked(true);
-// }
-
-// void MainGUIWindow::trajectoryTypeChanged(bool checked)
-// {
-//     QRadioButton* radioButton = qobject_cast<QRadioButton*>(sender());
-//         if (radioButton && checked)
-//         {
-//             m_trajectoryType=(ETrajectoryType)radioButton->property("trajectoryType").toInt();
-//         }
-// }
-
-// void MainGUIWindow::controllerTypeChanged(bool checked)
-// {
-//     QRadioButton* radioButton = qobject_cast<QRadioButton*>(sender());
-//         if (radioButton && checked)
-//         {
-//             m_controllerType.data=radioButton->property("controllerType").toInt();
-
-//             if(m_controllerType.data==ePIDPosition || m_controllerType.data==ePIDAngle ||
-//                     m_controllerType.data==eLQRNestedOnboardRate
-//                     || m_controllerType.data==eAngleCmdTest || m_controllerType.data==eRateCmdTest
-//                     || m_controllerType.data==eMotorCmdTest)
-//             {
-//                 ui->labelRateMode->setText("Onboard");
-//             }
-//             else if(m_controllerType.data==ePIDFull || m_controllerType.data==eLQRFull ||
-//                     m_controllerType.data==eLQRNestedOffboardRate )
-//             {
-//                ui->labelRateMode->setText("Offboard");
-//             }
-//             else
-//             {
-//                 ROS_ERROR("unknown controller type in MainGUIWindow::controllerTypeChanged");
-//             }
-
-//             m_pPublisherControllerType->publish(m_controllerType);
-//         }
-// }
-
-
-
-
-// void MainGUIWindow::on_buttonStop_clicked()
-// {
-//     if(!m_isStopButtonActive)
-//     {
-//         m_isStopButtonActive=true;
-//         m_DoSomething.data=eStopQuad;
-//         m_pPublisherDoSomething->publish(m_DoSomething);
-//         ui->buttonStop->setText("START");
-//     }
-//     else
-//     {
-//         m_isStopButtonActive=false;
-//         m_DoSomething.data=eStartQuad;
-//         m_pPublisherDoSomething->publish(m_DoSomething);
-//         ui->buttonStop->setText("STOP");
-//     }
-
-// }
-
-// void MainGUIWindow::on_buttonPrint_clicked()
-// {
-//     m_DoSomething.data=ePrintInfo;
-//     m_pPublisherDoSomething->publish(m_DoSomething);
-// }
-
-
-
-
-// void MainGUIWindow::initFeedforwardCmd()
-// {
-//     connect(ui->FeedforwardCmd1, SIGNAL(valueChanged(double)), this, SLOT(feedforwardCmdChanged(double)));
-//     connect(ui->FeedforwardCmd2, SIGNAL(valueChanged(double)), this, SLOT(feedforwardCmdChanged(double)));
-//     connect(ui->FeedforwardCmd3, SIGNAL(valueChanged(double)), this, SLOT(feedforwardCmdChanged(double)));
-//     connect(ui->FeedforwardCmd4, SIGNAL(valueChanged(double)), this, SLOT(feedforwardCmdChanged(double)));
-
-
-//     setDefaultFeedforwardCmd();
-// }
-
-// void MainGUIWindow::feedforwardCmdChanged(double cmd)
-// {
-//     m_feedforwardCmd.cmd1=ui->FeedforwardCmd1->value();
-//     m_feedforwardCmd.cmd2=ui->FeedforwardCmd2->value();
-//     m_feedforwardCmd.cmd3=ui->FeedforwardCmd3->value();
-//     m_feedforwardCmd.cmd4=ui->FeedforwardCmd4->value();
-//     m_pPublisherFeedforwardCmd->publish(m_feedforwardCmd);
-// }
-
-// void MainGUIWindow::setDefaultFeedforwardCmd()
-// {
-//     ui->FeedforwardCmd1->setValue(m_DefaultFeedforwardCmd.cmd1);
-//     ui->FeedforwardCmd2->setValue(m_DefaultFeedforwardCmd.cmd2);
-//     ui->FeedforwardCmd3->setValue(m_DefaultFeedforwardCmd.cmd3);
-//     ui->FeedforwardCmd4->setValue(m_DefaultFeedforwardCmd.cmd4);
-// }
-
-// void MainGUIWindow::on_buttonSetpointChange_clicked()
-// {
-//     publishSetpoint();
-// }
-
-// void MainGUIWindow::on_buttonResetControllers_clicked()
-// {
-//     m_DoSomething.data=eResetControllers;
-//     m_pPublisherDoSomething->publish(m_DoSomething);
-// }
-
-// void MainGUIWindow::on_buttonSetDefaultTs_clicked()
-// {
-//         setDefaultSampleTime();
-// }
-
-// void MainGUIWindow::on_buttonPIDDefaultParams_clicked()
-// {
-//     setDefaultPIDParameters();
-// }
-
-// void MainGUIWindow::on_buttonSetpointCurrPos_clicked()
-// {
-//     ui->SetpointX->setValue(ui->LCDViconDataX->value());
-//     ui->SetpointY->setValue(ui->LCDViconDataY->value());
-//     ui->SetpointZ->setValue(ui->LCDViconDataZ->value());
-//     ui->SetpointYaw->setValue(ui->LCDViconDataYaw->value());
-
-//     publishSetpoint();
-// }
-
-// void MainGUIWindow::on_buttonDefaultFeedforward_clicked()
-// {
-//     setDefaultFeedforwardCmd();
-// }
-
-
-// void MainGUIWindow::on_buttonResetMissed_clicked()
-// {
-//     ui->LCDMissedMes->display(0);
-//     m_DoSomething.data=eResetCntMissedViconData;
-//     m_pPublisherDoSomething->publish(m_DoSomething);
-// }
-
-// void MainGUIWindow::on_buttonSetDefaultRateParams_clicked()
-// {
-//     setDefaultRateParameters();
-// }
-
-// void MainGUIWindow::on_SetpointHome_clicked()
-// {
-//     ui->SetpointX->setValue(0);
-//     ui->SetpointY->setValue(-700);
-//     ui->SetpointZ->setValue(1200);
-
-//     publishSetpoint();
-// }
-
-// void MainGUIWindow::on_setpointZ200_clicked()
-// {
-//     ui->SetpointZ->setValue(200);
-
-//     publishSetpoint();
-// }
-
-
-
-
-// void MainGUIWindow::on_slideMotorCmdTest_valueChanged(int value)
-// {
-//     m_controllerParam.crazyControllerType=eMotorCmdTest;
-//     m_controllerParam.basicControllerType=eTestMotorCmd;
-//     m_controllerParam.value=value*600;
-//     m_pPublisherControllerParam->publish(m_controllerParam);
-//     ui->LCDMotorCmdTest->display(value*600);
-// }
-
-
-
-// void MainGUIWindow::on_slideRollAngleTest_valueChanged(int value)
-// {
-//     m_controllerParam.crazyControllerType=eAngleCmdTest;
-//     m_controllerParam.basicControllerType=eTestRoll;
-//     m_controllerParam.value=value*0.008;
-//     m_pPublisherControllerParam->publish(m_controllerParam);
-//     ui->LCDRollAngleTest->display(value*0.008*RAD2DEG);
-// }
-
-// void MainGUIWindow::on_slidePitchAngleTest_valueChanged(int value)
-// {
-//     m_controllerParam.crazyControllerType=eAngleCmdTest;
-//     m_controllerParam.basicControllerType=eTestPitch;
-//     m_controllerParam.value=value*0.008;
-//     m_pPublisherControllerParam->publish(m_controllerParam);
-//     ui->LCDPitchAngleTest->display(value*0.008*RAD2DEG);
-// }
-
-// void MainGUIWindow::on_slideYawAngleTest_valueChanged(int value)
-// {
-//     m_controllerParam.crazyControllerType=eAngleCmdTest;
-//     m_controllerParam.basicControllerType=eTestYaw;
-//     m_controllerParam.value=value*0.03;
-//     m_pPublisherControllerParam->publish(m_controllerParam);
-//     ui->LCDYawAngleTest->display(value*0.03*RAD2DEG);
-// }
-
-// void MainGUIWindow::on_slideRollRateTest_valueChanged(int value)
-// {
-//     m_controllerParam.crazyControllerType=eRateCmdTest;
-//     m_controllerParam.basicControllerType=eTestRoll;
-//     m_controllerParam.value=value*0.005;
-//     m_pPublisherControllerParam->publish(m_controllerParam);
-//     ui->LCDRollRateTest->display(value*0.005*RAD2DEG);
-// }
-
-// void MainGUIWindow::on_slidePitchRateTest_valueChanged(int value)
-// {
-//     m_controllerParam.crazyControllerType=eRateCmdTest;
-//     m_controllerParam.basicControllerType=eTestPitch;
-//     m_controllerParam.value=value*0.005;
-//     m_pPublisherControllerParam->publish(m_controllerParam);
-//     ui->LCDPitchRateTest->display(value*0.005*RAD2DEG);
-// }
-
-// void MainGUIWindow::on_slideYawRateTest_valueChanged(int value)
-// {
-//     m_controllerParam.crazyControllerType=eRateCmdTest;
-//     m_controllerParam.basicControllerType=eTestYaw;
-//     m_controllerParam.value=value*0.005;
-//     m_pPublisherControllerParam->publish(m_controllerParam);
-//     ui->LCDYawRateTest->display(value*0.005*RAD2DEG);
-// }
-
-
-CSetpointQueue::CSetpointQueue()
-{
-    startElem=NULL;
-    currElem=NULL;
-    lastElem=NULL;
-}
-
-void CSetpointQueue::insert(setpoint newElem)
-{
-    if (startElem==NULL)
-    {
-        startElem=new QueueElem(newElem);
-        lastElem=startElem;
-        currElem=startElem;
-    }
-    else
-    {
-    lastElem->next=new QueueElem(newElem);
-    lastElem=lastElem->next;
-    }
-}
-setpoint CSetpointQueue::getNext()
-{
-    setpoint ret;
-    ret.x=currElem->elem.x;
-    ret.y=currElem->elem.y;
-    ret.z=currElem->elem.z;
-    ret.yaw=currElem->elem.yaw;
-
-    if(currElem->next!=NULL)
-        currElem=currElem->next;
-    else currElem=startElem;
-
-    return ret;
-}
-
-void CSetpointQueue::print()
-{
-    QueueElem* p=startElem;
-
-    ROS_INFO_STREAM("queue elements: ");
-    int cnt=0;
-    while (p!=NULL)
-    {
-        cnt++;
-        ROS_INFO_STREAM("element "<<cnt<<": "<<"x="<<p->elem.x<<" y="<<p->elem.y<<" z="<<p->elem.z<<" yaw="<<p->elem.yaw);
-        p=p->next;
-    }
-}
-
-// void MainGUIWindow::initSetpointQueues()
-// {
-//     setpoint sp;
-
-//     int pointsCnt=500;
-//     for(int i=0; i<pointsCnt; i++)
-//     {
-//         double angle=i*2*3.141592/pointsCnt;
-//         sp.x=cos(angle)*0.7;
-//         sp.y=sin(angle)*0.7;
-//         sp.z=1;
-//         double yaw=PI/2+angle;
-//         while(yaw>PI) yaw-=2*PI;
-//         while(yaw<-PI) yaw+=2*PI;
-//         //sp.yaw=yaw;
-//         sp.yaw=0;
-//         m_trajCircle.insert(sp);
-//     }
-
-// //    for(int i=0; i<pointsCnt; i++)
-// //    {
-
-// //        sp.x=0;
-// //        sp.y=0.1;
-// //        sp.z=1.123;
-// //        sp.yaw=0;
-
-// //        m_trajSquare.insert(sp);
-// //    }
-
-//     sp.y=0;
-//     sp.yaw=0;
-
-
-//     sp.x=-0.250; sp.z=0.500; m_trajSquare.insert(sp);
-
-//     //sp.x=-0.550; sp.z=0.500; m_trajSquare.insert(sp);
-//     sp.x=-0.750; sp.z=0.500; m_trajSquare.insert(sp);
-
-//     //sp.x=-0.750; sp.z=0.750; m_trajSquare.insert(sp);
-//     sp.x=-0.750; sp.z=1; m_trajSquare.insert(sp);
-
-//     //sp.x=-0.500; sp.z=1; m_trajSquare.insert(sp);
-//     sp.x=-0.250; sp.z=1; m_trajSquare.insert(sp);
-
-//     //sp.x=-0.500; sp.z=1; m_trajSquare.insert(sp);
-//     sp.x=-0.750; sp.z=1; m_trajSquare.insert(sp);
-
-//     sp.x=-0.750; sp.z=1.5; m_trajSquare.insert(sp);
-
-//     sp.x=0; sp.z=1.5; m_trajSquare.insert(sp);
-
-//     sp.x=0; sp.z=0.500; m_trajSquare.insert(sp);
-
-//     sp.x=0; sp.z=1.5; m_trajSquare.insert(sp);
-
-//     sp.x=0.250; sp.z=1.5; m_trajSquare.insert(sp);
-
-//     sp.x=0.250; sp.z=0.500; m_trajSquare.insert(sp);
-
-//     sp.x=0.250; sp.z=1; m_trajSquare.insert(sp);
-
-//     sp.x=0.750; sp.z=1; m_trajSquare.insert(sp);
-
-//     sp.x=0.750; sp.z=1.5; m_trajSquare.insert(sp);
-
-//     sp.x=0.750; sp.z=0.500; m_trajSquare.insert(sp);
-
-//     m_trajSquare.print();
-
-// }
-
-// void MainGUIWindow::updateSetpoint()
-// {
-//   if(m_trajectoryType==eTrajCustom)
-//       return;
-
-//   double currX,currY,spX,spY,currZ,spZ;
-//   currX=ui->LCDViconDataX->value();
-//   currY=ui->LCDViconDataY->value();
-//   currZ=ui->LCDViconDataZ->value();
-
-//   spX=ui->SetpointX->value();
-//   spY=ui->SetpointY->value();
-//   spZ=ui->SetpointZ->value();
-
-//   while(sqrt((spX-currX)*(spX-currX)+(spY-currY)*(spY-currY)+(spZ-currZ)*(spZ-currZ))<40)
-// {
-
-//   setpoint sp;
-
-//     if(m_trajectoryType==eTrajCircle)
-//         sp=m_trajCircle.getNext();
-//     if(m_trajectoryType==eTrajSquare)
-//         sp=m_trajSquare.getNext();
-
-//     ui->SetpointX->setValue(sp.x*1000);
-//     ui->SetpointY->setValue(sp.y*1000);
-//     ui->SetpointZ->setValue(sp.z*1000);
-//     ui->SetpointYaw->setValue(sp.yaw*RAD2DEG);
-
-//     publishSetpoint();
-
-//     spX=ui->SetpointX->value();
-//     spY=ui->SetpointY->value();
-//     spZ=ui->SetpointZ->value();
-// }
-
-// }
-
-
-
-
-// void MainGUIWindow::on_buttonStop_2_clicked()
-// {
-
-//         if(!m_isCalActive)
-//         {
-//             m_isCalActive=true;
-//             m_DoSomething.data=eStartCal;
-//             m_pPublisherDoSomething->publish(m_DoSomething);
-//             ui->buttonStop_2->setText("STOP cal");
-//         }
-//         else
-//         {
-//             m_isCalActive=false;
-//             m_DoSomething.data=eStopCal;
-//             m_pPublisherDoSomething->publish(m_DoSomething);
-//             ui->buttonStop_2->setText("START cal");
-//         }
-
-
-// }
-#endif  // DEBUG_GUI
-void MainGUIWindow::on_spinBoxNumCrazyflies_valueChanged(int arg1)
-{
-    
-}
-
diff --git a/crazyflie_ws/sandbox/crazypkg/gui/untitled/Makefile b/crazyflie_ws/sandbox/crazypkg/gui/untitled/Makefile
index 5f88bcc27d48ed31bfe20428dbb30de256866e17..538c30862944a8478c7513a0bacc354f852bc63a 100644
--- a/crazyflie_ws/sandbox/crazypkg/gui/untitled/Makefile
+++ b/crazyflie_ws/sandbox/crazypkg/gui/untitled/Makefile
@@ -49,10 +49,14 @@ OBJECTS_DIR   = ./
 ####### Files
 
 SOURCES       = main.cpp \
-		mainguiwindow.cpp moc_mainguiwindow.cpp
+		mainguiwindow.cpp \
+		myGraphicsScene.cpp moc_mainguiwindow.cpp \
+		moc_myGraphicsScene.cpp
 OBJECTS       = main.o \
 		mainguiwindow.o \
-		moc_mainguiwindow.o
+		myGraphicsScene.o \
+		moc_mainguiwindow.o \
+		moc_myGraphicsScene.o
 DIST          = /home/gazebo-cf/Qt5.7.0/5.7/gcc_64/mkspecs/features/spec_pre.prf \
 		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/mkspecs/common/unix.conf \
 		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/mkspecs/common/linux.conf \
@@ -184,6 +188,7 @@ DIST          = /home/gazebo-cf/Qt5.7.0/5.7/gcc_64/mkspecs/features/spec_pre.prf
 		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/mkspecs/features/qt_config.prf \
 		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/mkspecs/linux-g++/qmake.conf \
 		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/mkspecs/features/spec_post.prf \
+		.qmake.stash \
 		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/mkspecs/features/exclusive_builds.prf \
 		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/mkspecs/features/default_pre.prf \
 		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/mkspecs/features/resolve_config.prf \
@@ -201,8 +206,10 @@ DIST          = /home/gazebo-cf/Qt5.7.0/5.7/gcc_64/mkspecs/features/spec_pre.prf
 		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/mkspecs/features/exceptions.prf \
 		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/mkspecs/features/yacc.prf \
 		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/mkspecs/features/lex.prf \
-		untitled.pro mainguiwindow.h main.cpp \
-		mainguiwindow.cpp
+		untitled.pro mainguiwindow.h \
+		myGraphicsScene.h main.cpp \
+		mainguiwindow.cpp \
+		myGraphicsScene.cpp
 QMAKE_TARGET  = untitled
 DESTDIR       = 
 TARGET        = untitled
@@ -345,6 +352,7 @@ Makefile: untitled.pro /home/gazebo-cf/Qt5.7.0/5.7/gcc_64/mkspecs/linux-g++/qmak
 		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/mkspecs/features/qt_config.prf \
 		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/mkspecs/linux-g++/qmake.conf \
 		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/mkspecs/features/spec_post.prf \
+		.qmake.stash \
 		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/mkspecs/features/exclusive_builds.prf \
 		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/mkspecs/features/default_pre.prf \
 		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/mkspecs/features/resolve_config.prf \
@@ -498,6 +506,7 @@ Makefile: untitled.pro /home/gazebo-cf/Qt5.7.0/5.7/gcc_64/mkspecs/linux-g++/qmak
 /home/gazebo-cf/Qt5.7.0/5.7/gcc_64/mkspecs/features/qt_config.prf:
 /home/gazebo-cf/Qt5.7.0/5.7/gcc_64/mkspecs/linux-g++/qmake.conf:
 /home/gazebo-cf/Qt5.7.0/5.7/gcc_64/mkspecs/features/spec_post.prf:
+.qmake.stash:
 /home/gazebo-cf/Qt5.7.0/5.7/gcc_64/mkspecs/features/exclusive_builds.prf:
 /home/gazebo-cf/Qt5.7.0/5.7/gcc_64/mkspecs/features/default_pre.prf:
 /home/gazebo-cf/Qt5.7.0/5.7/gcc_64/mkspecs/features/resolve_config.prf:
@@ -533,8 +542,8 @@ dist: distdir FORCE
 distdir: FORCE
 	@test -d $(DISTDIR) || mkdir -p $(DISTDIR)
 	$(COPY_FILE) --parents $(DIST) $(DISTDIR)/
-	$(COPY_FILE) --parents mainguiwindow.h $(DISTDIR)/
-	$(COPY_FILE) --parents main.cpp mainguiwindow.cpp $(DISTDIR)/
+	$(COPY_FILE) --parents mainguiwindow.h myGraphicsScene.h $(DISTDIR)/
+	$(COPY_FILE) --parents main.cpp mainguiwindow.cpp myGraphicsScene.cpp $(DISTDIR)/
 	$(COPY_FILE) --parents mainguiwindow.ui $(DISTDIR)/
 
 
@@ -561,9 +570,9 @@ benchmark: first
 
 compiler_rcc_make_all:
 compiler_rcc_clean:
-compiler_moc_header_make_all: moc_mainguiwindow.cpp
+compiler_moc_header_make_all: moc_mainguiwindow.cpp moc_myGraphicsScene.cpp
 compiler_moc_header_clean:
-	-$(DEL_FILE) moc_mainguiwindow.cpp
+	-$(DEL_FILE) moc_mainguiwindow.cpp moc_myGraphicsScene.cpp
 moc_mainguiwindow.cpp: /home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/QMainWindow \
 		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qmainwindow.h \
 		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qwidget.h \
@@ -667,11 +676,149 @@ moc_mainguiwindow.cpp: /home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/QMai
 		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/QTimer \
 		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qtimer.h \
 		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qbasictimer.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/QGridLayout \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qgridlayout.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qlayout.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qlayoutitem.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qboxlayout.h \
 		ui_mainguiwindow.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/QVariant \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/QAction \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qaction.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qactiongroup.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/QApplication \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qapplication.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qcoreapplication.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qeventloop.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qdesktopwidget.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qguiapplication.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qinputmethod.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/QButtonGroup \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qbuttongroup.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/QFrame \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qframe.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/QGraphicsView \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qgraphicsview.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qpainter.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qtextoption.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qpen.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qscrollarea.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qabstractscrollarea.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qgraphicsscene.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/QHeaderView \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qheaderview.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qabstractitemview.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qabstractitemmodel.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qitemselectionmodel.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qabstractitemdelegate.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qstyleoption.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qabstractspinbox.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qvalidator.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qregularexpression.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qslider.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qabstractslider.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qstyle.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qtabbar.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qrubberband.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/QMenuBar \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qmenubar.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qmenu.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/QSpinBox \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qspinbox.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/QStatusBar \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qstatusbar.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/QTabWidget \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/QToolBar \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qtoolbar.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/QWidget \
+		myGraphicsScene.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/QGraphicsScene \
 		mainguiwindow.h \
 		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/bin/moc
 	/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/bin/moc $(DEFINES) -I/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/mkspecs/linux-g++ -I/home/gazebo-cf/work/D-FaLL-System/crazyflie_ws/sandbox/crazypkg/gui/untitled -I/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include -I/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets -I/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui -I/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore -I/usr/include/c++/5 -I/usr/include/x86_64-linux-gnu/c++/5 -I/usr/include/c++/5/backward -I/usr/lib/gcc/x86_64-linux-gnu/5/include -I/usr/local/include -I/usr/lib/gcc/x86_64-linux-gnu/5/include-fixed -I/usr/include/x86_64-linux-gnu -I/usr/include mainguiwindow.h -o moc_mainguiwindow.cpp
 
+moc_myGraphicsScene.cpp: /home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/QGraphicsScene \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qgraphicsscene.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qobject.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qobjectdefs.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qnamespace.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qglobal.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qconfig.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qfeatures.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qsystemdetection.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qprocessordetection.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qcompilerdetection.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qtypeinfo.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qtypetraits.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qisenum.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qsysinfo.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qlogging.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qflags.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qatomic.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qbasicatomic.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qatomic_bootstrap.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qgenericatomic.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qatomic_cxx11.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qatomic_msvc.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qglobalstatic.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qmutex.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qnumeric.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qversiontagging.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qobjectdefs_impl.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qstring.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qchar.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qbytearray.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qrefcount.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qarraydata.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qstringbuilder.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qlist.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qalgorithms.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qiterator.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qhashfunctions.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qpair.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qbytearraylist.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qstringlist.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qregexp.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qstringmatcher.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qcoreevent.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qscopedpointer.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qmetatype.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qvarlengtharray.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qcontainerfwd.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qobject_impl.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qpoint.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qrect.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qmargins.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qsize.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qbrush.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qvector.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qcolor.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qrgb.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qrgba64.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qmatrix.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qpolygon.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qregion.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qwindowdefs.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qwindowdefs_win.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qdatastream.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qiodevice.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qline.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qtransform.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qpainterpath.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qimage.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qpaintdevice.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qpixelformat.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qpixmap.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qsharedpointer.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qshareddata.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qhash.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qsharedpointer_impl.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qfont.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qpen.h \
+		myGraphicsScene.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/bin/moc
+	/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/bin/moc $(DEFINES) -I/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/mkspecs/linux-g++ -I/home/gazebo-cf/work/D-FaLL-System/crazyflie_ws/sandbox/crazypkg/gui/untitled -I/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include -I/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets -I/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui -I/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore -I/usr/include/c++/5 -I/usr/include/x86_64-linux-gnu/c++/5 -I/usr/include/c++/5/backward -I/usr/lib/gcc/x86_64-linux-gnu/5/include -I/usr/local/include -I/usr/lib/gcc/x86_64-linux-gnu/5/include-fixed -I/usr/include/x86_64-linux-gnu -I/usr/include myGraphicsScene.h -o moc_myGraphicsScene.cpp
+
 compiler_moc_source_make_all:
 compiler_moc_source_clean:
 compiler_uic_make_all: ui_mainguiwindow.h
@@ -795,14 +942,63 @@ main.o: main.cpp mainguiwindow.h \
 		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/QTimer \
 		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qtimer.h \
 		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qbasictimer.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/QGridLayout \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qgridlayout.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qlayout.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qlayoutitem.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qboxlayout.h \
 		ui_mainguiwindow.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/QVariant \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/QAction \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qaction.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qactiongroup.h \
 		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/QApplication \
 		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qapplication.h \
 		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qcoreapplication.h \
 		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qeventloop.h \
 		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qdesktopwidget.h \
 		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qguiapplication.h \
-		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qinputmethod.h
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qinputmethod.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/QButtonGroup \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qbuttongroup.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/QFrame \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qframe.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/QGraphicsView \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qgraphicsview.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qpainter.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qtextoption.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qpen.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qscrollarea.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qabstractscrollarea.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qgraphicsscene.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/QHeaderView \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qheaderview.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qabstractitemview.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qabstractitemmodel.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qitemselectionmodel.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qabstractitemdelegate.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qstyleoption.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qabstractspinbox.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qvalidator.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qregularexpression.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qslider.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qabstractslider.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qstyle.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qtabbar.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qrubberband.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/QMenuBar \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qmenubar.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qmenu.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/QSpinBox \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qspinbox.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/QStatusBar \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qstatusbar.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/QTabWidget \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/QToolBar \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qtoolbar.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/QWidget \
+		myGraphicsScene.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/QGraphicsScene
 	$(CXX) -c $(CXXFLAGS) $(INCPATH) -o main.o main.cpp
 
 mainguiwindow.o: mainguiwindow.cpp mainguiwindow.h \
@@ -909,26 +1105,162 @@ mainguiwindow.o: mainguiwindow.cpp mainguiwindow.h \
 		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/QTimer \
 		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qtimer.h \
 		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qbasictimer.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/QGridLayout \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qgridlayout.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qlayout.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qlayoutitem.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qboxlayout.h \
 		ui_mainguiwindow.h \
-		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/QDoubleSpinBox \
-		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qspinbox.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/QVariant \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/QAction \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qaction.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qactiongroup.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/QApplication \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qapplication.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qcoreapplication.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qeventloop.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qdesktopwidget.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qguiapplication.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qinputmethod.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/QButtonGroup \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qbuttongroup.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/QFrame \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qframe.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/QGraphicsView \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qgraphicsview.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qpainter.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qtextoption.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qpen.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qscrollarea.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qabstractscrollarea.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qgraphicsscene.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/QHeaderView \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qheaderview.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qabstractitemview.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qabstractitemmodel.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qitemselectionmodel.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qabstractitemdelegate.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qstyleoption.h \
 		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qabstractspinbox.h \
 		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qvalidator.h \
 		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qregularexpression.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qslider.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qabstractslider.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qstyle.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qtabbar.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qrubberband.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/QMenuBar \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qmenubar.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qmenu.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/QSpinBox \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qspinbox.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/QStatusBar \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qstatusbar.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/QTabWidget \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/QToolBar \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qtoolbar.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/QWidget \
+		myGraphicsScene.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/QGraphicsScene \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/QDoubleSpinBox \
 		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/QTextEdit \
 		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qtextedit.h \
-		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qabstractscrollarea.h \
-		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qframe.h \
 		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qtextdocument.h \
-		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qtextoption.h \
 		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qtextcursor.h \
 		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qtextformat.h \
-		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qpen.h
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/QString
 	$(CXX) -c $(CXXFLAGS) $(INCPATH) -o mainguiwindow.o mainguiwindow.cpp
 
+myGraphicsScene.o: myGraphicsScene.cpp myGraphicsScene.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/QGraphicsScene \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qgraphicsscene.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qobject.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qobjectdefs.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qnamespace.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qglobal.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qconfig.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qfeatures.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qsystemdetection.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qprocessordetection.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qcompilerdetection.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qtypeinfo.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qtypetraits.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qisenum.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qsysinfo.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qlogging.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qflags.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qatomic.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qbasicatomic.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qatomic_bootstrap.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qgenericatomic.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qatomic_cxx11.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qatomic_msvc.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qglobalstatic.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qmutex.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qnumeric.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qversiontagging.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qobjectdefs_impl.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qstring.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qchar.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qbytearray.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qrefcount.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qarraydata.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qstringbuilder.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qlist.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qalgorithms.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qiterator.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qhashfunctions.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qpair.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qbytearraylist.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qstringlist.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qregexp.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qstringmatcher.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qcoreevent.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qscopedpointer.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qmetatype.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qvarlengtharray.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qcontainerfwd.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qobject_impl.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qpoint.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qrect.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qmargins.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qsize.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qbrush.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qvector.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qcolor.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qrgb.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qrgba64.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qmatrix.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qpolygon.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qregion.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qwindowdefs.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qwindowdefs_win.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qdatastream.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qiodevice.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qline.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qtransform.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qpainterpath.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qimage.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qpaintdevice.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qpixelformat.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qpixmap.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qsharedpointer.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qshareddata.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qhash.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qsharedpointer_impl.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qfont.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtGui/qpen.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/QGraphicsSceneMouseEvent \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtWidgets/qgraphicssceneevent.h \
+		/home/gazebo-cf/Qt5.7.0/5.7/gcc_64/include/QtCore/qset.h
+	$(CXX) -c $(CXXFLAGS) $(INCPATH) -o myGraphicsScene.o myGraphicsScene.cpp
+
 moc_mainguiwindow.o: moc_mainguiwindow.cpp 
 	$(CXX) -c $(CXXFLAGS) $(INCPATH) -o moc_mainguiwindow.o moc_mainguiwindow.cpp
 
+moc_myGraphicsScene.o: moc_myGraphicsScene.cpp 
+	$(CXX) -c $(CXXFLAGS) $(INCPATH) -o moc_myGraphicsScene.o moc_myGraphicsScene.cpp
+
 ####### Install
 
 install:  FORCE
diff --git a/crazyflie_ws/sandbox/crazypkg/gui/untitled/images/brick.png b/crazyflie_ws/sandbox/crazypkg/gui/untitled/images/brick.png
new file mode 100644
index 0000000000000000000000000000000000000000..87e7df5b0426e136be5cadaa2459f082601cd233
Binary files /dev/null and b/crazyflie_ws/sandbox/crazypkg/gui/untitled/images/brick.png differ
diff --git a/crazyflie_ws/sandbox/crazypkg/gui/untitled/images/qt-logo.png b/crazyflie_ws/sandbox/crazypkg/gui/untitled/images/qt-logo.png
new file mode 100644
index 0000000000000000000000000000000000000000..abfc8caadbbcd3d1c57ad4e15db32216b23f7e1d
Binary files /dev/null and b/crazyflie_ws/sandbox/crazypkg/gui/untitled/images/qt-logo.png differ
diff --git a/crazyflie_ws/sandbox/crazypkg/gui/untitled/main.o b/crazyflie_ws/sandbox/crazypkg/gui/untitled/main.o
index e92c558f3f8d00033b533a792591e4640456cfdb..0a3ad2ccf7d8622d495ea71699a78307060cf097 100644
Binary files a/crazyflie_ws/sandbox/crazypkg/gui/untitled/main.o and b/crazyflie_ws/sandbox/crazypkg/gui/untitled/main.o differ
diff --git a/crazyflie_ws/sandbox/crazypkg/gui/untitled/mainguiwindow.cpp b/crazyflie_ws/sandbox/crazypkg/gui/untitled/mainguiwindow.cpp
index fea3b92b01517806a68333384cd1236c498abe1f..4ae469dc9d380187a9f9853106c81ab9c986d3d6 100644
--- a/crazyflie_ws/sandbox/crazypkg/gui/untitled/mainguiwindow.cpp
+++ b/crazyflie_ws/sandbox/crazypkg/gui/untitled/mainguiwindow.cpp
@@ -1,5 +1,6 @@
 #include "mainguiwindow.h"
 #include "ui_mainguiwindow.h"
+
 #include <QDoubleSpinBox>
 #include <QTextEdit>
 #include <QString>
@@ -27,6 +28,7 @@ MainGUIWindow::MainGUIWindow(QWidget *parent) :
     QMainWindow(parent),
     ui(new Ui::MainGUIWindow)
 {
+
     ui->setupUi(this);
     _init();
 }
@@ -52,6 +54,14 @@ void MainGUIWindow::_refresh_tabs()
 
 void MainGUIWindow::_init()
 {
+
+    scene = new myGraphicsScene(this);
+
+    ui->graphicsView->setScene(scene);
+    QRect rect(10, 20, 80, 60);
+    scene->addText("Hello world!");
+    scene->addRect(rect, QPen(Qt::black), QBrush(Qt::blue));
+
     ui->spinBoxNumCrazyflies->setMaximum(N_MAX_CRAZYFLIES);
     _refresh_tabs();
 }
@@ -930,10 +940,15 @@ void CSetpointQueue::print()
 #endif  // DEBUG_GUI
 void MainGUIWindow::on_spinBoxNumCrazyflies_valueChanged(int arg1)
 {
-
+    _refresh_tabs();
 }
 
 void MainGUIWindow::on_spinBoxNumCrazyflies_editingFinished()
 {
-    _refresh_tabs();
+
+}
+
+void MainGUIWindow::on_graphicsView_rubberBandChanged(const QRect &viewportRect, const QPointF &fromScenePoint, const QPointF &toScenePoint)
+{
+
 }
diff --git a/crazyflie_ws/sandbox/crazypkg/gui/untitled/mainguiwindow.h b/crazyflie_ws/sandbox/crazypkg/gui/untitled/mainguiwindow.h
index 290a7fbeaef31b6519d24dac5b74052e2e61052d..06e12488ea49607a3a96959f54018df6d8b00218 100644
--- a/crazyflie_ws/sandbox/crazypkg/gui/untitled/mainguiwindow.h
+++ b/crazyflie_ws/sandbox/crazypkg/gui/untitled/mainguiwindow.h
@@ -5,6 +5,7 @@
 
 #include <QMainWindow>
 #include <QTimer>
+#include <QGridLayout>
 
 #ifndef DEBUG_GUI
 #include "ros/callback_queue.h"
@@ -12,7 +13,7 @@
 #include "CrazyFlieTypes.h"
 #endif
 #include "ui_mainguiwindow.h"
-
+#include "myGraphicsScene.h"
 
 namespace Ui {
 class MainGUIWindow;
@@ -130,9 +131,14 @@ private slots:
 
     void on_spinBoxNumCrazyflies_editingFinished();
 
+    void on_graphicsView_rubberBandChanged(const QRect &viewportRect, const QPointF &fromScenePoint, const QPointF &toScenePoint);
+
 private:
 
     Ui::MainGUIWindow *ui;
+    myGraphicsScene* scene;      //TODO: make a subclass from QGraphicScene class, mouse events
+
+
     void _init();
     void _refresh_tabs();
 
diff --git a/crazyflie_ws/sandbox/crazypkg/gui/untitled/mainguiwindow.o b/crazyflie_ws/sandbox/crazypkg/gui/untitled/mainguiwindow.o
index b8255eecd5ea167849fef1018ac18fa34dc2288a..5842b899542a3775b789b2c120dad92f85ad3f85 100644
Binary files a/crazyflie_ws/sandbox/crazypkg/gui/untitled/mainguiwindow.o and b/crazyflie_ws/sandbox/crazypkg/gui/untitled/mainguiwindow.o differ
diff --git a/crazyflie_ws/sandbox/crazypkg/gui/untitled/mainguiwindow.ui b/crazyflie_ws/sandbox/crazypkg/gui/untitled/mainguiwindow.ui
index 25439a706ee69ab7c1357df60e0f63426e0e414f..2cb2cf14efcb9de36b6a0019ff1f9353a9639d3f 100644
--- a/crazyflie_ws/sandbox/crazypkg/gui/untitled/mainguiwindow.ui
+++ b/crazyflie_ws/sandbox/crazypkg/gui/untitled/mainguiwindow.ui
@@ -46,13 +46,13 @@
      </property>
     </widget>
    </widget>
-   <widget class="QFrame" name="frame_2">
+   <widget class="QFrame" name="frame_drawing">
     <property name="geometry">
      <rect>
-      <x>40</x>
-      <y>10</y>
-      <width>571</width>
-      <height>481</height>
+      <x>30</x>
+      <y>20</y>
+      <width>582</width>
+      <height>469</height>
      </rect>
     </property>
     <property name="frameShape">
@@ -61,14 +61,27 @@
     <property name="frameShadow">
      <enum>QFrame::Raised</enum>
     </property>
+    <widget class="QGraphicsView" name="graphicsView">
+     <property name="geometry">
+      <rect>
+       <x>30</x>
+       <y>20</y>
+       <width>531</width>
+       <height>430</height>
+      </rect>
+     </property>
+     <property name="mouseTracking">
+      <bool>true</bool>
+     </property>
+    </widget>
    </widget>
    <widget class="QFrame" name="frame_3">
     <property name="geometry">
      <rect>
-      <x>630</x>
-      <y>10</y>
-      <width>791</width>
-      <height>481</height>
+      <x>629</x>
+      <y>21</y>
+      <width>581</width>
+      <height>469</height>
      </rect>
     </property>
     <property name="frameShape">
diff --git a/crazyflie_ws/sandbox/crazypkg/gui/untitled/moc_mainguiwindow.cpp b/crazyflie_ws/sandbox/crazypkg/gui/untitled/moc_mainguiwindow.cpp
index fe111b0b56a97dfc6531413855871f680c823520..2244cac40190f184987908dfa5fa092650a4417d 100644
--- a/crazyflie_ws/sandbox/crazypkg/gui/untitled/moc_mainguiwindow.cpp
+++ b/crazyflie_ws/sandbox/crazypkg/gui/untitled/moc_mainguiwindow.cpp
@@ -19,8 +19,8 @@
 
 QT_BEGIN_MOC_NAMESPACE
 struct qt_meta_stringdata_MainGUIWindow_t {
-    QByteArrayData data[5];
-    char stringdata0[97];
+    QByteArrayData data[9];
+    char stringdata0[172];
 };
 #define QT_MOC_LITERAL(idx, ofs, len) \
     Q_STATIC_BYTE_ARRAY_DATA_HEADER_INITIALIZER_WITH_OFFSET(len, \
@@ -33,11 +33,17 @@ QT_MOC_LITERAL(0, 0, 13), // "MainGUIWindow"
 QT_MOC_LITERAL(1, 14, 36), // "on_spinBoxNumCrazyflies_value..."
 QT_MOC_LITERAL(2, 51, 0), // ""
 QT_MOC_LITERAL(3, 52, 4), // "arg1"
-QT_MOC_LITERAL(4, 57, 39) // "on_spinBoxNumCrazyflies_editi..."
+QT_MOC_LITERAL(4, 57, 39), // "on_spinBoxNumCrazyflies_editi..."
+QT_MOC_LITERAL(5, 97, 33), // "on_graphicsView_rubberBandCha..."
+QT_MOC_LITERAL(6, 131, 12), // "viewportRect"
+QT_MOC_LITERAL(7, 144, 14), // "fromScenePoint"
+QT_MOC_LITERAL(8, 159, 12) // "toScenePoint"
 
     },
     "MainGUIWindow\0on_spinBoxNumCrazyflies_valueChanged\0"
-    "\0arg1\0on_spinBoxNumCrazyflies_editingFinished"
+    "\0arg1\0on_spinBoxNumCrazyflies_editingFinished\0"
+    "on_graphicsView_rubberBandChanged\0"
+    "viewportRect\0fromScenePoint\0toScenePoint"
 };
 #undef QT_MOC_LITERAL
 
@@ -47,7 +53,7 @@ static const uint qt_meta_data_MainGUIWindow[] = {
        7,       // revision
        0,       // classname
        0,    0, // classinfo
-       2,   14, // methods
+       3,   14, // methods
        0,    0, // properties
        0,    0, // enums/sets
        0,    0, // constructors
@@ -55,12 +61,14 @@ static const uint qt_meta_data_MainGUIWindow[] = {
        0,       // signalCount
 
  // slots: name, argc, parameters, tag, flags
-       1,    1,   24,    2, 0x08 /* Private */,
-       4,    0,   27,    2, 0x08 /* Private */,
+       1,    1,   29,    2, 0x08 /* Private */,
+       4,    0,   32,    2, 0x08 /* Private */,
+       5,    3,   33,    2, 0x08 /* Private */,
 
  // slots: parameters
     QMetaType::Void, QMetaType::Int,    3,
     QMetaType::Void,
+    QMetaType::Void, QMetaType::QRect, QMetaType::QPointF, QMetaType::QPointF,    6,    7,    8,
 
        0        // eod
 };
@@ -73,6 +81,7 @@ void MainGUIWindow::qt_static_metacall(QObject *_o, QMetaObject::Call _c, int _i
         switch (_id) {
         case 0: _t->on_spinBoxNumCrazyflies_valueChanged((*reinterpret_cast< int(*)>(_a[1]))); break;
         case 1: _t->on_spinBoxNumCrazyflies_editingFinished(); break;
+        case 2: _t->on_graphicsView_rubberBandChanged((*reinterpret_cast< const QRect(*)>(_a[1])),(*reinterpret_cast< const QPointF(*)>(_a[2])),(*reinterpret_cast< const QPointF(*)>(_a[3]))); break;
         default: ;
         }
     }
@@ -103,13 +112,13 @@ int MainGUIWindow::qt_metacall(QMetaObject::Call _c, int _id, void **_a)
     if (_id < 0)
         return _id;
     if (_c == QMetaObject::InvokeMetaMethod) {
-        if (_id < 2)
+        if (_id < 3)
             qt_static_metacall(this, _c, _id, _a);
-        _id -= 2;
+        _id -= 3;
     } else if (_c == QMetaObject::RegisterMethodArgumentMetaType) {
-        if (_id < 2)
+        if (_id < 3)
             *reinterpret_cast<int*>(_a[0]) = -1;
-        _id -= 2;
+        _id -= 3;
     }
     return _id;
 }
diff --git a/crazyflie_ws/sandbox/crazypkg/gui/untitled/moc_mainguiwindow.o b/crazyflie_ws/sandbox/crazypkg/gui/untitled/moc_mainguiwindow.o
index 796aab7c2d24669cc667daba1a5663a5a47dbca6..fcaa169895bc162359958b332d835fff7ad863f7 100644
Binary files a/crazyflie_ws/sandbox/crazypkg/gui/untitled/moc_mainguiwindow.o and b/crazyflie_ws/sandbox/crazypkg/gui/untitled/moc_mainguiwindow.o differ
diff --git a/crazyflie_ws/sandbox/crazypkg/gui/untitled/moc_myGraphicsScene.cpp b/crazyflie_ws/sandbox/crazypkg/gui/untitled/moc_myGraphicsScene.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3e763a3b383d25b431ea495871e4f53b4984bdc7
--- /dev/null
+++ b/crazyflie_ws/sandbox/crazypkg/gui/untitled/moc_myGraphicsScene.cpp
@@ -0,0 +1,89 @@
+/****************************************************************************
+** Meta object code from reading C++ file 'myGraphicsScene.h'
+**
+** Created by: The Qt Meta Object Compiler version 67 (Qt 5.7.0)
+**
+** WARNING! All changes made in this file will be lost!
+*****************************************************************************/
+
+#include "myGraphicsScene.h"
+#include <QtCore/qbytearray.h>
+#include <QtCore/qmetatype.h>
+#if !defined(Q_MOC_OUTPUT_REVISION)
+#error "The header file 'myGraphicsScene.h' doesn't include <QObject>."
+#elif Q_MOC_OUTPUT_REVISION != 67
+#error "This file was generated using the moc from 5.7.0. It"
+#error "cannot be used with the include files from this version of Qt."
+#error "(The moc has changed too much.)"
+#endif
+
+QT_BEGIN_MOC_NAMESPACE
+struct qt_meta_stringdata_myGraphicsScene_t {
+    QByteArrayData data[1];
+    char stringdata0[16];
+};
+#define QT_MOC_LITERAL(idx, ofs, len) \
+    Q_STATIC_BYTE_ARRAY_DATA_HEADER_INITIALIZER_WITH_OFFSET(len, \
+    qptrdiff(offsetof(qt_meta_stringdata_myGraphicsScene_t, stringdata0) + ofs \
+        - idx * sizeof(QByteArrayData)) \
+    )
+static const qt_meta_stringdata_myGraphicsScene_t qt_meta_stringdata_myGraphicsScene = {
+    {
+QT_MOC_LITERAL(0, 0, 15) // "myGraphicsScene"
+
+    },
+    "myGraphicsScene"
+};
+#undef QT_MOC_LITERAL
+
+static const uint qt_meta_data_myGraphicsScene[] = {
+
+ // content:
+       7,       // revision
+       0,       // classname
+       0,    0, // classinfo
+       0,    0, // methods
+       0,    0, // properties
+       0,    0, // enums/sets
+       0,    0, // constructors
+       0,       // flags
+       0,       // signalCount
+
+       0        // eod
+};
+
+void myGraphicsScene::qt_static_metacall(QObject *_o, QMetaObject::Call _c, int _id, void **_a)
+{
+    Q_UNUSED(_o);
+    Q_UNUSED(_id);
+    Q_UNUSED(_c);
+    Q_UNUSED(_a);
+}
+
+const QMetaObject myGraphicsScene::staticMetaObject = {
+    { &QGraphicsScene::staticMetaObject, qt_meta_stringdata_myGraphicsScene.data,
+      qt_meta_data_myGraphicsScene,  qt_static_metacall, Q_NULLPTR, Q_NULLPTR}
+};
+
+
+const QMetaObject *myGraphicsScene::metaObject() const
+{
+    return QObject::d_ptr->metaObject ? QObject::d_ptr->dynamicMetaObject() : &staticMetaObject;
+}
+
+void *myGraphicsScene::qt_metacast(const char *_clname)
+{
+    if (!_clname) return Q_NULLPTR;
+    if (!strcmp(_clname, qt_meta_stringdata_myGraphicsScene.stringdata0))
+        return static_cast<void*>(const_cast< myGraphicsScene*>(this));
+    return QGraphicsScene::qt_metacast(_clname);
+}
+
+int myGraphicsScene::qt_metacall(QMetaObject::Call _c, int _id, void **_a)
+{
+    _id = QGraphicsScene::qt_metacall(_c, _id, _a);
+    if (_id < 0)
+        return _id;
+    return _id;
+}
+QT_END_MOC_NAMESPACE
diff --git a/crazyflie_ws/sandbox/crazypkg/gui/untitled/moc_myGraphicsScene.o b/crazyflie_ws/sandbox/crazypkg/gui/untitled/moc_myGraphicsScene.o
new file mode 100644
index 0000000000000000000000000000000000000000..10a7b46eedaac352de220e5ff84174cb6256dd81
Binary files /dev/null and b/crazyflie_ws/sandbox/crazypkg/gui/untitled/moc_myGraphicsScene.o differ
diff --git a/crazyflie_ws/sandbox/crazypkg/gui/untitled/moc_renderarea.cpp b/crazyflie_ws/sandbox/crazypkg/gui/untitled/moc_renderarea.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..363fae90b43d5381b75d2159286b00e83e00283f
--- /dev/null
+++ b/crazyflie_ws/sandbox/crazypkg/gui/untitled/moc_renderarea.cpp
@@ -0,0 +1,134 @@
+/****************************************************************************
+** Meta object code from reading C++ file 'renderarea.h'
+**
+** Created by: The Qt Meta Object Compiler version 67 (Qt 5.7.0)
+**
+** WARNING! All changes made in this file will be lost!
+*****************************************************************************/
+
+#include "renderarea.h"
+#include <QtCore/qbytearray.h>
+#include <QtCore/qmetatype.h>
+#if !defined(Q_MOC_OUTPUT_REVISION)
+#error "The header file 'renderarea.h' doesn't include <QObject>."
+#elif Q_MOC_OUTPUT_REVISION != 67
+#error "This file was generated using the moc from 5.7.0. It"
+#error "cannot be used with the include files from this version of Qt."
+#error "(The moc has changed too much.)"
+#endif
+
+QT_BEGIN_MOC_NAMESPACE
+struct qt_meta_stringdata_RenderArea_t {
+    QByteArrayData data[13];
+    char stringdata0[113];
+};
+#define QT_MOC_LITERAL(idx, ofs, len) \
+    Q_STATIC_BYTE_ARRAY_DATA_HEADER_INITIALIZER_WITH_OFFSET(len, \
+    qptrdiff(offsetof(qt_meta_stringdata_RenderArea_t, stringdata0) + ofs \
+        - idx * sizeof(QByteArrayData)) \
+    )
+static const qt_meta_stringdata_RenderArea_t qt_meta_stringdata_RenderArea = {
+    {
+QT_MOC_LITERAL(0, 0, 10), // "RenderArea"
+QT_MOC_LITERAL(1, 11, 8), // "setShape"
+QT_MOC_LITERAL(2, 20, 0), // ""
+QT_MOC_LITERAL(3, 21, 5), // "Shape"
+QT_MOC_LITERAL(4, 27, 5), // "shape"
+QT_MOC_LITERAL(5, 33, 6), // "setPen"
+QT_MOC_LITERAL(6, 40, 3), // "pen"
+QT_MOC_LITERAL(7, 44, 8), // "setBrush"
+QT_MOC_LITERAL(8, 53, 5), // "brush"
+QT_MOC_LITERAL(9, 59, 14), // "setAntialiased"
+QT_MOC_LITERAL(10, 74, 11), // "antialiased"
+QT_MOC_LITERAL(11, 86, 14), // "setTransformed"
+QT_MOC_LITERAL(12, 101, 11) // "transformed"
+
+    },
+    "RenderArea\0setShape\0\0Shape\0shape\0"
+    "setPen\0pen\0setBrush\0brush\0setAntialiased\0"
+    "antialiased\0setTransformed\0transformed"
+};
+#undef QT_MOC_LITERAL
+
+static const uint qt_meta_data_RenderArea[] = {
+
+ // content:
+       7,       // revision
+       0,       // classname
+       0,    0, // classinfo
+       5,   14, // methods
+       0,    0, // properties
+       0,    0, // enums/sets
+       0,    0, // constructors
+       0,       // flags
+       0,       // signalCount
+
+ // slots: name, argc, parameters, tag, flags
+       1,    1,   39,    2, 0x0a /* Public */,
+       5,    1,   42,    2, 0x0a /* Public */,
+       7,    1,   45,    2, 0x0a /* Public */,
+       9,    1,   48,    2, 0x0a /* Public */,
+      11,    1,   51,    2, 0x0a /* Public */,
+
+ // slots: parameters
+    QMetaType::Void, 0x80000000 | 3,    4,
+    QMetaType::Void, QMetaType::QPen,    6,
+    QMetaType::Void, QMetaType::QBrush,    8,
+    QMetaType::Void, QMetaType::Bool,   10,
+    QMetaType::Void, QMetaType::Bool,   12,
+
+       0        // eod
+};
+
+void RenderArea::qt_static_metacall(QObject *_o, QMetaObject::Call _c, int _id, void **_a)
+{
+    if (_c == QMetaObject::InvokeMetaMethod) {
+        RenderArea *_t = static_cast<RenderArea *>(_o);
+        Q_UNUSED(_t)
+        switch (_id) {
+        case 0: _t->setShape((*reinterpret_cast< Shape(*)>(_a[1]))); break;
+        case 1: _t->setPen((*reinterpret_cast< const QPen(*)>(_a[1]))); break;
+        case 2: _t->setBrush((*reinterpret_cast< const QBrush(*)>(_a[1]))); break;
+        case 3: _t->setAntialiased((*reinterpret_cast< bool(*)>(_a[1]))); break;
+        case 4: _t->setTransformed((*reinterpret_cast< bool(*)>(_a[1]))); break;
+        default: ;
+        }
+    }
+}
+
+const QMetaObject RenderArea::staticMetaObject = {
+    { &QWidget::staticMetaObject, qt_meta_stringdata_RenderArea.data,
+      qt_meta_data_RenderArea,  qt_static_metacall, Q_NULLPTR, Q_NULLPTR}
+};
+
+
+const QMetaObject *RenderArea::metaObject() const
+{
+    return QObject::d_ptr->metaObject ? QObject::d_ptr->dynamicMetaObject() : &staticMetaObject;
+}
+
+void *RenderArea::qt_metacast(const char *_clname)
+{
+    if (!_clname) return Q_NULLPTR;
+    if (!strcmp(_clname, qt_meta_stringdata_RenderArea.stringdata0))
+        return static_cast<void*>(const_cast< RenderArea*>(this));
+    return QWidget::qt_metacast(_clname);
+}
+
+int RenderArea::qt_metacall(QMetaObject::Call _c, int _id, void **_a)
+{
+    _id = QWidget::qt_metacall(_c, _id, _a);
+    if (_id < 0)
+        return _id;
+    if (_c == QMetaObject::InvokeMetaMethod) {
+        if (_id < 5)
+            qt_static_metacall(this, _c, _id, _a);
+        _id -= 5;
+    } else if (_c == QMetaObject::RegisterMethodArgumentMetaType) {
+        if (_id < 5)
+            *reinterpret_cast<int*>(_a[0]) = -1;
+        _id -= 5;
+    }
+    return _id;
+}
+QT_END_MOC_NAMESPACE
diff --git a/crazyflie_ws/sandbox/crazypkg/gui/untitled/moc_renderarea.o b/crazyflie_ws/sandbox/crazypkg/gui/untitled/moc_renderarea.o
new file mode 100644
index 0000000000000000000000000000000000000000..7a366130cd083c6bd08ec52f5b3a18d5c6f3ed93
Binary files /dev/null and b/crazyflie_ws/sandbox/crazypkg/gui/untitled/moc_renderarea.o differ
diff --git a/crazyflie_ws/sandbox/crazypkg/gui/untitled/myGraphicsScene.cpp b/crazyflie_ws/sandbox/crazypkg/gui/untitled/myGraphicsScene.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..cff4d244c56cbce54cd0fff9e5102e45f7e6d53f
--- /dev/null
+++ b/crazyflie_ws/sandbox/crazypkg/gui/untitled/myGraphicsScene.cpp
@@ -0,0 +1,74 @@
+#include "myGraphicsScene.h"
+
+#include <QGraphicsSceneMouseEvent>
+#include <QRect>
+
+myGraphicsScene::myGraphicsScene(QObject *parent)
+    : QGraphicsScene(parent)
+{
+    pen = new QPen(Qt::black);
+    brush = new QBrush(Qt::blue);
+    rect = 0;
+    // startedRect = false;
+    firstClick = true;
+}
+
+
+void myGraphicsScene::mousePressEvent(QGraphicsSceneMouseEvent *mouseEvent)
+{
+    if (mouseEvent->button() != Qt::LeftButton)
+        return;
+
+
+    // rect = new QRect((mouseEvent->scenePos()).toPoint(), (mouseEvent->scenePos()).toPoint());
+    // addRect(*rect, *pen, *brush);
+    // startedRect = true;
+
+    if(firstClick)
+    {
+        p1 = new QPoint((mouseEvent->scenePos()).toPoint());
+        QRect tmp_rect(*p1, *p1);
+        addRect(tmp_rect, *pen, *brush);
+    }
+    else
+    {
+        p2 = new QPoint((mouseEvent->scenePos()).toPoint());
+        QRect tmp_rect(*p2, *p2);
+        addRect(tmp_rect, *pen, *brush);
+    }
+    QGraphicsScene::mousePressEvent(mouseEvent);
+}
+
+void myGraphicsScene::mouseMoveEvent(QGraphicsSceneMouseEvent *mouseEvent)
+{
+    // if(startedRect)
+    // {
+    //     rect->moveBottomRight((mouseEvent->scenePos()).toPoint());
+    //     qDebug("Mouse Position: %d, %d", (mouseEvent->scenePos()).toPoint().x(), (mouseEvent->scenePos()).toPoint().y());
+    //     qDebug("Rectangle BottomRight Position: %d, %d", rect->bottomRight().x(), rect->bottomRight().y());
+    // }
+    QGraphicsScene::mouseMoveEvent(mouseEvent);
+}
+
+void myGraphicsScene::mouseReleaseEvent(QGraphicsSceneMouseEvent *mouseEvent)
+{
+
+    if (mouseEvent->button() != Qt::LeftButton)
+        return;
+    // rect = 0;
+    // startedRect = false;
+    if(firstClick)
+    {
+        firstClick = false;
+    }
+    else
+    {
+        rect = new QRect(*p1, *p2);
+        addRect(*rect, *pen, *brush);
+        p1 = 0;
+        p2 = 0;
+        rect = 0;
+        firstClick = true;
+    }
+    QGraphicsScene::mouseReleaseEvent(mouseEvent);
+}
diff --git a/crazyflie_ws/sandbox/crazypkg/gui/untitled/myGraphicsScene.h b/crazyflie_ws/sandbox/crazypkg/gui/untitled/myGraphicsScene.h
new file mode 100644
index 0000000000000000000000000000000000000000..96c7e3d5c4ca8723a3e9512dacf9e4c1abcbc55a
--- /dev/null
+++ b/crazyflie_ws/sandbox/crazypkg/gui/untitled/myGraphicsScene.h
@@ -0,0 +1,39 @@
+#ifndef DIAGRAMSCENE_H
+#define DIAGRAMSCENE_H
+
+#include <QGraphicsScene>
+
+class QGraphicsSceneMouseEvent;
+class QPointF;
+class QColor;
+
+
+class myGraphicsScene : public QGraphicsScene
+{
+    Q_OBJECT
+
+public:
+    explicit myGraphicsScene(QObject *parent = 0);
+
+public slots:
+
+signals:
+
+protected:
+    void mousePressEvent(QGraphicsSceneMouseEvent *mouseEvent) override;
+    void mouseMoveEvent(QGraphicsSceneMouseEvent *mouseEvent) override;
+    void mouseReleaseEvent(QGraphicsSceneMouseEvent *mouseEvent) override;
+
+private:
+    QPen* pen;
+    QBrush* brush;
+    QRect* rect;
+    QPoint* p1;
+    QPoint* p2;
+
+    bool firstClick;
+
+    // bool startedRect;
+};
+
+#endif
diff --git a/crazyflie_ws/sandbox/crazypkg/gui/untitled/myGraphicsScene.o b/crazyflie_ws/sandbox/crazypkg/gui/untitled/myGraphicsScene.o
new file mode 100644
index 0000000000000000000000000000000000000000..7fb1f539f4f9f389b25f804ae5caaa9127a11f81
Binary files /dev/null and b/crazyflie_ws/sandbox/crazypkg/gui/untitled/myGraphicsScene.o differ
diff --git a/crazyflie_ws/sandbox/crazypkg/gui/untitled/renderarea.o b/crazyflie_ws/sandbox/crazypkg/gui/untitled/renderarea.o
new file mode 100644
index 0000000000000000000000000000000000000000..262e29f403790fd75f60d55453e4119318ddaf02
Binary files /dev/null and b/crazyflie_ws/sandbox/crazypkg/gui/untitled/renderarea.o differ
diff --git a/crazyflie_ws/sandbox/crazypkg/gui/untitled/ui_mainguiwindow.h b/crazyflie_ws/sandbox/crazypkg/gui/untitled/ui_mainguiwindow.h
index 06cdc94a7018ef9b2d30958bdc5da3e85fb65af9..76c39f15638db06e346b6ad31d3ce3ca3c8adb82 100644
--- a/crazyflie_ws/sandbox/crazypkg/gui/untitled/ui_mainguiwindow.h
+++ b/crazyflie_ws/sandbox/crazypkg/gui/untitled/ui_mainguiwindow.h
@@ -14,6 +14,7 @@
 #include <QtWidgets/QApplication>
 #include <QtWidgets/QButtonGroup>
 #include <QtWidgets/QFrame>
+#include <QtWidgets/QGraphicsView>
 #include <QtWidgets/QHeaderView>
 #include <QtWidgets/QMainWindow>
 #include <QtWidgets/QMenuBar>
@@ -31,7 +32,8 @@ public:
     QWidget *centralWidget;
     QFrame *frame;
     QTabWidget *tabWidget;
-    QFrame *frame_2;
+    QFrame *frame_drawing;
+    QGraphicsView *graphicsView;
     QFrame *frame_3;
     QSpinBox *spinBoxNumCrazyflies;
     QMenuBar *menuBar;
@@ -54,14 +56,18 @@ public:
         tabWidget->setObjectName(QStringLiteral("tabWidget"));
         tabWidget->setGeometry(QRect(20, 19, 1131, 401));
         tabWidget->setLayoutDirection(Qt::LeftToRight);
-        frame_2 = new QFrame(centralWidget);
-        frame_2->setObjectName(QStringLiteral("frame_2"));
-        frame_2->setGeometry(QRect(40, 10, 571, 481));
-        frame_2->setFrameShape(QFrame::StyledPanel);
-        frame_2->setFrameShadow(QFrame::Raised);
+        frame_drawing = new QFrame(centralWidget);
+        frame_drawing->setObjectName(QStringLiteral("frame_drawing"));
+        frame_drawing->setGeometry(QRect(30, 20, 582, 469));
+        frame_drawing->setFrameShape(QFrame::StyledPanel);
+        frame_drawing->setFrameShadow(QFrame::Raised);
+        graphicsView = new QGraphicsView(frame_drawing);
+        graphicsView->setObjectName(QStringLiteral("graphicsView"));
+        graphicsView->setGeometry(QRect(30, 20, 531, 430));
+        graphicsView->setMouseTracking(true);
         frame_3 = new QFrame(centralWidget);
         frame_3->setObjectName(QStringLiteral("frame_3"));
-        frame_3->setGeometry(QRect(630, 10, 791, 481));
+        frame_3->setGeometry(QRect(629, 21, 581, 469));
         frame_3->setFrameShape(QFrame::StyledPanel);
         frame_3->setFrameShadow(QFrame::Raised);
         spinBoxNumCrazyflies = new QSpinBox(centralWidget);
diff --git a/crazyflie_ws/sandbox/crazypkg/gui/untitled/untitled b/crazyflie_ws/sandbox/crazypkg/gui/untitled/untitled
index ef8094d3f786ef56a56ec32231706d4b927b626f..d537e2520d18e3a42e5b1b0159507ee6e31d47a3 100755
Binary files a/crazyflie_ws/sandbox/crazypkg/gui/untitled/untitled and b/crazyflie_ws/sandbox/crazypkg/gui/untitled/untitled differ
diff --git a/crazyflie_ws/sandbox/crazypkg/gui/untitled/untitled.pro b/crazyflie_ws/sandbox/crazypkg/gui/untitled/untitled.pro
index dd8ec93c0e58be52129cb2c010c9ff631fed4698..3cd9724e6682dd4ec11577528bc30ad950bf8823 100644
--- a/crazyflie_ws/sandbox/crazypkg/gui/untitled/untitled.pro
+++ b/crazyflie_ws/sandbox/crazypkg/gui/untitled/untitled.pro
@@ -13,8 +13,10 @@ TEMPLATE = app
 
 
 SOURCES += main.cpp\
-        mainguiwindow.cpp
+        mainguiwindow.cpp \
+    myGraphicsScene.cpp
 
-HEADERS  += mainguiwindow.h
+HEADERS  += mainguiwindow.h \
+    myGraphicsScene.h
 
 FORMS    += mainguiwindow.ui
diff --git a/crazyflie_ws/sandbox/crazypkg/gui/untitled/untitled.pro.user b/crazyflie_ws/sandbox/crazypkg/gui/untitled/untitled.pro.user
index 74c1505cfd4a3242ff6940b5bc55ea9244a89977..da5245d20408a4f42ffb3a36452a759a76664aba 100644
--- a/crazyflie_ws/sandbox/crazypkg/gui/untitled/untitled.pro.user
+++ b/crazyflie_ws/sandbox/crazypkg/gui/untitled/untitled.pro.user
@@ -1,6 +1,6 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <!DOCTYPE QtCreatorProject>
-<!-- Written by QtCreator 4.0.2, 2017-03-22T17:44:21. -->
+<!-- Written by QtCreator 4.0.2, 2017-03-23T17:10:24. -->
 <qtcreator>
  <data>
   <variable>EnvironmentId</variable>