diff --git a/.gitignore b/.gitignore
index f5729a575b4c7e3de328564c496a4a2a67441a9d..1cf80a15db1af308fc06e72919546343a4ff24a9 100644
--- a/.gitignore
+++ b/.gitignore
@@ -31,3 +31,6 @@ pps_ws/devel/
 ./pps_ws/src/d_fall_pps/crazyradio/cflib/bootloader/cloader.pyc
 ./pps_ws/src/d_fall_pps/crazyradio/leapsdk/__init__.pyc
 
+
+
+pps_ws/src/d_fall_gui/GUI_Qt/build*
\ No newline at end of file
diff --git a/crazyflie_ws/sandbox/crazypkg/gui/untitled/untitled.pro.user b/crazyflie_ws/sandbox/crazypkg/gui/untitled/untitled.pro.user
index 94a7d132475bad46e3e354b393f155ae14a74374..a3c8bdfe0029e511baee8e0d77948bf8ff215b8a 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-04-18T16:57:38. -->
+<!-- Written by QtCreator 4.0.2, 2017-04-20T11:42:52. -->
 <qtcreator>
  <data>
   <variable>EnvironmentId</variable>
diff --git a/pps_ws/src/d_fall_gui/GUI_Qt/CrazyFlyGUI/CrazyFlyGUI.pro b/pps_ws/src/d_fall_gui/GUI_Qt/CrazyFlyGUI/CrazyFlyGUI.pro
new file mode 100644
index 0000000000000000000000000000000000000000..d06c90c4c39bf96f98690f2ba4380fa17351e370
--- /dev/null
+++ b/pps_ws/src/d_fall_gui/GUI_Qt/CrazyFlyGUI/CrazyFlyGUI.pro
@@ -0,0 +1,37 @@
+#-------------------------------------------------
+#
+# Project created by QtCreator 2017-04-20T11:20:48
+#
+#-------------------------------------------------
+
+QT       += core gui
+
+greaterThan(QT_MAJOR_VERSION, 4): QT += widgets
+
+TARGET = CrazyFlyGUI
+TEMPLATE = app
+
+
+SOURCES += \
+    ../../src/cornergrabber.cpp \
+    ../../src/crazyFlyZone.cpp \
+    ../../src/crazyFlyZoneTab.cpp \
+    ../../src/main.cpp \
+    ../../src/mainguiwindow.cpp \
+    ../../src/myGraphicsRectItem.cpp \
+    ../../src/myGraphicsScene.cpp \
+    ../../src/myGraphicsView.cpp \
+    ../../src/tablePiece.cpp
+
+HEADERS  += \
+    ../../include/cornergrabber.h \
+    ../../include/crazyFlyZone.h \
+    ../../include/crazyFlyZoneTab.h \
+    ../../include/mainguiwindow.h \
+    ../../include/myGraphicsRectItem.h \
+    ../../include/myGraphicsScene.h \
+    ../../include/myGraphicsView.h \
+    ../../include/tablePiece.h
+
+FORMS    += \
+    ../../src/mainguiwindow.ui
diff --git a/pps_ws/src/d_fall_gui/GUI_Qt/CrazyFlyGUI/CrazyFlyGUI.pro.user b/pps_ws/src/d_fall_gui/GUI_Qt/CrazyFlyGUI/CrazyFlyGUI.pro.user
new file mode 100644
index 0000000000000000000000000000000000000000..6df48ec3f7d7506446a3463d3c03d83782cf8ee7
--- /dev/null
+++ b/pps_ws/src/d_fall_gui/GUI_Qt/CrazyFlyGUI/CrazyFlyGUI.pro.user
@@ -0,0 +1,336 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE QtCreatorProject>
+<!-- Written by QtCreator 4.0.2, 2017-04-20T11:30:47. -->
+<qtcreator>
+ <data>
+  <variable>EnvironmentId</variable>
+  <value type="QByteArray">{72576140-2426-4e8d-b4f8-00ed8021ee7f}</value>
+ </data>
+ <data>
+  <variable>ProjectExplorer.Project.ActiveTarget</variable>
+  <value type="int">0</value>
+ </data>
+ <data>
+  <variable>ProjectExplorer.Project.EditorSettings</variable>
+  <valuemap type="QVariantMap">
+   <value type="bool" key="EditorConfiguration.AutoIndent">true</value>
+   <value type="bool" key="EditorConfiguration.AutoSpacesForTabs">false</value>
+   <value type="bool" key="EditorConfiguration.CamelCaseNavigation">true</value>
+   <valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.0">
+    <value type="QString" key="language">Cpp</value>
+    <valuemap type="QVariantMap" key="value">
+     <value type="QByteArray" key="CurrentPreferences">CppGlobal</value>
+    </valuemap>
+   </valuemap>
+   <valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.1">
+    <value type="QString" key="language">QmlJS</value>
+    <valuemap type="QVariantMap" key="value">
+     <value type="QByteArray" key="CurrentPreferences">QmlJSGlobal</value>
+    </valuemap>
+   </valuemap>
+   <value type="int" key="EditorConfiguration.CodeStyle.Count">2</value>
+   <value type="QByteArray" key="EditorConfiguration.Codec">UTF-8</value>
+   <value type="bool" key="EditorConfiguration.ConstrainTooltips">false</value>
+   <value type="int" key="EditorConfiguration.IndentSize">4</value>
+   <value type="bool" key="EditorConfiguration.KeyboardTooltips">false</value>
+   <value type="int" key="EditorConfiguration.MarginColumn">80</value>
+   <value type="bool" key="EditorConfiguration.MouseHiding">true</value>
+   <value type="bool" key="EditorConfiguration.MouseNavigation">true</value>
+   <value type="int" key="EditorConfiguration.PaddingMode">1</value>
+   <value type="bool" key="EditorConfiguration.ScrollWheelZooming">true</value>
+   <value type="bool" key="EditorConfiguration.ShowMargin">false</value>
+   <value type="int" key="EditorConfiguration.SmartBackspaceBehavior">0</value>
+   <value type="bool" key="EditorConfiguration.SmartSelectionChanging">true</value>
+   <value type="bool" key="EditorConfiguration.SpacesForTabs">true</value>
+   <value type="int" key="EditorConfiguration.TabKeyBehavior">0</value>
+   <value type="int" key="EditorConfiguration.TabSize">8</value>
+   <value type="bool" key="EditorConfiguration.UseGlobal">true</value>
+   <value type="int" key="EditorConfiguration.Utf8BomBehavior">1</value>
+   <value type="bool" key="EditorConfiguration.addFinalNewLine">true</value>
+   <value type="bool" key="EditorConfiguration.cleanIndentation">true</value>
+   <value type="bool" key="EditorConfiguration.cleanWhitespace">true</value>
+   <value type="bool" key="EditorConfiguration.inEntireDocument">false</value>
+  </valuemap>
+ </data>
+ <data>
+  <variable>ProjectExplorer.Project.PluginSettings</variable>
+  <valuemap type="QVariantMap"/>
+ </data>
+ <data>
+  <variable>ProjectExplorer.Project.Target.0</variable>
+  <valuemap type="QVariantMap">
+   <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Desktop Qt 5.7.0 GCC 64bit</value>
+   <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Desktop Qt 5.7.0 GCC 64bit</value>
+   <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">qt.57.gcc_64_kit</value>
+   <value type="int" key="ProjectExplorer.Target.ActiveBuildConfiguration">0</value>
+   <value type="int" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
+   <value type="int" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
+   <valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.0">
+    <value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/gazebo-cf/work/D-FaLL-System/pps_ws/src/d_fall_gui/GUI_Qt/build-CrazyFlyGUI-Desktop_Qt_5_7_0_GCC_64bit-Debug</value>
+    <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
+     <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
+      <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">qmake</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">QtProjectManager.QMakeBuildStep</value>
+      <value type="bool" key="QtProjectManager.QMakeBuildStep.LinkQmlDebuggingLibrary">true</value>
+      <value type="QString" key="QtProjectManager.QMakeBuildStep.QMakeArguments"></value>
+      <value type="bool" key="QtProjectManager.QMakeBuildStep.QMakeForced">false</value>
+      <value type="bool" key="QtProjectManager.QMakeBuildStep.SeparateDebugInfo">false</value>
+      <value type="bool" key="QtProjectManager.QMakeBuildStep.UseQtQuickCompiler">false</value>
+     </valuemap>
+     <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.1">
+      <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.MakeStep</value>
+      <valuelist type="QVariantList" key="Qt4ProjectManager.MakeStep.AutomaticallyAddedMakeArguments">
+       <value type="QString">-w</value>
+       <value type="QString">-r</value>
+      </valuelist>
+      <value type="bool" key="Qt4ProjectManager.MakeStep.Clean">false</value>
+      <value type="QString" key="Qt4ProjectManager.MakeStep.MakeArguments"></value>
+      <value type="QString" key="Qt4ProjectManager.MakeStep.MakeCommand"></value>
+     </valuemap>
+     <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">2</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Build</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
+    </valuemap>
+    <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
+     <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
+      <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.MakeStep</value>
+      <valuelist type="QVariantList" key="Qt4ProjectManager.MakeStep.AutomaticallyAddedMakeArguments">
+       <value type="QString">-w</value>
+       <value type="QString">-r</value>
+      </valuelist>
+      <value type="bool" key="Qt4ProjectManager.MakeStep.Clean">true</value>
+      <value type="QString" key="Qt4ProjectManager.MakeStep.MakeArguments">clean</value>
+      <value type="QString" key="Qt4ProjectManager.MakeStep.MakeCommand"></value>
+     </valuemap>
+     <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Clean</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
+    </valuemap>
+    <value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
+    <value type="bool" key="ProjectExplorer.BuildConfiguration.ClearSystemEnvironment">false</value>
+    <valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.UserEnvironmentChanges"/>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Debug</value>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.Qt4BuildConfiguration</value>
+    <value type="int" key="Qt4ProjectManager.Qt4BuildConfiguration.BuildConfiguration">2</value>
+    <value type="bool" key="Qt4ProjectManager.Qt4BuildConfiguration.UseShadowBuild">true</value>
+   </valuemap>
+   <valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.1">
+    <value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/gazebo-cf/work/D-FaLL-System/pps_ws/src/d_fall_gui/GUI_Qt/build-CrazyFlyGUI-Desktop_Qt_5_7_0_GCC_64bit-Release</value>
+    <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
+     <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
+      <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">qmake</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">QtProjectManager.QMakeBuildStep</value>
+      <value type="bool" key="QtProjectManager.QMakeBuildStep.LinkQmlDebuggingLibrary">false</value>
+      <value type="QString" key="QtProjectManager.QMakeBuildStep.QMakeArguments"></value>
+      <value type="bool" key="QtProjectManager.QMakeBuildStep.QMakeForced">false</value>
+      <value type="bool" key="QtProjectManager.QMakeBuildStep.SeparateDebugInfo">false</value>
+      <value type="bool" key="QtProjectManager.QMakeBuildStep.UseQtQuickCompiler">false</value>
+     </valuemap>
+     <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.1">
+      <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.MakeStep</value>
+      <valuelist type="QVariantList" key="Qt4ProjectManager.MakeStep.AutomaticallyAddedMakeArguments">
+       <value type="QString">-w</value>
+       <value type="QString">-r</value>
+      </valuelist>
+      <value type="bool" key="Qt4ProjectManager.MakeStep.Clean">false</value>
+      <value type="QString" key="Qt4ProjectManager.MakeStep.MakeArguments"></value>
+      <value type="QString" key="Qt4ProjectManager.MakeStep.MakeCommand"></value>
+     </valuemap>
+     <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">2</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Build</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
+    </valuemap>
+    <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
+     <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
+      <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.MakeStep</value>
+      <valuelist type="QVariantList" key="Qt4ProjectManager.MakeStep.AutomaticallyAddedMakeArguments">
+       <value type="QString">-w</value>
+       <value type="QString">-r</value>
+      </valuelist>
+      <value type="bool" key="Qt4ProjectManager.MakeStep.Clean">true</value>
+      <value type="QString" key="Qt4ProjectManager.MakeStep.MakeArguments">clean</value>
+      <value type="QString" key="Qt4ProjectManager.MakeStep.MakeCommand"></value>
+     </valuemap>
+     <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Clean</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
+    </valuemap>
+    <value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
+    <value type="bool" key="ProjectExplorer.BuildConfiguration.ClearSystemEnvironment">false</value>
+    <valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.UserEnvironmentChanges"/>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Release</value>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.Qt4BuildConfiguration</value>
+    <value type="int" key="Qt4ProjectManager.Qt4BuildConfiguration.BuildConfiguration">0</value>
+    <value type="bool" key="Qt4ProjectManager.Qt4BuildConfiguration.UseShadowBuild">true</value>
+   </valuemap>
+   <valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.2">
+    <value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/gazebo-cf/work/D-FaLL-System/pps_ws/src/d_fall_gui/GUI_Qt/build-CrazyFlyGUI-Desktop_Qt_5_7_0_GCC_64bit-Profile</value>
+    <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
+     <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
+      <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">qmake</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">QtProjectManager.QMakeBuildStep</value>
+      <value type="bool" key="QtProjectManager.QMakeBuildStep.LinkQmlDebuggingLibrary">true</value>
+      <value type="QString" key="QtProjectManager.QMakeBuildStep.QMakeArguments"></value>
+      <value type="bool" key="QtProjectManager.QMakeBuildStep.QMakeForced">false</value>
+      <value type="bool" key="QtProjectManager.QMakeBuildStep.SeparateDebugInfo">true</value>
+      <value type="bool" key="QtProjectManager.QMakeBuildStep.UseQtQuickCompiler">false</value>
+     </valuemap>
+     <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.1">
+      <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.MakeStep</value>
+      <valuelist type="QVariantList" key="Qt4ProjectManager.MakeStep.AutomaticallyAddedMakeArguments">
+       <value type="QString">-w</value>
+       <value type="QString">-r</value>
+      </valuelist>
+      <value type="bool" key="Qt4ProjectManager.MakeStep.Clean">false</value>
+      <value type="QString" key="Qt4ProjectManager.MakeStep.MakeArguments"></value>
+      <value type="QString" key="Qt4ProjectManager.MakeStep.MakeCommand"></value>
+     </valuemap>
+     <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">2</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Build</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
+    </valuemap>
+    <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
+     <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
+      <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+      <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.MakeStep</value>
+      <valuelist type="QVariantList" key="Qt4ProjectManager.MakeStep.AutomaticallyAddedMakeArguments">
+       <value type="QString">-w</value>
+       <value type="QString">-r</value>
+      </valuelist>
+      <value type="bool" key="Qt4ProjectManager.MakeStep.Clean">true</value>
+      <value type="QString" key="Qt4ProjectManager.MakeStep.MakeArguments">clean</value>
+      <value type="QString" key="Qt4ProjectManager.MakeStep.MakeCommand"></value>
+     </valuemap>
+     <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Clean</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
+    </valuemap>
+    <value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
+    <value type="bool" key="ProjectExplorer.BuildConfiguration.ClearSystemEnvironment">false</value>
+    <valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.UserEnvironmentChanges"/>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Profile</value>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.Qt4BuildConfiguration</value>
+    <value type="int" key="Qt4ProjectManager.Qt4BuildConfiguration.BuildConfiguration">0</value>
+    <value type="bool" key="Qt4ProjectManager.Qt4BuildConfiguration.UseShadowBuild">true</value>
+   </valuemap>
+   <value type="int" key="ProjectExplorer.Target.BuildConfigurationCount">3</value>
+   <valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
+    <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
+     <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy</value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+     <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
+    </valuemap>
+    <value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy locally</value>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.DefaultDeployConfiguration</value>
+   </valuemap>
+   <value type="int" key="ProjectExplorer.Target.DeployConfigurationCount">1</value>
+   <valuemap type="QVariantMap" key="ProjectExplorer.Target.PluginSettings"/>
+   <valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
+    <value type="bool" key="Analyzer.QmlProfiler.AggregateTraces">false</value>
+    <value type="bool" key="Analyzer.QmlProfiler.FlushEnabled">false</value>
+    <value type="uint" key="Analyzer.QmlProfiler.FlushInterval">1000</value>
+    <value type="QString" key="Analyzer.QmlProfiler.LastTraceFile"></value>
+    <value type="bool" key="Analyzer.QmlProfiler.Settings.UseGlobalSettings">true</value>
+    <valuelist type="QVariantList" key="Analyzer.Valgrind.AddedSuppressionFiles"/>
+    <value type="bool" key="Analyzer.Valgrind.Callgrind.CollectBusEvents">false</value>
+    <value type="bool" key="Analyzer.Valgrind.Callgrind.CollectSystime">false</value>
+    <value type="bool" key="Analyzer.Valgrind.Callgrind.EnableBranchSim">false</value>
+    <value type="bool" key="Analyzer.Valgrind.Callgrind.EnableCacheSim">false</value>
+    <value type="bool" key="Analyzer.Valgrind.Callgrind.EnableEventToolTips">true</value>
+    <value type="double" key="Analyzer.Valgrind.Callgrind.MinimumCostRatio">0.01</value>
+    <value type="double" key="Analyzer.Valgrind.Callgrind.VisualisationMinimumCostRatio">10</value>
+    <value type="bool" key="Analyzer.Valgrind.FilterExternalIssues">true</value>
+    <value type="int" key="Analyzer.Valgrind.LeakCheckOnFinish">1</value>
+    <value type="int" key="Analyzer.Valgrind.NumCallers">25</value>
+    <valuelist type="QVariantList" key="Analyzer.Valgrind.RemovedSuppressionFiles"/>
+    <value type="int" key="Analyzer.Valgrind.SelfModifyingCodeDetection">1</value>
+    <value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
+    <value type="bool" key="Analyzer.Valgrind.ShowReachable">false</value>
+    <value type="bool" key="Analyzer.Valgrind.TrackOrigins">true</value>
+    <value type="QString" key="Analyzer.Valgrind.ValgrindExecutable">valgrind</value>
+    <valuelist type="QVariantList" key="Analyzer.Valgrind.VisibleErrorKinds">
+     <value type="int">0</value>
+     <value type="int">1</value>
+     <value type="int">2</value>
+     <value type="int">3</value>
+     <value type="int">4</value>
+     <value type="int">5</value>
+     <value type="int">6</value>
+     <value type="int">7</value>
+     <value type="int">8</value>
+     <value type="int">9</value>
+     <value type="int">10</value>
+     <value type="int">11</value>
+     <value type="int">12</value>
+     <value type="int">13</value>
+     <value type="int">14</value>
+    </valuelist>
+    <value type="int" key="PE.EnvironmentAspect.Base">2</value>
+    <valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">CrazyFlyGUI</value>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+    <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.Qt4RunConfiguration:/home/gazebo-cf/work/D-FaLL-System/pps_ws/src/d_fall_gui/GUI_Qt/CrazyFlyGUI/CrazyFlyGUI.pro</value>
+    <value type="bool" key="QmakeProjectManager.QmakeRunConfiguration.UseLibrarySearchPath">true</value>
+    <value type="QString" key="Qt4ProjectManager.Qt4RunConfiguration.CommandLineArguments"></value>
+    <value type="QString" key="Qt4ProjectManager.Qt4RunConfiguration.ProFile">CrazyFlyGUI.pro</value>
+    <value type="bool" key="Qt4ProjectManager.Qt4RunConfiguration.UseDyldImageSuffix">false</value>
+    <value type="QString" key="Qt4ProjectManager.Qt4RunConfiguration.UserWorkingDirectory"></value>
+    <value type="QString" key="Qt4ProjectManager.Qt4RunConfiguration.UserWorkingDirectory.default">/home/gazebo-cf/work/D-FaLL-System/pps_ws/src/d_fall_gui/GUI_Qt/build-CrazyFlyGUI-Desktop_Qt_5_7_0_GCC_64bit-Debug</value>
+    <value type="uint" key="RunConfiguration.QmlDebugServerPort">3768</value>
+    <value type="bool" key="RunConfiguration.UseCppDebugger">false</value>
+    <value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
+    <value type="bool" key="RunConfiguration.UseMultiProcess">false</value>
+    <value type="bool" key="RunConfiguration.UseQmlDebugger">false</value>
+    <value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
+   </valuemap>
+   <value type="int" key="ProjectExplorer.Target.RunConfigurationCount">1</value>
+  </valuemap>
+ </data>
+ <data>
+  <variable>ProjectExplorer.Project.TargetCount</variable>
+  <value type="int">1</value>
+ </data>
+ <data>
+  <variable>ProjectExplorer.Project.Updater.FileVersion</variable>
+  <value type="int">18</value>
+ </data>
+ <data>
+  <variable>Version</variable>
+  <value type="int">18</value>
+ </data>
+</qtcreator>
diff --git a/pps_ws/src/d_fall_gui/include/cornergrabber.h b/pps_ws/src/d_fall_gui/include/cornergrabber.h
new file mode 100644
index 0000000000000000000000000000000000000000..ae096d6d61cbb3452ff2fb9a3630fe23efdc458a
--- /dev/null
+++ b/pps_ws/src/d_fall_gui/include/cornergrabber.h
@@ -0,0 +1,67 @@
+#ifndef CORNERGRABBER_H
+#define CORNERGRABBER_H
+
+#include <QObject>
+#include <QGraphicsItem>
+#include <QGraphicsRectItem>
+#include <QGraphicsTextItem>
+#include <QGraphicsSceneHoverEvent>
+#include <QGraphicsSceneMouseEvent>
+#include <QColor>
+#include <QPainter>
+#include <QPen>
+#include <QPointF>
+
+
+#define GRABBER_HEIGHT         10
+#define GRABBER_WIDTH          10
+
+class CornerGrabber : public QGraphicsItem
+{
+
+public:
+    explicit CornerGrabber(QGraphicsItem *parent = 0,  int corner = 0);
+
+    int getCorner(); ///< allows the owner to find out which coner this is
+    qreal getHeight();
+    qreal getWidth();
+    bool isActive();
+    QRectF rect() const;
+
+    enum {noCorner, bottomLeft, topLeft, topRight, bottomRight};
+
+private:
+
+    virtual QRectF boundingRect() const; ///< must be re-implemented in this class to provide the diminsions of the box to the QGraphicsView
+    virtual void paint (QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget); ///< must be re-implemented here to pain the box on the paint-event
+    virtual void hoverEnterEvent ( QGraphicsSceneHoverEvent * event ); ///< must be re-implemented to handle mouse hover enter events
+    virtual void hoverLeaveEvent ( QGraphicsSceneHoverEvent * event ); ///< must be re-implemented to handle mouse hover leave events
+
+    // once the hover event handlers are implemented in this class,
+    // the mouse events must allow be implemented because of
+    // some linkage issue - apparrently there is some connection
+    // between the hover events and mouseMove/Press/Release
+    // events which triggers a vtable issue
+    virtual void mouseMoveEvent ( QGraphicsSceneMouseEvent * event );
+    virtual void mouseMoveEvent(QGraphicsSceneDragDropEvent *event);
+    virtual void mousePressEvent (QGraphicsSceneMouseEvent * event );
+    virtual void mousePressEvent(QGraphicsSceneDragDropEvent *event);
+    virtual void mouseReleaseEvent (QGraphicsSceneMouseEvent * event );
+
+    QRectF createRect();
+    void setRect(const QRectF & rectangle);
+
+    QColor _outterborderColor; ///< the hover event handlers will toggle this between red and black
+    QPen _outterborderPen; ///< the pen is used to paint the red/black border
+
+    qreal   _width;
+    qreal   _height;
+
+    int _corner;// 0,1,2,3  - starting at x=0,y=0 and moving clockwise around the box
+
+    bool _is_active;
+
+    QRectF _rect;
+};
+
+#endif // CORNERGRABBER_H
diff --git a/pps_ws/src/d_fall_gui/include/crazyFlyZone.h b/pps_ws/src/d_fall_gui/include/crazyFlyZone.h
new file mode 100644
index 0000000000000000000000000000000000000000..f1f230d4677dff6705e3f902e47bbf05233c50d5
--- /dev/null
+++ b/pps_ws/src/d_fall_gui/include/crazyFlyZone.h
@@ -0,0 +1,28 @@
+#ifndef CRAZYFLYZONE_H
+#define CRAZYFLYZONE_H
+
+
+#include <QGraphicsSimpleTextItem>
+
+#include "myGraphicsRectItem.h"
+
+class crazyFlyZone : public myGraphicsRectItem
+{
+public:
+    explicit crazyFlyZone(const QRectF & rect, int index, QGraphicsItem * parent = 0);
+
+    int getIndex();
+    void setIndex(int index);
+    void setLabel(QString string);
+    void setLabelPosition();
+    void updateLabel(QString string);
+    void rectSizeChanged();
+protected:
+
+private:
+    int _index;
+    QGraphicsSimpleTextItem* label;
+};
+
+
+#endif
diff --git a/pps_ws/src/d_fall_gui/include/crazyFlyZoneTab.h b/pps_ws/src/d_fall_gui/include/crazyFlyZoneTab.h
new file mode 100644
index 0000000000000000000000000000000000000000..164ba764777bea716d610089af06f5ab08f5245d
--- /dev/null
+++ b/pps_ws/src/d_fall_gui/include/crazyFlyZoneTab.h
@@ -0,0 +1,26 @@
+#ifndef CRAZYFLYZONETAB_H
+#define CRAZYFLYZONETAB_H
+
+#include <QObject>
+#include <QWidget>
+#include <QPushButton>
+
+class crazyFlyZoneTab : public QWidget
+{
+    Q_OBJECT
+public:
+    explicit crazyFlyZoneTab(int index, QWidget *parent = 0);
+    QPushButton* center_button;
+private:
+    int _index;
+    int _num_rows;
+    int _num_columns;
+signals:
+    void centerButtonClickedSignal(int index);
+
+public slots:
+    void centerButtonClicked();
+};
+
+
+#endif
diff --git a/pps_ws/src/d_fall_gui/include/mainguiwindow.h b/pps_ws/src/d_fall_gui/include/mainguiwindow.h
new file mode 100644
index 0000000000000000000000000000000000000000..f0fac1a6c1edfd71cbcb9b62803d963785633f61
--- /dev/null
+++ b/pps_ws/src/d_fall_gui/include/mainguiwindow.h
@@ -0,0 +1,249 @@
+#ifndef MAINGUIWINDOW_H
+#define MAINGUIWINDOW_H
+
+#define DEBUG_GUI
+
+#include <QMainWindow>
+#include <QTimer>
+#include <QGridLayout>
+#include <QGraphicsRectItem>
+
+#ifndef DEBUG_GUI
+#include "ros/callback_queue.h"
+#include "ros/ros.h"
+#include "CrazyFlieTypes.h"
+#endif
+#include "ui_mainguiwindow.h"
+#include "myGraphicsScene.h"
+
+namespace Ui {
+class MainGUIWindow;
+}
+
+
+#ifndef DEBUG_GUI
+struct setpoint
+{
+    double x;
+    double y;
+    double z;
+    double yaw;
+
+};
+
+class CSetpointQueue
+{
+public:
+    CSetpointQueue();
+    void insert(setpoint newElem);
+    setpoint getNext();
+    void print();
+
+private:
+    struct QueueElem
+    {
+        QueueElem(setpoint newElem) {elem.x=newElem.x; elem.y=newElem.y; elem.z=newElem.z; elem.yaw=newElem.yaw; next=NULL;}
+        setpoint elem;
+        QueueElem* next;
+    };
+
+    QueueElem* startElem;
+    QueueElem* currElem;
+    QueueElem* lastElem;
+};
+#endif
+
+class MainGUIWindow : public QMainWindow
+{
+    Q_OBJECT
+
+public:
+    #ifdef DEBUG_GUI
+    explicit MainGUIWindow(/*ros::NodeHandle* ,*/ /*ros::CallbackQueue* callbackQueue, ros::Publisher* publisherMotorCommandsGUI,*/ QWidget *parent = 0);
+    #else
+    explicit MainGUIWindow(ros::NodeHandle* , /*ros::CallbackQueue* callbackQueue, ros::Publisher* publisherMotorCommandsGUI,*/ QWidget *parent = 0);
+    #endif
+    ~MainGUIWindow();
+    #ifndef DEBUG_GUI
+    void init();
+    #endif
+
+public slots:
+    #ifndef DEBUG_GUI
+    void runCallbacks();
+    #endif
+private slots:
+    #ifndef DEBUG_GUI
+   // void PIDParamTableChanged(double param);
+
+   // void RateParamTableChanged(double param);
+
+   // void positionSetpointChanged(double param);
+
+   // void sampleTimeChanged(double param);
+
+   // void feedforwardCmdChanged(double cmd);
+
+   // void controllerTypeChanged(bool checked);
+
+   // void trajectoryTypeChanged(bool checked);
+
+   // void on_buttonStop_clicked();
+
+   // void on_buttonPrint_clicked();
+
+   // void on_buttonSetpointChange_clicked();
+
+   // void on_buttonPIDDefaultParams_clicked();
+
+   // void on_buttonSetpointCurrPos_clicked();
+
+   // void on_buttonDefaultFeedforward_clicked();
+
+   // void on_buttonResetMissed_clicked();
+
+   // void on_SetpointHome_clicked();
+
+   // void on_setpointZ200_clicked();
+
+   // void on_slideMotorCmdTest_valueChanged(int value);
+
+   // void on_buttonResetControllers_clicked();
+
+   // void on_buttonSetDefaultTs_clicked();
+
+   // void on_buttonSetDefaultRateParams_clicked();
+
+   // void on_slideRollAngleTest_valueChanged(int value);
+
+   // void on_slidePitchAngleTest_valueChanged(int value);
+
+   // void on_slideYawAngleTest_valueChanged(int value);
+
+   // void on_slideRollRateTest_valueChanged(int value);
+
+   // void on_slidePitchRateTest_valueChanged(int value);
+
+   // void on_slideYawRateTest_valueChanged(int value);
+
+   // void on_buttonStop_2_clicked();
+   #endif
+
+    void set_tabs(int n);
+    void transitionToMode(int mode);
+    void on_removeTable_clicked();
+
+    void on_radioButton_table_mode_toggled(bool checked);
+
+    void on_radioButton_crazyfly_zones_mode_toggled(bool checked);
+    void handleTablePiecesNumChanged(int newNum);
+
+    void on_radioButton_lock_mode_toggled(bool checked);
+
+    void on_checkBox_grid_toggled(bool checked);
+
+    void on_checkBox_table_toggled(bool checked);
+
+    void on_checkBox_crazyfly_zones_toggled(bool checked);
+
+    void on_tabWidget_currentChanged(int index);
+
+    void centerViewIndex(int index);
+
+    void on_pushButton_fitAll_clicked();
+
+private:
+
+    Ui::MainGUIWindow *ui;
+    myGraphicsScene* scene;
+    QGraphicsRectItem* item1;
+
+    void _init();
+
+    #ifndef DEBUG_GUI
+    // void refreshScreen();
+
+    // callbacks
+    // void callbackControllerOutput(const crazypkg::ControllerOutputPackage& msg);
+    // void callbackViconData(const crazypkg::ViconData& msg);
+    // void callbackCntViconDataMissed(const std_msgs::Int32& msg);
+
+    void readDefaultParameters();
+    // void setDefaultPIDParameters();
+    // void setDefaultRateParameters();
+    // void setDefaultSampleTime();
+    // void setDefaultFeedforwardCmd();
+
+    // void initPIDParamsTable();
+    // void initRateParamsTable();
+    // void initPositionSetpoint();
+    // void initSampleTime();
+    // void initControllerType();
+    // void initFeedforwardCmd();
+    // void initSetpointQueues();
+    // void initSetpointType();
+
+    // void publishSetpoint();
+    // void updateSetpoint();
+
+    // void publishSampleTime(EControllerType controller);
+
+    ros::CallbackQueue m_CallbackQueue;
+    ros::NodeHandle* m_pNodeHandle;
+
+    // publishers
+    // ros::Publisher* m_pPublisherMotorCommandsGUI;
+    ros::Publisher* m_pPublisherControllerParam;
+    ros::Publisher* m_pPublisherPositionSetpoint;
+    ros::Publisher* m_pPublisherSampleTime;
+    ros::Publisher* m_pPublisherControllerType;
+    ros::Publisher* m_pPublisherDoSomething;
+    ros::Publisher* m_pPublisherFeedforwardCmd;
+
+    crazypkg::ControllerParam m_controllerParam;
+    crazypkg::PositionSetpoint m_positionSetpoint;
+    crazypkg::MotorCommands m_DummyCommands;
+    crazypkg::SampleTimeParam m_sampleTimeParam;
+    std_msgs::Int32 m_controllerType;
+    std_msgs::Int32 m_DoSomething;
+    crazypkg::MotorCommands m_feedforwardCmd;
+
+    //subscribers
+    ros::Subscriber* m_pSubscriberControllerOutput;
+    ros::Subscriber* m_pSubscriberViconData;
+    ros::Subscriber* m_pSubscriberCntViconDataMissed;
+
+    // crazypkg::MotorCommands m_MotorCommands;
+    // crazypkg::ViconData m_ViconData;
+
+    // params
+    // PIDParams m_PIDParams [countPIDControllers];
+
+    // default params
+    PIDParams m_DefaultPIDParams[countPIDControllers];
+    PIDParams m_DefaultRateParams[countRateControllers];
+    double m_DefaultSampleTime[countSampleTimeTypes];
+    crazypkg::MotorCommands m_DefaultFeedforwardCmd;
+
+    CSetpointQueue m_trajCircle;
+    CSetpointQueue m_trajSquare;
+    setpoint m_currSetpoint;
+
+    bool m_isStopButtonActive;
+    bool m_isCalActive;
+
+    enum ETrajectoryType
+    {
+        eTrajCustom,
+        eTrajCircle,
+        eTrajSquare
+    };
+
+    ETrajectoryType m_trajectoryType;
+
+    #endif
+};
+
+
+
+#endif // MAINGUIWINDOW_H
diff --git a/pps_ws/src/d_fall_gui/include/myGraphicsRectItem.h b/pps_ws/src/d_fall_gui/include/myGraphicsRectItem.h
new file mode 100644
index 0000000000000000000000000000000000000000..1e991648c4d8b7b172ed47d30f655811b270c7c6
--- /dev/null
+++ b/pps_ws/src/d_fall_gui/include/myGraphicsRectItem.h
@@ -0,0 +1,58 @@
+#ifndef MYGRAPHICSRECTITEM_H
+#define MYGRAPHICSRECTITEM_H
+
+#include <QGraphicsRectItem>
+#include "cornergrabber.h"
+
+class QGraphicsSceneMouseEvent;
+class QPointF;
+class QColor;
+
+
+class myGraphicsRectItem : public QGraphicsRectItem
+{
+public:
+    explicit myGraphicsRectItem(const QRectF & rect, QGraphicsItem * parent = 0);
+    void lock();
+    void unlock();
+
+public slots:
+
+signals:
+
+protected:
+    void mousePressEvent(QGraphicsSceneMouseEvent *mouseEvent) override;
+    void mouseMoveEvent(QGraphicsSceneMouseEvent *mouseEvent) override;
+    void mouseReleaseEvent(QGraphicsSceneMouseEvent *mouseEvent) override;
+
+    virtual void rectSizeChanged() = 0; // pure virtual function, has to be overridden in derived class
+
+    QVariant itemChange(GraphicsItemChange change, const QVariant &value) override;
+
+private:
+    void setCornerPositions();
+    bool anyGrabber();
+    int checkCornerGrabbers();
+    void createGrabbers();
+    bool grabbersAreCreated();
+    void deleteGrabbers();
+
+    QPen* pen;
+    QBrush* brush;
+    QRectF* tmp_rect;
+    QGraphicsRectItem* tmp_rect_item;
+    QPointF* p1;
+    QPointF* p2;
+
+    CornerGrabber* _bottomLeft_corner;
+    CornerGrabber* _topLeft_corner;
+    CornerGrabber* _topRight_corner;
+    CornerGrabber* _bottomRight_corner;
+
+    bool _grabbers_created;
+    bool resize_mode;
+    bool locked;
+
+};
+
+#endif
diff --git a/pps_ws/src/d_fall_gui/include/myGraphicsScene.h b/pps_ws/src/d_fall_gui/include/myGraphicsScene.h
new file mode 100644
index 0000000000000000000000000000000000000000..6576f83dcbf6421e1a15795b1e9b9e17969f86c7
--- /dev/null
+++ b/pps_ws/src/d_fall_gui/include/myGraphicsScene.h
@@ -0,0 +1,86 @@
+#ifndef MYGRAPHICSSCENE_H
+#define MYGRAPHICSSCENE_H
+
+#include <vector>
+
+#include <QGraphicsScene>
+
+#include "crazyFlyZone.h"
+#include "tablePiece.h"
+
+class QGraphicsSceneMouseEvent;
+class QPointF;
+class QColor;
+
+
+class myGraphicsScene : public QGraphicsScene
+{
+    Q_OBJECT
+
+public:
+
+    explicit myGraphicsScene(QObject *parent = 0);
+    std::vector<crazyFlyZone*> crazyfly_zones;
+    std::vector<tablePiece*> table_pieces;
+    int getMode();
+
+    void setMode(int new_mode);
+    void setGrid(bool enable);
+
+    void hideTable();
+    void showTable();
+
+    void hideCrazyFlyZones();
+    void showCrazyFlyZones();
+
+    QRectF getRectFCrazyFlyZone(int index);
+
+    enum {mode_table, mode_crazyfly_zones, mode_locked};
+
+    void removeTable();
+
+public slots:
+    void removeCrazyFlyZone(int index);
+    void setSelectedCrazyFlyZone(int index);
+    void changeModeTo(int next_mode);
+
+signals:
+    void numCrazyFlyZonesChanged(int newNum);
+    void crazyFlyZoneSelected(int index);
+    void modeChanged(int mode);
+    void numTablePiecesChanged(int newNum);
+
+protected:
+    void mousePressEvent(QGraphicsSceneMouseEvent *mouseEvent) override;
+    void mouseMoveEvent(QGraphicsSceneMouseEvent *mouseEvent) override;
+    void mouseReleaseEvent(QGraphicsSceneMouseEvent *mouseEvent) override;
+
+    void keyPressEvent(QKeyEvent * keyEvent) override;
+    void drawBackground(QPainter *painter, const QRectF &rect);
+
+private:
+    void lockTablePieces(void);
+    void unlockTablePieces(void);
+    void lockCrazyFlyZones(void);
+    void unlockCrazyFlyZones(void);
+
+    void addCrazyFlyZoneToVector(crazyFlyZone* rect);
+    void addTablePieceToVector(tablePiece* rect);
+    int checkSelectedCrazyFlyZone();
+    void updateIndexesAndLabelsCrazyFlyZones();
+    void removeTablePiece(int index);
+
+    QPen* pen;
+    QBrush* brush;
+    QRectF* tmp_rect;
+    crazyFlyZone* tmp_crazyfly_zone_item;
+    tablePiece* tmp_table_piece_item;
+    QPointF* p1;
+    QPointF* p2;
+
+    bool startedRect;
+    int mode;
+    bool grid_enable;
+};
+
+#endif
diff --git a/pps_ws/src/d_fall_gui/include/myGraphicsView.h b/pps_ws/src/d_fall_gui/include/myGraphicsView.h
new file mode 100644
index 0000000000000000000000000000000000000000..a9983705e9a62f4b6f4d5ced0393867753e7fb85
--- /dev/null
+++ b/pps_ws/src/d_fall_gui/include/myGraphicsView.h
@@ -0,0 +1,36 @@
+#ifndef MYGRAPHICSVIEW_H
+#define MYGRAPHICSVIEW_H
+
+#include <vector>
+
+#include <QGraphicsView>
+#include <QWheelEvent>
+
+class myGraphicsView : public QGraphicsView
+{
+    Q_OBJECT
+
+public:
+
+    explicit myGraphicsView(QWidget *parent = 0);
+
+public slots:
+
+signals:
+
+protected:
+    void mousePressEvent(QMouseEvent *mouseEvent) override;
+    void mouseMoveEvent(QMouseEvent *mouseEvent) override;
+    void mouseReleaseEvent(QMouseEvent *mouseEvent) override;
+
+    virtual void wheelEvent(QWheelEvent* event) override; // TODO: right now, do it in the whole MainGUIWindow. Afterwards maybe do this only in the QGraphicsScene (need to do own class)
+
+private:
+
+    bool translation_mode;
+    qreal translate_dx;
+    qreal translate_dy;
+    QPointF* tmp_point;
+};
+
+#endif
diff --git a/pps_ws/src/d_fall_gui/include/tablePiece.h b/pps_ws/src/d_fall_gui/include/tablePiece.h
new file mode 100644
index 0000000000000000000000000000000000000000..17d2f1c5899628ea88eb935cde8f2811813596d2
--- /dev/null
+++ b/pps_ws/src/d_fall_gui/include/tablePiece.h
@@ -0,0 +1,22 @@
+#ifndef TABLEPIECE_H
+#define TABLEPIECE_H
+
+
+#include <QGraphicsSimpleTextItem>
+
+#include "myGraphicsRectItem.h"
+
+class tablePiece : public myGraphicsRectItem
+{
+public:
+    explicit tablePiece(const QRectF & rect, QGraphicsItem * parent = 0);
+    void rectSizeChanged();     // pure virtual, need to implement it
+    void setLightColor();
+    void setDarkColor();
+protected:
+
+private:
+};
+
+
+#endif
diff --git a/pps_ws/src/d_fall_gui/src/cornergrabber.cpp b/pps_ws/src/d_fall_gui/src/cornergrabber.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d63041ac52e2a6c07f56ff417e11d8bf4b798d2b
--- /dev/null
+++ b/pps_ws/src/d_fall_gui/src/cornergrabber.cpp
@@ -0,0 +1,156 @@
+#include "../../include/cornergrabber.h"
+
+CornerGrabber::CornerGrabber(QGraphicsItem *parent,  int corner) :
+    QGraphicsItem(parent),
+    _outterborderColor(Qt::black),
+    _outterborderPen(),
+    _width(GRABBER_WIDTH),
+    _height(GRABBER_HEIGHT),
+    _corner(corner),
+    _is_active(false)
+{
+    setParentItem(parent);
+    _outterborderPen.setWidth(2);
+    _outterborderPen.setColor(_outterborderColor);
+    this->setAcceptHoverEvents(true);
+    this->setFlag(QGraphicsItem::ItemIgnoresTransformations);
+}
+
+
+qreal CornerGrabber::getHeight()
+{
+    return _height;
+}
+
+qreal CornerGrabber::getWidth()
+{
+    return _width;
+}
+
+int CornerGrabber::getCorner()
+{
+    return _corner;
+}
+
+bool CornerGrabber::isActive()
+{
+    return _is_active;
+}
+
+
+// we have to implement the mouse events to keep the linker happy,
+// but just set accepted to false since are not actually handling them
+
+void CornerGrabber::mouseMoveEvent(QGraphicsSceneDragDropEvent *event)
+{
+    event->setAccepted(false);
+}
+
+void CornerGrabber::mousePressEvent(QGraphicsSceneDragDropEvent *event)
+{
+    event->setAccepted(false);
+}
+
+void CornerGrabber::mouseReleaseEvent ( QGraphicsSceneMouseEvent * event )
+{
+    event->setAccepted(false);
+}
+
+void CornerGrabber::mousePressEvent ( QGraphicsSceneMouseEvent * event )
+{
+    event->setAccepted(false);
+}
+
+void CornerGrabber::mouseMoveEvent ( QGraphicsSceneMouseEvent * event )
+{
+    event->setAccepted(false);
+}
+
+
+// change the color on hover events to indicate to the use the object has
+// been captured by the mouse
+
+void CornerGrabber::hoverLeaveEvent ( QGraphicsSceneHoverEvent * )
+{
+    _outterborderColor = Qt::black;
+    _is_active = false;
+    this->update(0,0,_width,_height);
+}
+
+void CornerGrabber::hoverEnterEvent ( QGraphicsSceneHoverEvent * )
+{
+    _outterborderColor = Qt::red;
+    _is_active = true;
+    this->update(0,0,_width,_height);
+}
+
+QRectF CornerGrabber::boundingRect() const
+{
+    QRectF bounding_rect = this->rect();
+    return bounding_rect;
+}
+
+QRectF CornerGrabber::rect() const
+{
+    return _rect;
+}
+
+void CornerGrabber::setRect(const QRectF & rectangle)
+{
+    _rect = rectangle;
+}
+
+void CornerGrabber::paint (QPainter *painter, const QStyleOptionGraphicsItem *, QWidget *)
+{
+
+    // fill the box with solid color, use sharp corners
+    prepareGeometryChange();
+    _outterborderPen.setCapStyle(Qt::SquareCap);
+    _outterborderPen.setStyle(Qt::SolidLine);
+    painter->setPen(_outterborderPen);
+    QRectF rect = createRect();
+    this->setRect(rect);
+    QBrush brush (Qt::SolidPattern);
+    brush.setColor (_outterborderColor);
+    painter->fillRect(_rect,brush);
+
+}
+
+QRectF CornerGrabber::createRect()
+{
+    QPointF topLeft(0,0);
+    QPointF bottomRight(_width, _height);
+    QRectF rect(topLeft, bottomRight);
+
+    switch(_corner)
+    {
+        case CornerGrabber::bottomLeft:
+        {
+            QPointF move_to(0,0);
+            rect.moveBottomLeft(move_to);
+            return rect;
+            break;
+        }
+        case CornerGrabber::topLeft:
+        {
+            return rect;
+            break;
+        }
+        case CornerGrabber::topRight:
+        {
+            QPointF move_to(0,0);
+            rect.moveTopRight(move_to);
+            return rect;
+            break;
+        }
+        case CornerGrabber::bottomRight:
+        {
+            QPointF move_to(0,0);
+            rect.moveBottomRight(move_to);
+            return rect;
+            break;
+        }
+        default:
+            break;
+    }
+}
diff --git a/pps_ws/src/d_fall_gui/src/crazyFlyZone.cpp b/pps_ws/src/d_fall_gui/src/crazyFlyZone.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..657e39ce6edf32f31d7703ebc92de3ef3d408de0
--- /dev/null
+++ b/pps_ws/src/d_fall_gui/src/crazyFlyZone.cpp
@@ -0,0 +1,46 @@
+#include "../../include/crazyFlyZone.h"
+
+crazyFlyZone::crazyFlyZone(const QRectF & rect, int index,  QGraphicsItem * parent)
+    : myGraphicsRectItem(rect, parent)
+{
+    this->setPen(QPen(Qt::black, 0));
+    setIndex(index);
+}
+
+void crazyFlyZone::updateLabel(QString string)
+{
+    label->setText(string);
+    setLabelPosition();
+}
+
+void crazyFlyZone::setLabel(QString string)
+{
+    label = new QGraphicsSimpleTextItem(string, this);
+    label->setFlag(QGraphicsItem::ItemIgnoresTransformations);
+    label->setFont(QFont("Arial", 18, QFont::Bold, true));
+    setLabelPosition();
+}
+
+void crazyFlyZone::setLabelPosition()
+{
+    qreal x_offset = 10;
+    qreal y_offset = 5;
+    label->setPos(this->rect().topLeft().x() + x_offset,this->rect().topLeft().y() + y_offset);
+}
+
+int crazyFlyZone::getIndex()
+{
+    return _index;
+}
+
+void crazyFlyZone::setIndex(int index)
+{
+    // TODO: how to make sure that we never have two rectangles with the same index?
+    // Maybe only when we reduce the size of the rectangles vector?
+    _index = index;
+}
+
+void crazyFlyZone::rectSizeChanged() // pure virtual coming from parent
+{
+    setLabelPosition();
+}
diff --git a/pps_ws/src/d_fall_gui/src/crazyFlyZoneTab.cpp b/pps_ws/src/d_fall_gui/src/crazyFlyZoneTab.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d306a16ce76bf9b2cc0de10f09cfe0bf1f0f7e0f
--- /dev/null
+++ b/pps_ws/src/d_fall_gui/src/crazyFlyZoneTab.cpp
@@ -0,0 +1,37 @@
+#include "../../include/crazyFlyZoneTab.h"
+
+#include <QLayout>
+
+crazyFlyZoneTab::crazyFlyZoneTab(int index, QWidget *parent)
+    : QWidget(parent)
+{
+    _index = index;
+    _num_rows = 3;
+    _num_columns = 3;
+    center_button = new QPushButton("Fit view");
+    QGridLayout *mainLayout = new QGridLayout;
+    // mainLayout->setRowMinimumHeight(1, 25);
+    // mainLayout->setRowMinimumHeight(2, 25);
+    // mainLayout->setRowStretch(1);
+    // mainLayout->setColumnMinimumWidth(5);
+    for(int i = 0; i < _num_rows; i++)
+    {
+        mainLayout->setRowStretch(i, 1);
+    }
+
+    for(int i = 0; i < _num_columns; i++)
+    {
+        mainLayout->setColumnStretch(i, 1);
+    }
+
+    mainLayout->addWidget(center_button, _num_rows - 1, _num_columns - 1);
+    setLayout(mainLayout);
+    QObject::connect(center_button, SIGNAL(clicked()), this, SLOT(centerButtonClicked()));
+    qDebug("tab widget created, index: %d", _index);
+}
+
+void crazyFlyZoneTab::centerButtonClicked()
+{
+    qDebug("index clicked: %d", _index);
+    emit centerButtonClickedSignal(_index);
+}
diff --git a/pps_ws/src/d_fall_gui/src/main.cpp b/pps_ws/src/d_fall_gui/src/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a54d39a7f5f37fc317bdb7dd8e1efd06c43375a8
--- /dev/null
+++ b/pps_ws/src/d_fall_gui/src/main.cpp
@@ -0,0 +1,36 @@
+#include "../../include/mainguiwindow.h"
+#include "ui_mainguiwindow.h"
+#include <QApplication>
+
+#ifndef DEBUG_GUI
+#include "CrazyFlieInclude.h"
+#endif
+
+int main(int argc, char *argv[])
+{
+    #ifndef DEBUG_GUI
+    ros::init(argc, argv, "GUI");
+    ros::NodeHandle nodeHandle("~");
+    #endif
+    QApplication applicationGUI(argc, argv);
+
+    #ifndef DEBUG_GUI
+    MainGUIWindow mainWindow(&nodeHandle);
+    mainWindow.init();
+    #else
+    MainGUIWindow mainWindow;
+    #endif
+
+    #ifndef DEBUG_GUI
+    QTimer *timerExecuteCallbacks = new QTimer(&applicationGUI);
+    mainWindow.connect(timerExecuteCallbacks, SIGNAL(timeout()), &mainWindow, SLOT(runCallbacks()));
+    timerExecuteCallbacks->start(100);
+    #endif
+
+    mainWindow.show();
+    applicationGUI.exec();
+
+    #ifndef DEBUG_GUI
+    ROS_WARN("GUI application terminated");
+    #endif
+}
diff --git a/pps_ws/src/d_fall_gui/src/mainguiwindow.cpp b/pps_ws/src/d_fall_gui/src/mainguiwindow.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a1d50f0cf2fc5c38f0ad073ac09cc4c183fec380
--- /dev/null
+++ b/pps_ws/src/d_fall_gui/src/mainguiwindow.cpp
@@ -0,0 +1,1110 @@
+#include "../../include/mainguiwindow.h"
+#include "ui_mainguiwindow.h"
+#include "../../include/crazyFlyZoneTab.h"
+
+#include <QObject>
+#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::set_tabs(int n)
+{
+    ui->tabWidget->clear();
+    std::string str;
+    for (int i = 0; i < n; i++)
+    {
+        str = "CrazyFly ";
+        str += std::to_string(i+1);
+        QString qstr(str.c_str());
+        crazyFlyZoneTab* widget = new crazyFlyZoneTab(i);
+        ui->tabWidget->addTab(widget, qstr);
+        connect(widget, SIGNAL(centerButtonClickedSignal(int)), this, SLOT(centerViewIndex(int)));
+    }
+}
+
+void MainGUIWindow::_init()
+{
+
+    scene = new myGraphicsScene(ui->frame_drawing);
+    scene->setSceneRect(QRectF(QPointF(-1400, 1400), QSizeF(1400, 1400)));
+
+    ui->graphicsView->setViewportUpdateMode(QGraphicsView::FullViewportUpdate);
+    ui->graphicsView->setScene(scene);
+
+    // ui->radioButton_crazyfly_zones_mode->setCheckable(false);
+    // ui->radioButton_crazyfly_zones_mode->setEnabled(false);
+
+
+    QObject::connect(ui->tabWidget, SIGNAL(tabCloseRequested(int)), scene, SLOT(removeCrazyFlyZone(int)));
+    QObject::connect(scene, SIGNAL(numCrazyFlyZonesChanged(int)), this, SLOT(set_tabs(int)));
+    QObject::connect(ui->tabWidget, SIGNAL(currentChanged(int)), scene, SLOT(setSelectedCrazyFlyZone(int)));
+    QObject::connect(scene, SIGNAL(crazyFlyZoneSelected(int)), ui->tabWidget, SLOT(setCurrentIndex(int)));
+    QObject::connect(scene, SIGNAL(modeChanged(int)), this, SLOT(transitionToMode(int)));
+    QObject::connect(scene, SIGNAL(numTablePiecesChanged(int)), this, SLOT(handleTablePiecesNumChanged(int)));
+}
+
+#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_removeTable_clicked()
+{
+    if(scene->getMode() == myGraphicsScene::mode_table)
+    {
+        scene->removeTable();
+    }
+}
+
+void MainGUIWindow::transitionToMode(int mode)
+{
+    switch(mode)
+    {
+        case myGraphicsScene::mode_table:
+        {
+            ui->removeTable->setDisabled(false);
+            break;
+        }
+        case myGraphicsScene::mode_crazyfly_zones:
+        {
+            ui->removeTable->setDisabled(true);
+            break;
+        }
+    }
+}
+
+void MainGUIWindow::on_radioButton_table_mode_toggled(bool checked)
+{
+    switch(scene->getMode())
+    {
+        case myGraphicsScene::mode_table:
+        {
+            // already in the mode we want, do nothing
+            break;
+        }
+        case myGraphicsScene::mode_crazyfly_zones:
+        {
+            scene->setMode(myGraphicsScene::mode_table);
+            break;
+        }
+        case myGraphicsScene::mode_locked:
+        {
+            scene->setMode(myGraphicsScene::mode_table);
+            break;
+        }
+    }
+
+}
+
+
+void MainGUIWindow::on_radioButton_crazyfly_zones_mode_toggled(bool checked)
+{
+    switch(scene->getMode())
+    {
+        case myGraphicsScene::mode_table:
+        {
+            scene->setMode(myGraphicsScene::mode_crazyfly_zones);
+            break;
+        }
+        case myGraphicsScene::mode_crazyfly_zones:
+        {
+            // already in the mode we want, do nothing
+            break;
+        }
+        case myGraphicsScene::mode_locked:
+        {
+            scene->setMode(myGraphicsScene::mode_crazyfly_zones);
+            break;
+        }
+    }
+}
+
+void MainGUIWindow::handleTablePiecesNumChanged(int newNum)
+{
+    // if(newNum == 0)
+    // {
+    //     ui->radioButton_crazyfly_zones_mode->setCheckable(false);
+    //     ui->radioButton_crazyfly_zones_mode->setEnabled(false);
+    // }
+    // else
+    // {
+    //     ui->radioButton_crazyfly_zones_mode->setCheckable(true);
+    //     ui->radioButton_crazyfly_zones_mode->setEnabled(true);
+    // }
+}
+
+void MainGUIWindow::on_radioButton_lock_mode_toggled(bool checked)
+{
+    switch(scene->getMode())
+    {
+        case myGraphicsScene::mode_table:
+        {
+            scene->setMode(myGraphicsScene::mode_locked);
+            break;
+        }
+        case myGraphicsScene::mode_crazyfly_zones:
+        {
+            scene->setMode(myGraphicsScene::mode_locked);
+            break;
+        }
+        case myGraphicsScene::mode_locked:
+        {
+            break;
+        }
+    }
+}
+
+void MainGUIWindow::on_checkBox_grid_toggled(bool checked)
+{
+    scene->setGrid(checked);
+}
+
+void MainGUIWindow::on_checkBox_table_toggled(bool checked)
+{
+    if(checked)
+    {
+        scene->showTable();
+    }
+    else
+    {
+        scene->hideTable();
+    }
+}
+
+void MainGUIWindow::on_checkBox_crazyfly_zones_toggled(bool checked)
+{
+    if(checked)
+    {
+        scene->showCrazyFlyZones();
+    }
+    else
+    {
+        scene->hideCrazyFlyZones();
+    }
+}
+
+void MainGUIWindow::on_tabWidget_currentChanged(int index)
+{
+    if(index >= 0)
+    {
+        scene->setSelectedCrazyFlyZone(index);
+    }
+}
+
+void MainGUIWindow::centerViewIndex(int index)
+{
+    ui->graphicsView->fitInView(scene->getRectFCrazyFlyZone(index), Qt::KeepAspectRatio);
+    ui->graphicsView->scale(0.95, 0.95); // A bit back zoom, so we can see everything better
+}
+
+
+void MainGUIWindow::on_pushButton_fitAll_clicked()
+{
+    ui->graphicsView->fitInView(scene->itemsBoundingRect(), Qt::KeepAspectRatio);
+    ui->graphicsView->scale(0.95, 0.95); // A bit back zoom, so we can see everything better
+}
diff --git a/pps_ws/src/d_fall_gui/src/mainguiwindow.ui b/pps_ws/src/d_fall_gui/src/mainguiwindow.ui
new file mode 100644
index 0000000000000000000000000000000000000000..5edea92c20030060a076581e1e80a30f3b672a81
--- /dev/null
+++ b/pps_ws/src/d_fall_gui/src/mainguiwindow.ui
@@ -0,0 +1,324 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>MainGUIWindow</class>
+ <widget class="QMainWindow" name="MainGUIWindow">
+  <property name="geometry">
+   <rect>
+    <x>0</x>
+    <y>0</y>
+    <width>1479</width>
+    <height>1029</height>
+   </rect>
+  </property>
+  <property name="windowTitle">
+   <string>MainGUIWindow</string>
+  </property>
+  <widget class="QWidget" name="centralWidget">
+   <widget class="QFrame" name="frame">
+    <property name="geometry">
+     <rect>
+      <x>40</x>
+      <y>510</y>
+      <width>1431</width>
+      <height>451</height>
+     </rect>
+    </property>
+    <property name="frameShape">
+     <enum>QFrame::StyledPanel</enum>
+    </property>
+    <property name="frameShadow">
+     <enum>QFrame::Raised</enum>
+    </property>
+    <widget class="QTabWidget" name="tabWidget">
+     <property name="geometry">
+      <rect>
+       <x>20</x>
+       <y>19</y>
+       <width>1391</width>
+       <height>401</height>
+      </rect>
+     </property>
+     <property name="layoutDirection">
+      <enum>Qt::LeftToRight</enum>
+     </property>
+     <property name="currentIndex">
+      <number>-1</number>
+     </property>
+     <property name="tabsClosable">
+      <bool>true</bool>
+     </property>
+    </widget>
+   </widget>
+   <widget class="QFrame" name="frame_drawing">
+    <property name="geometry">
+     <rect>
+      <x>20</x>
+      <y>18</y>
+      <width>951</width>
+      <height>471</height>
+     </rect>
+    </property>
+    <property name="frameShape">
+     <enum>QFrame::StyledPanel</enum>
+    </property>
+    <property name="frameShadow">
+     <enum>QFrame::Raised</enum>
+    </property>
+    <widget class="myGraphicsView" name="graphicsView">
+     <property name="geometry">
+      <rect>
+       <x>10</x>
+       <y>9</y>
+       <width>741</width>
+       <height>401</height>
+      </rect>
+     </property>
+     <property name="mouseTracking">
+      <bool>true</bool>
+     </property>
+    </widget>
+    <widget class="QPushButton" name="removeTable">
+     <property name="geometry">
+      <rect>
+       <x>340</x>
+       <y>430</y>
+       <width>91</width>
+       <height>21</height>
+      </rect>
+     </property>
+     <property name="text">
+      <string>Remove Table</string>
+     </property>
+    </widget>
+    <widget class="QGroupBox" name="groupBox">
+     <property name="geometry">
+      <rect>
+       <x>790</x>
+       <y>30</y>
+       <width>121</width>
+       <height>121</height>
+      </rect>
+     </property>
+     <property name="title">
+      <string>Creation Modes</string>
+     </property>
+     <widget class="QRadioButton" name="radioButton_table_mode">
+      <property name="geometry">
+       <rect>
+        <x>10</x>
+        <y>20</y>
+        <width>61</width>
+        <height>20</height>
+       </rect>
+      </property>
+      <property name="font">
+       <font>
+        <pointsize>7</pointsize>
+       </font>
+      </property>
+      <property name="text">
+       <string>Tables</string>
+      </property>
+      <property name="checked">
+       <bool>true</bool>
+      </property>
+     </widget>
+     <widget class="QRadioButton" name="radioButton_crazyfly_zones_mode">
+      <property name="geometry">
+       <rect>
+        <x>10</x>
+        <y>50</y>
+        <width>91</width>
+        <height>16</height>
+       </rect>
+      </property>
+      <property name="font">
+       <font>
+        <pointsize>7</pointsize>
+       </font>
+      </property>
+      <property name="text">
+       <string>CrazyFly Zones</string>
+      </property>
+      <property name="checkable">
+       <bool>true</bool>
+      </property>
+     </widget>
+     <widget class="QRadioButton" name="radioButton_lock_mode">
+      <property name="geometry">
+       <rect>
+        <x>10</x>
+        <y>80</y>
+        <width>91</width>
+        <height>16</height>
+       </rect>
+      </property>
+      <property name="font">
+       <font>
+        <pointsize>7</pointsize>
+       </font>
+      </property>
+      <property name="text">
+       <string>Lock</string>
+      </property>
+      <property name="checkable">
+       <bool>true</bool>
+      </property>
+     </widget>
+    </widget>
+    <widget class="QGroupBox" name="groupBox_2">
+     <property name="geometry">
+      <rect>
+       <x>790</x>
+       <y>230</y>
+       <width>120</width>
+       <height>141</height>
+      </rect>
+     </property>
+     <property name="title">
+      <string>What to show</string>
+     </property>
+     <widget class="QCheckBox" name="checkBox_crazyfly_zones">
+      <property name="geometry">
+       <rect>
+        <x>10</x>
+        <y>20</y>
+        <width>101</width>
+        <height>20</height>
+       </rect>
+      </property>
+      <property name="font">
+       <font>
+        <pointsize>7</pointsize>
+       </font>
+      </property>
+      <property name="text">
+       <string>Show CF Zones</string>
+      </property>
+      <property name="checked">
+       <bool>true</bool>
+      </property>
+     </widget>
+     <widget class="QCheckBox" name="checkBox_vicon_markers">
+      <property name="geometry">
+       <rect>
+        <x>10</x>
+        <y>40</y>
+        <width>101</width>
+        <height>20</height>
+       </rect>
+      </property>
+      <property name="font">
+       <font>
+        <pointsize>7</pointsize>
+       </font>
+      </property>
+      <property name="text">
+       <string>Show Markers</string>
+      </property>
+     </widget>
+     <widget class="QCheckBox" name="checkBox_grid">
+      <property name="geometry">
+       <rect>
+        <x>10</x>
+        <y>60</y>
+        <width>101</width>
+        <height>20</height>
+       </rect>
+      </property>
+      <property name="font">
+       <font>
+        <pointsize>7</pointsize>
+       </font>
+      </property>
+      <property name="text">
+       <string>Show Grid</string>
+      </property>
+      <property name="checked">
+       <bool>true</bool>
+      </property>
+     </widget>
+     <widget class="QCheckBox" name="checkBox_table">
+      <property name="geometry">
+       <rect>
+        <x>10</x>
+        <y>80</y>
+        <width>101</width>
+        <height>20</height>
+       </rect>
+      </property>
+      <property name="font">
+       <font>
+        <pointsize>7</pointsize>
+       </font>
+      </property>
+      <property name="text">
+       <string>Show Table</string>
+      </property>
+      <property name="checked">
+       <bool>true</bool>
+      </property>
+     </widget>
+    </widget>
+    <widget class="QPushButton" name="pushButton_fitAll">
+     <property name="geometry">
+      <rect>
+       <x>800</x>
+       <y>410</y>
+       <width>51</width>
+       <height>51</height>
+      </rect>
+     </property>
+     <property name="text">
+      <string>Fit AllT</string>
+     </property>
+    </widget>
+   </widget>
+   <widget class="QFrame" name="frame_3">
+    <property name="geometry">
+     <rect>
+      <x>990</x>
+      <y>20</y>
+      <width>481</width>
+      <height>469</height>
+     </rect>
+    </property>
+    <property name="frameShape">
+     <enum>QFrame::StyledPanel</enum>
+    </property>
+    <property name="frameShadow">
+     <enum>QFrame::Raised</enum>
+    </property>
+   </widget>
+  </widget>
+  <widget class="QMenuBar" name="menuBar">
+   <property name="geometry">
+    <rect>
+     <x>0</x>
+     <y>0</y>
+     <width>1479</width>
+     <height>19</height>
+    </rect>
+   </property>
+  </widget>
+  <widget class="QToolBar" name="mainToolBar">
+   <attribute name="toolBarArea">
+    <enum>TopToolBarArea</enum>
+   </attribute>
+   <attribute name="toolBarBreak">
+    <bool>false</bool>
+   </attribute>
+  </widget>
+  <widget class="QStatusBar" name="statusBar"/>
+ </widget>
+ <layoutdefault spacing="6" margin="11"/>
+ <customwidgets>
+  <customwidget>
+   <class>myGraphicsView</class>
+   <extends>QGraphicsView</extends>
+   <header>../../include/myGraphicsView.h</header>
+  </customwidget>
+ </customwidgets>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/pps_ws/src/d_fall_gui/src/myGraphicsRectItem.cpp b/pps_ws/src/d_fall_gui/src/myGraphicsRectItem.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c214eb42545e77790f275ba30348ce8bfcb4455e
--- /dev/null
+++ b/pps_ws/src/d_fall_gui/src/myGraphicsRectItem.cpp
@@ -0,0 +1,221 @@
+#include "../../include/myGraphicsRectItem.h"
+
+#include <QGraphicsSceneMouseEvent>
+#include <QGraphicsRectItem>
+#include <QPen>
+#include <QBrush>
+#include <QApplication>
+#include <QVariant>
+#include <string.h>
+
+myGraphicsRectItem::myGraphicsRectItem(const QRectF & rect,  QGraphicsItem * parent)
+    : QGraphicsRectItem(rect, parent)
+{
+    unlock();
+    this->setFlag(QGraphicsItem::ItemSendsScenePositionChanges);
+    pen = new QPen(Qt::red);
+    brush = new QBrush(Qt::red);
+
+    tmp_rect = 0;
+    _grabbers_created = false;
+    resize_mode = false;
+}
+
+void myGraphicsRectItem::lock()
+{
+    this->setFlag(QGraphicsItem::ItemIsSelectable, false);
+    this->setFlag(QGraphicsItem::ItemIsMovable, false);
+    locked = true;
+}
+
+void myGraphicsRectItem::unlock()
+{
+    this->setFlag(QGraphicsItem::ItemIsSelectable, true);
+    this->setFlag(QGraphicsItem::ItemIsMovable, true);
+    locked = false;
+}
+
+void myGraphicsRectItem::deleteGrabbers()
+{
+    _bottomLeft_corner->setParentItem(NULL);
+    _topLeft_corner->setParentItem(NULL);
+    _topRight_corner->setParentItem(NULL);
+    _bottomRight_corner->setParentItem(NULL);
+
+    delete _bottomLeft_corner;
+    delete _topLeft_corner;
+    delete _topRight_corner;
+    delete _bottomRight_corner;
+    _grabbers_created = false;
+}
+
+QVariant myGraphicsRectItem::itemChange(GraphicsItemChange change, const QVariant &value)
+{
+    if (change == QGraphicsItem::ItemSelectedChange)
+    {
+        if (value == true)
+        {
+            qDebug("selected!");
+            createGrabbers();
+        }
+        else
+        {
+            qDebug("unselected!");
+            deleteGrabbers();
+        }
+    }
+    if (change == QGraphicsItem::ItemPositionChange && scene()) // with this, block movement of object when we are hovering through cornergrabbers
+    {
+        if(checkCornerGrabbers() == CornerGrabber::noCorner)
+        {
+            qDebug("move now!");
+            return QPointF(value.toPointF().x(), value.toPointF().y());
+        }
+        else
+        {
+            qDebug("dont move now!");
+            return QPointF(pos().x(), pos().y());
+        }
+    }
+
+
+    return QGraphicsItem::itemChange(change, value);
+}
+
+bool myGraphicsRectItem::grabbersAreCreated()
+{
+    return _grabbers_created;
+}
+
+void myGraphicsRectItem::setCornerPositions() //need to call this function whenever we chnge the size of the rectangle
+{
+    QRectF rect = this->rect();
+    _bottomLeft_corner->setPos(rect.bottomLeft().x(), rect.bottomLeft().y());
+    _topLeft_corner->setPos(rect.topLeft().x(), rect.topLeft().y());
+    _topRight_corner->setPos(rect.topRight().x(), rect.topRight().y());
+    _bottomRight_corner->setPos(rect.bottomRight().x(), rect.bottomRight().y());
+}
+
+void myGraphicsRectItem::createGrabbers()
+{
+    if(!grabbersAreCreated())
+    {
+        _bottomLeft_corner = new CornerGrabber(this, CornerGrabber::bottomLeft);
+        _topLeft_corner = new CornerGrabber(this, CornerGrabber::topLeft);
+        _topRight_corner = new CornerGrabber(this, CornerGrabber::topRight);
+        _bottomRight_corner = new CornerGrabber(this, CornerGrabber::bottomRight);
+
+
+        _bottomLeft_corner->installSceneEventFilter(this);
+        _topLeft_corner->installSceneEventFilter(this);
+        _topRight_corner->installSceneEventFilter(this);
+        _bottomRight_corner->installSceneEventFilter(this);
+
+        _grabbers_created = true;
+        setCornerPositions();
+    }
+}
+
+int myGraphicsRectItem::checkCornerGrabbers()
+{
+
+    // we return the first active found. Hopefully we never have two at the same time.
+    if(_bottomLeft_corner->isActive())
+        return CornerGrabber::bottomLeft;
+    else if(_topLeft_corner->isActive())
+        return CornerGrabber::topLeft;
+    else if (_topRight_corner->isActive())
+        return CornerGrabber::topRight;
+    else if( _bottomRight_corner->isActive())
+        return CornerGrabber::bottomRight;
+    else
+        return CornerGrabber::noCorner;               //0 is none
+}
+
+bool myGraphicsRectItem::anyGrabber()
+{
+    if(checkCornerGrabbers() != CornerGrabber::noCorner)
+        return true;
+    else
+        return false;
+}
+
+void myGraphicsRectItem::mousePressEvent(QGraphicsSceneMouseEvent *mouseEvent)
+{
+    if (mouseEvent->button() != Qt::LeftButton)
+        return;
+
+    if(!locked)
+    {
+        createGrabbers();           //This is just in case they have not been created by now. We have a creator guardian anyhow. Change this maybe?
+        if(anyGrabber())
+        {
+            resize_mode = true;
+        }
+    }
+
+    QGraphicsRectItem::mousePressEvent(mouseEvent);
+}
+
+void myGraphicsRectItem::mouseMoveEvent(QGraphicsSceneMouseEvent *mouseEvent)
+{
+    this->prepareGeometryChange();
+    if(resize_mode)
+    {
+        switch(checkCornerGrabbers())
+        {
+            case CornerGrabber::bottomLeft:
+            {
+                qDebug("bottomLeft");
+                QRectF resize_rect = this->rect();
+                resize_rect.setBottomLeft(mouseEvent->pos());
+                this->setRect(resize_rect.normalized());
+                break;
+            }
+            case CornerGrabber::topLeft:
+            {
+                qDebug("topLeft");
+                QRectF resize_rect = this->rect();
+                resize_rect.setTopLeft(mouseEvent->pos());
+                this->setRect(resize_rect.normalized());
+                break;
+            }
+            case CornerGrabber::topRight:
+            {
+                qDebug("topRight");
+                QRectF resize_rect = this->rect();
+                resize_rect.setTopRight(mouseEvent->pos());
+                this->setRect(resize_rect.normalized());
+                break;
+            }
+            case CornerGrabber::bottomRight:
+            {
+                qDebug("bottomRight");
+                QRectF resize_rect = this->rect();
+                resize_rect.setBottomRight(mouseEvent->pos());
+                this->setRect(resize_rect.normalized());
+                break;
+            }
+            case CornerGrabber::noCorner:
+                qDebug("No corner. Should never enter here");
+            default:
+                break;
+        }
+        setCornerPositions();
+        rectSizeChanged();
+    }
+    QGraphicsRectItem::mouseMoveEvent(mouseEvent);
+}
+
+void myGraphicsRectItem::mouseReleaseEvent(QGraphicsSceneMouseEvent *mouseEvent)
+{
+
+    if (mouseEvent->button() != Qt::LeftButton)
+        return;
+    // TODO: stop resize mode
+    if(resize_mode)
+    {
+        resize_mode = false;
+    }
+    QGraphicsRectItem::mouseReleaseEvent(mouseEvent);
+}
diff --git a/pps_ws/src/d_fall_gui/src/myGraphicsScene.cpp b/pps_ws/src/d_fall_gui/src/myGraphicsScene.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bdbc4e54e032523c11878b5df8f5c024c026c368
--- /dev/null
+++ b/pps_ws/src/d_fall_gui/src/myGraphicsScene.cpp
@@ -0,0 +1,400 @@
+#include "../../include/myGraphicsScene.h"
+
+#include <QGraphicsSceneMouseEvent>
+#include <QRect>
+#include <QApplication>
+#include <QKeyEvent>
+
+myGraphicsScene::myGraphicsScene(QObject *parent)
+    : QGraphicsScene(parent)
+{
+    pen = new QPen(Qt::black);
+    brush = new QBrush(Qt::blue);
+    setMode(mode_table);
+    tmp_rect = 0;
+    startedRect = false;
+    setGrid(true);
+}
+
+
+void myGraphicsScene::keyPressEvent(QKeyEvent * keyEvent)
+{
+
+    switch(mode)
+    {
+        case mode_table:
+        {
+            if(keyEvent->key() == Qt::Key_Delete)
+            {
+                qDebug("del key pressed");
+                for(unsigned int i = 0; i < table_pieces.size(); i++)
+                {
+                    if(table_pieces[i]->isSelected())
+                    {
+                        qDebug("selectedRectangle: %d", i);
+                        removeTablePiece(i);
+                    }
+                }
+            }
+            break;
+        }
+        case mode_crazyfly_zones:
+        {
+            if(keyEvent->key() == Qt::Key_Delete)
+            {
+                qDebug("del key pressed");
+                for(unsigned int i = 0; i < crazyfly_zones.size(); i++)
+                {
+                    if(crazyfly_zones[i]->isSelected())
+                    {
+                        qDebug("selectedRectangle: %d", i);
+                        removeCrazyFlyZone(i);
+                    }
+                }
+            }
+            break;
+        }
+        case mode_locked:
+        {
+            // nothing so far
+            break;
+        }
+        default:
+            break;
+    }
+
+    QGraphicsScene::keyPressEvent(keyEvent);
+}
+
+void myGraphicsScene::setSelectedCrazyFlyZone(int index)
+{
+    for(unsigned int i = 0; i < crazyfly_zones.size(); i++)
+    {
+        crazyfly_zones[i]->setSelected(false);
+        if(index == i)
+        {
+            crazyfly_zones[index]->setSelected(true);
+        }
+    }
+}
+
+int myGraphicsScene::checkSelectedCrazyFlyZone()
+{
+    for(unsigned int i = 0; i < crazyfly_zones.size(); i++)
+    {
+        if(crazyfly_zones[i]->isSelected())
+        {
+            qDebug("rectangle selected index = %d", i);
+            return i;
+        }
+    }
+    return -1;
+}
+
+void myGraphicsScene::mousePressEvent(QGraphicsSceneMouseEvent *mouseEvent)
+{
+    if (mouseEvent->button() != Qt::LeftButton)
+        return;
+    if(Qt::ControlModifier == QApplication::keyboardModifiers())
+    {
+        switch(mode)
+        {
+            case mode_table:
+            {
+                startedRect = true;
+                p1 = new QPointF(mouseEvent->scenePos());
+                tmp_rect = new QRectF(*p1, *p1);
+                tmp_table_piece_item = new tablePiece(*tmp_rect);
+                addItem(tmp_table_piece_item);
+                break;
+            }
+            case mode_crazyfly_zones:
+            {
+                startedRect = true;
+                p1 = new QPointF(mouseEvent->scenePos());
+                tmp_rect = new QRectF(*p1, *p1);
+                int index = crazyfly_zones.size();
+                tmp_crazyfly_zone_item = new crazyFlyZone(*tmp_rect, index);
+                addItem(tmp_crazyfly_zone_item);
+                break;
+            }
+            case mode_locked:
+            {
+                // do nothing so far
+                startedRect = false;
+                break;
+            }
+            default:
+                break;
+        }
+    }
+    else
+    {
+        QGraphicsScene::mousePressEvent(mouseEvent);
+    }
+}
+
+void myGraphicsScene::mouseMoveEvent(QGraphicsSceneMouseEvent *mouseEvent)
+{
+    if(startedRect)
+    {
+        switch(mode)
+        {
+            case mode_table:
+            {
+                tmp_table_piece_item->setRect(QRectF(*p1, mouseEvent->scenePos()));
+                break;
+            }
+            case mode_crazyfly_zones:
+            {
+                tmp_crazyfly_zone_item->setRect(QRectF(*p1, mouseEvent->scenePos()));
+                qDebug("Mouse Position: %d, %d", (mouseEvent->scenePos()).toPoint().x(), (mouseEvent->scenePos()).toPoint().y());
+                qDebug("Rectangle BottomRight Position: %d, %d", tmp_crazyfly_zone_item->rect().bottomRight().x(), tmp_crazyfly_zone_item->rect().bottomRight().y());
+                break;
+            }
+        }
+
+    }
+    QGraphicsScene::mouseMoveEvent(mouseEvent);
+}
+
+void myGraphicsScene::addCrazyFlyZoneToVector(crazyFlyZone* rect)
+{
+    crazyfly_zones.push_back(rect);
+    emit numCrazyFlyZonesChanged(crazyfly_zones.size());
+}
+
+void myGraphicsScene::addTablePieceToVector(tablePiece* rect)
+{
+    table_pieces.push_back(rect);
+    emit numTablePiecesChanged(table_pieces.size());
+}
+
+void myGraphicsScene::updateIndexesAndLabelsCrazyFlyZones()
+{
+    for(int i = 0; i < crazyfly_zones.size(); i++)
+    {
+        crazyfly_zones[i]->setIndex(i);
+        std::string str = std::to_string(i + 1);
+        crazyfly_zones[i]->updateLabel(str.c_str());
+        qDebug("reset Index %d and update label",i);
+
+    }
+}
+
+void myGraphicsScene::changeModeTo(int next_mode)
+{
+    mode = next_mode;
+}
+
+int myGraphicsScene::getMode(void)
+{
+    return mode;
+}
+
+void myGraphicsScene::lockTablePieces(void)
+{
+    for(int i = 0; i < table_pieces.size(); i++)
+    {
+        table_pieces[i]->lock();
+        table_pieces[i]->setDarkColor();
+    }
+}
+
+void myGraphicsScene::unlockTablePieces(void)
+{
+    for(int i = 0; i < table_pieces.size(); i++)
+    {
+        table_pieces[i]->unlock();
+        table_pieces[i]->setLightColor();
+    }
+}
+
+void myGraphicsScene::lockCrazyFlyZones(void)
+{
+    for(int i = 0; i < crazyfly_zones.size(); i++)
+    {
+        crazyfly_zones[i]->lock();
+    }
+}
+
+void myGraphicsScene::unlockCrazyFlyZones(void)
+{
+    for(int i = 0; i < crazyfly_zones.size(); i++)
+    {
+        crazyfly_zones[i]->unlock();
+    }
+}
+
+void myGraphicsScene::setMode(int new_mode)
+{
+    switch(new_mode)
+    {
+        case mode_table:
+        {
+            lockCrazyFlyZones();
+            unlockTablePieces();
+            break;
+        }
+        case mode_crazyfly_zones:
+        {
+            lockTablePieces();
+            unlockCrazyFlyZones();
+            break;
+        }
+        case mode_locked:
+        {
+            // TODO: define locked mode. Do not allow to create anything, change some color of something to state that we are in that mode
+            lockTablePieces();
+            lockCrazyFlyZones();
+            break;
+        }
+    }
+    mode = new_mode;
+    emit modeChanged(new_mode);
+}
+
+void myGraphicsScene::setGrid(bool enable)
+{
+    grid_enable = enable;
+    update();
+}
+
+void myGraphicsScene::hideTable()
+{
+    for(int i = 0; i < table_pieces.size(); i++)
+    {
+        this->removeItem(table_pieces[i]);
+    }
+}
+
+void myGraphicsScene::showTable()
+{
+    for(int i = 0; i < table_pieces.size(); i++)
+    {
+        this->addItem(table_pieces[i]);
+    }
+}
+
+void myGraphicsScene::hideCrazyFlyZones()
+{
+    for(int i = 0; i < crazyfly_zones.size(); i++)
+    {
+        this->removeItem(crazyfly_zones[i]);
+    }
+}
+
+void myGraphicsScene::showCrazyFlyZones()
+{
+    for(int i = 0; i < crazyfly_zones.size(); i++)
+    {
+        this->addItem(crazyfly_zones[i]);
+    }
+}
+
+QRectF myGraphicsScene::getRectFCrazyFlyZone(int index)
+{
+    QRectF rect(crazyfly_zones[index]->sceneBoundingRect());
+    return rect;
+}
+
+void myGraphicsScene::removeCrazyFlyZone(int index)
+{
+    this->removeItem(crazyfly_zones[index]);
+    crazyfly_zones.erase(crazyfly_zones.begin() + index);
+    qDebug("removed CFzone %d", index);
+    updateIndexesAndLabelsCrazyFlyZones();
+    emit numCrazyFlyZonesChanged(crazyfly_zones.size()); // for tab managing
+}
+
+void myGraphicsScene::removeTable()
+{
+    for(int i = 0; i < table_pieces.size(); i++)
+    {
+        this->removeItem(table_pieces[i]);
+    }
+    table_pieces.clear();
+    emit numTablePiecesChanged(table_pieces.size());
+}
+
+void myGraphicsScene::drawBackground(QPainter *painter, const QRectF &rect)
+{
+
+    if(grid_enable)
+    {
+        const int gridSize = 25;
+
+        qreal left = int(rect.left()) - (int(rect.left()) % gridSize);
+        qreal top = int(rect.top()) - (int(rect.top()) % gridSize);
+
+        QVarLengthArray<QLineF, 100> lines;
+
+        for (qreal x = left; x < rect.right(); x += gridSize)
+            lines.append(QLineF(x, rect.top(), x, rect.bottom()));
+        for (qreal y = top; y < rect.bottom(); y += gridSize)
+            lines.append(QLineF(rect.left(), y, rect.right(), y));
+
+        // qDebug() << lines.size();
+
+        painter->setPen(QPen(QColor(0, 0, 0, 0x40), 0));
+        painter->drawLines(lines.data(), lines.size());
+    }
+}
+
+void myGraphicsScene::removeTablePiece(int index)
+{
+    this->removeItem(table_pieces[index]);
+    table_pieces.erase(table_pieces.begin() + index);
+    qDebug("removed TabledPiece %d", index);
+    emit numTablePiecesChanged(table_pieces.size());
+}
+
+void myGraphicsScene::mouseReleaseEvent(QGraphicsSceneMouseEvent *mouseEvent)
+{
+        if (mouseEvent->button() != Qt::LeftButton)
+            return;
+
+        switch(mode)
+        {
+            case mode_table:
+            {
+                if(startedRect)
+                {
+                    tmp_table_piece_item->setRect(tmp_table_piece_item->rect().normalized());
+                    addTablePieceToVector(tmp_table_piece_item);
+                    tmp_rect = 0;
+                    startedRect = false;
+                }
+                break;
+            }
+            case mode_crazyfly_zones:
+            {
+                if(startedRect)
+                {
+                    tmp_crazyfly_zone_item->setRect(tmp_crazyfly_zone_item->rect().normalized());
+                    addCrazyFlyZoneToVector(tmp_crazyfly_zone_item);
+                    std::string str = std::to_string(crazyfly_zones.size());
+                    tmp_crazyfly_zone_item->setLabel(str.c_str());
+                    setSelectedCrazyFlyZone(crazyfly_zones.size() - 1); //select just created rectangle
+                    tmp_rect = 0;
+                    startedRect = false;
+                }
+                int selected_crazyfly_zone = checkSelectedCrazyFlyZone();
+                if(selected_crazyfly_zone != -1)
+                {
+                    emit crazyFlyZoneSelected(selected_crazyfly_zone);
+                }
+                break;
+            }
+            case mode_locked:
+            {
+                // Do nothing so far..
+                break;
+            }
+            default:
+                break;
+        }
+
+
+    QGraphicsScene::mouseReleaseEvent(mouseEvent);
+}
diff --git a/pps_ws/src/d_fall_gui/src/myGraphicsView.cpp b/pps_ws/src/d_fall_gui/src/myGraphicsView.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9b0d03007f9b0ee08f0cebc0fa8d90f727246ae9
--- /dev/null
+++ b/pps_ws/src/d_fall_gui/src/myGraphicsView.cpp
@@ -0,0 +1,62 @@
+#include "../../include/myGraphicsView.h"
+
+#include <QApplication>
+#include <QMouseEvent>
+
+myGraphicsView::myGraphicsView(QWidget *parent)
+    : QGraphicsView(parent)
+{
+    translation_mode = false;
+}
+
+
+void myGraphicsView::wheelEvent(QWheelEvent *event)
+{
+
+    if(Qt::ControlModifier == QApplication::keyboardModifiers())
+    {
+        this->setTransformationAnchor(QGraphicsView::AnchorUnderMouse);
+        // Scale the view / do the zoom
+        double scaleFactor = 1.15;
+        if(event->delta() > 0) {
+            // Zoom in
+            this->scale(scaleFactor, scaleFactor);
+
+        } else {
+            // Zooming out
+            this->scale(1.0 / scaleFactor, 1.0 / scaleFactor);
+        }
+    }
+    else
+    {
+        QGraphicsView::wheelEvent(event); // dont propagate if we are zooming. If we propagate, we will also scroll
+    }
+}
+
+void myGraphicsView::mousePressEvent(QMouseEvent *mouseEvent)
+{
+    if (mouseEvent->button() == Qt::MiddleButton)
+    {
+        translation_mode = true;
+        tmp_point = new QPointF(mouseEvent->localPos());
+    }
+    QGraphicsView::mousePressEvent(mouseEvent);
+}
+
+void myGraphicsView::mouseMoveEvent(QMouseEvent *mouseEvent)
+{
+    if(translation_mode)
+    {
+        translate_dx = mouseEvent->localPos().x() - tmp_point->x();
+        translate_dy = mouseEvent->localPos().y() - tmp_point->y();
+        this->translate(translate_dx, translate_dy);
+    }
+    QGraphicsView::mouseMoveEvent(mouseEvent);
+}
+
+
+void myGraphicsView::mouseReleaseEvent(QMouseEvent *mouseEvent)
+{
+    translation_mode = false;
+    QGraphicsView::mouseReleaseEvent(mouseEvent);
+}
diff --git a/pps_ws/src/d_fall_gui/src/tablePiece.cpp b/pps_ws/src/d_fall_gui/src/tablePiece.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..71f43b79c493c386e797c2af389af708a7991dd4
--- /dev/null
+++ b/pps_ws/src/d_fall_gui/src/tablePiece.cpp
@@ -0,0 +1,23 @@
+#include "../../include/tablePiece.h"
+
+tablePiece::tablePiece(const QRectF & rect,  QGraphicsItem * parent)
+    : myGraphicsRectItem(rect, parent)
+{
+    setLightColor();
+    this->setPen(Qt::NoPen);
+    this->setZValue(-10);
+}
+
+void tablePiece::rectSizeChanged() // pure virtual coming from parent
+{
+}
+
+void tablePiece::setLightColor()
+{
+    this->setBrush(QColor(123, 209, 226)); //last byte is transparency
+}
+
+void tablePiece::setDarkColor()
+{
+    this->setBrush(QColor(10, 103, 164)); //last byte is transparency
+}