diff --git a/pps_ws/src/d_fall_pps/CMakeLists.txt b/pps_ws/src/d_fall_pps/CMakeLists.txt
index 1247d944bd944b45d9154945b4ca10fdcd6b4df7..48bfe27fea1d4e18f178ec2994fadd7f237e94ee 100755
--- a/pps_ws/src/d_fall_pps/CMakeLists.txt
+++ b/pps_ws/src/d_fall_pps/CMakeLists.txt
@@ -42,10 +42,6 @@ set(MY_GUI_LIB_PATH_INC ${PROJECT_SOURCE_DIR}/GUI_Qt/CrazyFlyGUI/include)
 set(STUDENT_GUI_LIB_PATH_SRC ${PROJECT_SOURCE_DIR}/GUI_Qt/studentGUI/src)
 set(STUDENT_GUI_LIB_PATH_INC ${PROJECT_SOURCE_DIR}/GUI_Qt/studentGUI/include)
 
-# CoorindatorGUI -- Add src and includes
-set(COORDINATOR_GUI_LIB_PATH_SRC ${PROJECT_SOURCE_DIR}/GUI_Qt/coordinatorGUI/src)
-set(COORDINATOR_GUI_LIB_PATH_INC ${PROJECT_SOURCE_DIR}/GUI_Qt/coordinatorGUI/include)
-
 # GUI -- Resource file
 set(MY_RESOURCE_FILE_QRC ${PROJECT_SOURCE_DIR}/GUI_Qt/CrazyFlyGUI/CrazyFlyGUI.qrc)
 
@@ -89,17 +85,6 @@ qt5_wrap_ui(UIS_HDRS_STUDENT_GUI ${STUDENT_GUI_LIB_PATH_SRC}/MainWindow.ui)
 qt5_wrap_cpp(SRC_MOC_HDRS_STUDENT_GUI ${SRC_HDRS_QOBJECT_STUDENT_GUI})
 
 
-# CoordinatorGUI -- Special Qt sources that need to be wrapped before being compiled
-
-set(SRC_HDRS_QOBJECT_COORDINATOR_GUI
-  ${COORDINATOR_GUI_LIB_PATH_INC}/MainWindow.h
-  ${COORDINATOR_GUI_LIB_PATH_INC}/rosNodeThread.h
-  )
-
-# CoordinatorGUI -- wrap UI file and QOBJECT files
-qt5_wrap_ui(UIS_HDRS_COORDINATOR_GUI ${COORDINATOR_GUI_LIB_PATH_SRC}/MainWindow.ui)
-qt5_wrap_cpp(SRC_MOC_HDRS_COORDINATOR_GUI ${SRC_HDRS_QOBJECT_COORDINATOR_GUI})
-
 
 ## Uncomment this if the package has a setup.py. This macro ensures
 ## modules and global scripts declared therein get installed
@@ -217,7 +202,6 @@ generate_messages(
 catkin_package(
   INCLUDE_DIRS include ${MY_GUI_LIB_PATH_INC}            # GUI -- include headers from GUI in package
   INCLUDE_DIRS include ${STUDENT_GUI_LIB_PATH_INC}       # StudentGUI -- include headers from GUI in package
-  INCLUDE_DIRS include ${COORDINATOR_GUI_LIB_PATH_INC}   # CoordinatorGUI -- include headers from GUI in package
   LIBRARIES
   CATKIN_DEPENDS roscpp rospy std_msgs rosbag roslib
   DEPENDS
@@ -233,7 +217,6 @@ catkin_package(
 include_directories(
   ${MY_GUI_LIB_PATH_INC}            # GUI -- include directory inside GUI folder
   ${STUDENT_GUI_LIB_PATH_INC}       # StudentGUI -- include directory inside GUI folder
-  ${COORDINATOR_GUI_LIB_PATH_INC}   # CoordinatorGUI -- include directory inside GUI folder
   ${catkin_INCLUDE_DIRS}
   include
   include/nodes
@@ -291,12 +274,6 @@ set(MY_CPP_SOURCES_STUDENT_GUI              # compilation of sources
     ${STUDENT_GUI_LIB_PATH_SRC}/rosNodeThread.cpp
     )
 
-# CoordinatorGUI -- Add sources here
-set(MY_CPP_SOURCES_COORDINATOR_GUI              # compilation of sources
-    ${COORDINATOR_GUI_LIB_PATH_SRC}/MainWindow.cpp
-    ${COORDINATOR_GUI_LIB_PATH_SRC}/main.cpp
-    ${COORDINATOR_GUI_LIB_PATH_SRC}/rosNodeThread.cpp
-    )
 
 
 # GUI -- Add executables here
@@ -307,9 +284,6 @@ qt5_use_modules(my_GUI Widgets)
 add_executable(student_GUI ${MY_CPP_SOURCES_STUDENT_GUI} ${UIS_HDRS_STUDENT_GUI} ${SRC_MOC_HDRS_STUDENT_GUI})
 qt5_use_modules(student_GUI Widgets)
 
-# CoordinatorGUI -- Add executables here
-add_executable(coordinator_GUI ${MY_CPP_SOURCES_COORDINATOR_GUI} ${UIS_HDRS_COORDINATOR_GUI} ${SRC_MOC_HDRS_COORDINATOR_GUI})
-qt5_use_modules(coordinator_GUI Widgets)
 
 
 
@@ -328,9 +302,6 @@ add_dependencies(my_GUI d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGE
 # StudentGUI-- dependencies
 add_dependencies(student_GUI d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
 
-# CoordinatorGUI-- dependencies
-add_dependencies(coordinator_GUI d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
-
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
 ## target back to the shorter version for ease of user use
@@ -372,10 +343,6 @@ target_link_libraries(my_GUI ${catkin_LIBRARIES})
 target_link_libraries(student_GUI Qt5::Widgets) # GUI -- let my_GUI have acesss to Qt stuff
 target_link_libraries(student_GUI ${catkin_LIBRARIES})
 
-# CoordinatorGUI -- link libraries
-target_link_libraries(coordinator_GUI Qt5::Widgets) # GUI -- let coordinator_GUI have acesss to Qt stuff
-target_link_libraries(coordinator_GUI ${catkin_LIBRARIES})
-
 
 #############
 ## Install ##
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/coordinatorGUI/coordinatorGUI.pro b/pps_ws/src/d_fall_pps/GUI_Qt/coordinatorGUI/coordinatorGUI.pro
deleted file mode 100644
index 4f43aedbed97bb2800fd5eac5501c59fad890f7b..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/GUI_Qt/coordinatorGUI/coordinatorGUI.pro
+++ /dev/null
@@ -1,25 +0,0 @@
-#-------------------------------------------------
-#
-# Project created by QtCreator 2017-08-21T11:01:25
-#
-#-------------------------------------------------
-
-QT       += core gui
-
-greaterThan(QT_MAJOR_VERSION, 4): QT += widgets
-
-TARGET = studentGUI
-TEMPLATE = app
-
-INCLUDEPATH += $$PWD/include
-CONFIG += c++11
-
-SOURCES += \
-         src/main.cpp \
-         src/MainWindow.cpp
-
-HEADERS  += \
-         include/MainWindow.h \
-
-FORMS    += \
-         src/MainWindow.ui
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/coordinatorGUI/include/MainWindow.h b/pps_ws/src/d_fall_pps/GUI_Qt/coordinatorGUI/include/MainWindow.h
deleted file mode 100644
index be4888a000e4ac892adc5da572790d1404c9a286..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/GUI_Qt/coordinatorGUI/include/MainWindow.h
+++ /dev/null
@@ -1,217 +0,0 @@
-//    Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat, Angel Romero
-//
-//    This file is part of D-FaLL-System.
-//    
-//    D-FaLL-System is free software: you can redistribute it and/or modify
-//    it under the terms of the GNU General Public License as published by
-//    the Free Software Foundation, either version 3 of the License, or
-//    (at your option) any later version.
-//    
-//    D-FaLL-System is distributed in the hope that it will be useful,
-//    but WITHOUT ANY WARRANTY; without even the implied warranty of
-//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-//    GNU General Public License for more details.
-//    
-//    You should have received a copy of the GNU General Public License
-//    along with D-FaLL-System.  If not, see <http://www.gnu.org/licenses/>.
-//    
-//
-//    ----------------------------------------------------------------------------------
-//    DDDD        FFFFF        L     L           SSSS  Y   Y   SSSS  TTTTT  EEEEE  M   M
-//    D   D       F      aaa   L     L          S       Y Y   S        T    E      MM MM
-//    D   D  ---  FFFF  a   a  L     L     ---   SSS     Y     SSS     T    EEE    M M M
-//    D   D       F     a  aa  L     L              S    Y        S    T    E      M   M
-//    DDDD        F      aa a  LLLL  LLLL       SSSS     Y    SSSS     T    EEEEE  M   M
-//
-//
-//    DESCRIPTION:
-//    Main window of the Student's GUI
-//
-//    ----------------------------------------------------------------------------------
-
-
-#ifndef MAINWINDOW_H
-#define MAINWINDOW_H
-
-#include <QMainWindow>
-
-#include <std_msgs/Int32.h>
-#include <std_msgs/Float32.h>
-
-#include "rosNodeThread.h"
-
-#include "d_fall_pps/CrazyflieContext.h"
-#include "d_fall_pps/CrazyflieData.h"
-#include "d_fall_pps/Setpoint.h"
-
-
-// Types of controllers being used:
-#define SAFE_CONTROLLER   0
-#define CUSTOM_CONTROLLER 1
-
-// Commands for CrazyRadio
-#define CMD_RECONNECT  0
-#define CMD_DISCONNECT 1
-
-// CrazyRadio states:
-#define CONNECTED        0
-#define CONNECTING       1
-#define DISCONNECTED     2
-
-// The constants that "command" changes in the
-// operation state of this agent. These "commands"
-// are sent from this GUI node to the "PPSClient"
-// node where the command is enacted
-#define CMD_USE_SAFE_CONTROLLER   1
-#define CMD_USE_CUSTOM_CONTROLLER 2
-#define CMD_CRAZYFLY_TAKE_OFF     3
-#define CMD_CRAZYFLY_LAND         4
-#define CMD_CRAZYFLY_MOTORS_OFF   5
-
-// Flying States
-#define STATE_MOTORS_OFF 1
-#define STATE_TAKE_OFF   2
-#define STATE_FLYING     3
-#define STATE_LAND       4
-
-// Battery states
-#define BATTERY_STATE_NORMAL 0
-#define BATTERY_STATE_LOW    1
-
-// For which controller parameters to load
-#define LOAD_YAML_SAFE_CONTROLLER_AGENT           1
-#define LOAD_YAML_CUSTOM_CONTROLLER_AGENT         2
-#define LOAD_YAML_SAFE_CONTROLLER_COORDINATOR     3
-#define LOAD_YAML_CUSTOM_CONTROLLER_COORDINATOR   4
-
-// Universal constants
-#define PI 3.141592653589
-
-#define RAD2DEG 180.0/PI
-#define DEG2RAD PI/180.0
-
-namespace Ui {
-class MainWindow;
-}
-
-class MainWindow : public QMainWindow
-{
-    Q_OBJECT
-
-public:
-    explicit MainWindow(int argc, char **argv, QWidget *parent = 0);
-    ~MainWindow();
-
-private slots:
-    void updateNewViconData(const ptrToMessage& p_msg);
-    void on_RF_Connect_button_clicked();
-
-    void on_take_off_button_clicked();
-
-    void on_land_button_clicked();
-
-    void on_motors_OFF_button_clicked();
-
-    void on_set_setpoint_button_clicked();
-    void on_set_setpoint_button_2_clicked();
-
-    void on_RF_disconnect_button_clicked();
-
-    void on_load_custom_yaml_button_clicked();
-    void on_load_safe_yaml_button_clicked();
-
-    void on_en_custom_controller_clicked();
-
-    void on_en_safe_controller_clicked();
-
-    void on_customButton_1_clicked();
-    void on_customButton_2_clicked();
-    void on_customButton_3_clicked();
-private:
-    Ui::MainWindow *ui;
-
-    rosNodeThread* m_rosNodeThread;
-    int m_radio_status;
-    float m_battery_voltage;
-    int m_battery_level;
-
-    std::string m_ros_namespace;
-
-    ros::Timer m_timer_yaml_file_for_safe_controller;
-    ros::Timer m_timer_yaml_file_for_custom_controlller;
-
-    int m_student_id;
-    CrazyflieContext m_context;
-
-    Setpoint m_safe_setpoint;
-    Setpoint m_custom_setpoint;
-
-    int m_battery_state;
-
-    ros::Publisher crazyRadioCommandPublisher;
-    ros::Subscriber crazyRadioStatusSubscriber;
-    ros::Publisher PPSClientCommandPublisher;
-    ros::Subscriber CFBatterySubscriber;
-    ros::Subscriber flyingStateSubscriber;
-    ros::Subscriber batteryStateSubscriber;
-
-    ros::Publisher controllerSetpointPublisher;
-    ros::Subscriber safeSetpointSubscriber;
-
-    ros::Publisher customSetpointPublisher;
-    ros::Subscriber customSetpointSubscriber;
-
-    ros::Publisher PPSClientStudentCustomButtonPublisher;
-
-    ros::Subscriber DBChangedSubscriber;
-
-
-
-    // > For publishing a message that requests the
-    //   YAML parameters to be re-loaded from file
-    // > The message contents specify which controller
-    //   the parameters should be re-loaded for
-    ros::Publisher requestLoadControllerYamlPublisher;
-
-    // Subscriber for locking the load the controller YAML
-    // parameters when the Coordintor GUI requests a load
-    ros::Subscriber requestLoadControllerYaml_from_my_GUI_Subscriber;
-
-
-    ros::Subscriber controllerUsedSubscriber;
-
-    ros::ServiceClient centralManager;
-
-    // callbacks
-    void crazyRadioStatusCallback(const std_msgs::Int32& msg);
-    void CFBatteryCallback(const std_msgs::Float32& msg);
-    void flyingStateChangedCallback(const std_msgs::Int32& msg);
-    void safeSetpointCallback(const Setpoint& newSetpoint);
-    void customSetpointCallback(const Setpoint& newSetpoint);
-    void DBChangedCallback(const std_msgs::Int32& msg);
-    void customYamlFileTimerCallback(const ros::TimerEvent&);
-    void safeYamlFileTimerCallback(const ros::TimerEvent&);
-    void requestLoadControllerYaml_from_my_GUI_Callback(const std_msgs::Int32& msg);
-    void controllerUsedChangedCallback(const std_msgs::Int32& msg);
-    void batteryStateChangedCallback(const std_msgs::Int32& msg);
-
-    float fromVoltageToPercent(float voltage);
-    void updateBatteryVoltage(float battery_voltage);
-    void setCrazyRadioStatus(int radio_status);
-    void loadCrazyflieContext();
-    void coordinatesToLocal(CrazyflieData& cf);
-    void initialize_custom_setpoint();
-
-
-    void disableGUI();
-    void enableGUI();
-    void highlightSafeControllerTab();
-    void highlightCustomControllerTab();
-
-    bool setpointInsideBox(Setpoint setpoint, CrazyflieContext context);
-    Setpoint correctSetpointBox(Setpoint setpoint, CrazyflieContext context);
-
-    const std::vector<float> m_cutoff_voltages {3.1966,        3.2711,        3.3061,        3.3229,        3.3423,        3.3592,        3.3694,        3.385,        3.4006,        3.4044,        3.4228,        3.4228,        3.4301,        3.4445,        3.4531,        3.4677,        3.4705,        3.4712,        3.4756,        3.483,        3.4944,        3.5008,        3.5008,        3.5084,        3.511,        3.5122,        3.5243,        3.5329,        3.5412,        3.5529,        3.5609,        3.5625,        3.5638,        3.5848,        3.6016,        3.6089,        3.6223,        3.628,        3.6299,        3.6436,        3.6649,        3.6878,        3.6983,        3.7171,        3.7231,        3.7464,        3.7664,        3.7938,        3.8008,        3.816,        3.8313,        3.8482,        3.866,        3.8857,        3.8984,        3.9159,        3.9302,        3.9691,        3.997,        4.14    };
-};
-
-#endif // MAINWINDOW_H
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/coordinatorGUI/include/rosNodeThread.h b/pps_ws/src/d_fall_pps/GUI_Qt/coordinatorGUI/include/rosNodeThread.h
deleted file mode 100644
index 193561248a04e927fc8b337f9384bbf6e33b07e8..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/GUI_Qt/coordinatorGUI/include/rosNodeThread.h
+++ /dev/null
@@ -1,93 +0,0 @@
-//    Copyright (C) 2017, ETH Zurich, D-ITET, Angel Romero
-//
-//    This file is part of D-FaLL-System.
-//    
-//    D-FaLL-System is free software: you can redistribute it and/or modify
-//    it under the terms of the GNU General Public License as published by
-//    the Free Software Foundation, either version 3 of the License, or
-//    (at your option) any later version.
-//    
-//    D-FaLL-System is distributed in the hope that it will be useful,
-//    but WITHOUT ANY WARRANTY; without even the implied warranty of
-//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-//    GNU General Public License for more details.
-//    
-//    You should have received a copy of the GNU General Public License
-//    along with D-FaLL-System.  If not, see <http://www.gnu.org/licenses/>.
-//    
-//
-//    ----------------------------------------------------------------------------------
-//    DDDD        FFFFF        L     L           SSSS  Y   Y   SSSS  TTTTT  EEEEE  M   M
-//    D   D       F      aaa   L     L          S       Y Y   S        T    E      MM MM
-//    D   D  ---  FFFF  a   a  L     L     ---   SSS     Y     SSS     T    EEE    M M M
-//    D   D       F     a  aa  L     L              S    Y        S    T    E      M   M
-//    DDDD        F      aa a  LLLL  LLLL       SSSS     Y    SSSS     T    EEEEE  M   M
-//
-//
-//    DESCRIPTION:
-//    Takes care of creating a ros node thread.
-//
-//    ----------------------------------------------------------------------------------
-
-
-#ifndef ___ROSNODETHREAD_H___
-#define ___ROSNODETHREAD_H___
-
-#include <QtCore>
-#include <QThread>
-#include <QStringList>
-#include <stdlib.h>
-#include <QMutex>
-#include <iostream>
-#include "assert.h"
-
-#include <ros/ros.h>
-#include <ros/network.h>
-#include "d_fall_pps/UnlabeledMarker.h"
-#include "d_fall_pps/CrazyflieData.h"
-#include "d_fall_pps/ViconData.h"
-
-using namespace d_fall_pps;
-
-typedef ViconData::ConstPtr ptrToMessage;
-
-Q_DECLARE_METATYPE(ptrToMessage)
-
-
-class rosNodeThread : public QObject {
-	Q_OBJECT
-public:
-    explicit rosNodeThread(int argc, char **pArgv, const char * node_name, QObject *parent = 0);
-    virtual ~rosNodeThread();
-
-    bool init();
-
-    // void messageCallback(const ViconData& data);
-    void messageCallback(const ptrToMessage& p_msg);
-
-    ros::ServiceClient m_read_db_client;
-    ros::ServiceClient m_update_db_client;
-    ros::ServiceClient m_command_db_client;
-
-
-signals:
-
-    void newViconData(const ptrToMessage& p_msg);
-
-public slots:
-    void run();
-
-private:
-    int m_Init_argc;
-    char** m_pInit_argv;
-    const char * m_node_name;
-
-    QThread * m_pThread;
-
-    ViconData m_vicon_data;
-
-    ros::Subscriber m_vicon_subscriber;
-    // ros::Publisher  sim_velocity;
-};
-#endif
-
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/coordinatorGUI/src/MainWindow.cpp b/pps_ws/src/d_fall_pps/GUI_Qt/coordinatorGUI/src/MainWindow.cpp
deleted file mode 100644
index d86a3dd007e7c682d4b1049f6f46d01f54b9a2a1..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/GUI_Qt/coordinatorGUI/src/MainWindow.cpp
+++ /dev/null
@@ -1,756 +0,0 @@
-//    Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat, Angel Romero
-//
-//    This file is part of D-FaLL-System.
-//    
-//    D-FaLL-System is free software: you can redistribute it and/or modify
-//    it under the terms of the GNU General Public License as published by
-//    the Free Software Foundation, either version 3 of the License, or
-//    (at your option) any later version.
-//    
-//    D-FaLL-System is distributed in the hope that it will be useful,
-//    but WITHOUT ANY WARRANTY; without even the implied warranty of
-//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-//    GNU General Public License for more details.
-//    
-//    You should have received a copy of the GNU General Public License
-//    along with D-FaLL-System.  If not, see <http://www.gnu.org/licenses/>.
-//    
-//
-//    ----------------------------------------------------------------------------------
-//    DDDD        FFFFF        L     L           SSSS  Y   Y   SSSS  TTTTT  EEEEE  M   M
-//    D   D       F      aaa   L     L          S       Y Y   S        T    E      MM MM
-//    D   D  ---  FFFF  a   a  L     L     ---   SSS     Y     SSS     T    EEE    M M M
-//    D   D       F     a  aa  L     L              S    Y        S    T    E      M   M
-//    DDDD        F      aa a  LLLL  LLLL       SSSS     Y    SSSS     T    EEEEE  M   M
-//
-//
-//    DESCRIPTION:
-//    Main window of the Student's GUI
-//
-//    ----------------------------------------------------------------------------------
-
-
-#include "MainWindow.h"
-#include "ui_MainWindow.h"
-#include <string>
-
-#include <ros/ros.h>
-#include <ros/network.h>
-#include <ros/package.h>
-
-#include "d_fall_pps/CMQuery.h"
-
-#include "d_fall_pps/ViconData.h"
-
-#include "d_fall_pps/CustomButton.h"
-
-MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
-    QMainWindow(parent),
-    ui(new Ui::MainWindow),
-    m_battery_level(0)
-{
-
-    ui->setupUi(this);
-
-    m_rosNodeThread = new rosNodeThread(argc, argv, "student_GUI");
-    m_rosNodeThread->init();
-
-    setCrazyRadioStatus(DISCONNECTED);
-
-    m_ros_namespace = ros::this_node::getNamespace();
-    ROS_INFO("Student GUI node namespace: %s", m_ros_namespace.c_str());
-
-    qRegisterMetaType<ptrToMessage>("ptrToMessage");
-    QObject::connect(m_rosNodeThread, SIGNAL(newViconData(const ptrToMessage&)), this, SLOT(updateNewViconData(const ptrToMessage&)));
-
-    ros::NodeHandle nodeHandle(m_ros_namespace);
-
-    customSetpointPublisher = nodeHandle.advertise<Setpoint>("CustomControllerService/Setpoint", 1);
-    customSetpointSubscriber = nodeHandle.subscribe("CustomControllerService/Setpoint", 1, &MainWindow::customSetpointCallback, this);
-
-    // subscribers
-    crazyRadioStatusSubscriber = nodeHandle.subscribe("CrazyRadio/CrazyRadioStatus", 1, &MainWindow::crazyRadioStatusCallback, this);
-
-    CFBatterySubscriber = nodeHandle.subscribe("CrazyRadio/CFBattery", 1, &MainWindow::CFBatteryCallback, this);
-
-    flyingStateSubscriber = nodeHandle.subscribe("PPSClient/flyingState", 1, &MainWindow::flyingStateChangedCallback, this);
-
-    batteryStateSubscriber = nodeHandle.subscribe("PPSClient/batteryState", 1, &MainWindow::batteryStateChangedCallback, this);
-
-    controllerUsedSubscriber = nodeHandle.subscribe("PPSClient/controllerUsed", 1, &MainWindow::controllerUsedChangedCallback, this);
-
-
-    safeSetpointSubscriber = nodeHandle.subscribe("SafeControllerService/Setpoint", 1, &MainWindow::safeSetpointCallback, this);
-    DBChangedSubscriber = nodeHandle.subscribe("/my_GUI/DBChanged", 1, &MainWindow::DBChangedCallback, this);
-
-    ros::NodeHandle my_nodeHandle("~");
-    controllerSetpointPublisher = my_nodeHandle.advertise<Setpoint>("ControllerSetpoint", 1);
-
-
-    // communication with PPS Client, just to make it possible to communicate through terminal also we use PPSClient's name
-    //ros::NodeHandle nh_PPSClient(m_ros_namespace + "/PPSClient");
-    ros::NodeHandle nh_PPSClient("PPSClient");
-    crazyRadioCommandPublisher = nh_PPSClient.advertise<std_msgs::Int32>("crazyRadioCommand", 1);
-    PPSClientCommandPublisher = nh_PPSClient.advertise<std_msgs::Int32>("Command", 1);
-
-    PPSClientStudentCustomButtonPublisher = nh_PPSClient.advertise<CustomButton>("StudentCustomButton", 1);
-
-
-    // > For publishing a message that requests the
-    //   YAML parameters to be re-loaded from file
-    // > The message contents specify which controller
-    //   the parameters should be re-loaded for
-    requestLoadControllerYamlPublisher = nh_PPSClient.advertise<std_msgs::Int32>("requestLoadControllerYaml", 1);
-
-
-    // Subscriber for locking the load the controller YAML
-    // parameters when the Coordintor GUI requests a load
-    requestLoadControllerYaml_from_my_GUI_Subscriber = nodeHandle.subscribe("/my_GUI/requestLoadControllerYaml", 1, &MainWindow::requestLoadControllerYaml_from_my_GUI_Callback, this);
-
-    // First get student ID
-    if(!nh_PPSClient.getParam("studentID", m_student_id))
-    {
-		ROS_ERROR("Failed to get studentID");
-	}
-
-    // Then, Central manager
-    centralManager = nodeHandle.serviceClient<CMQuery>("/CentralManagerService/Query", false);
-    loadCrazyflieContext();
-
-
-
-
-    // Load default setpoint from the "SafeController" namespace of the "ParameterService"
-    std::vector<float> default_setpoint(4);
-    ros::NodeHandle nodeHandle_to_own_agent_parameter_service("ParameterService");
-    ros::NodeHandle nodeHandle_for_safeController(nodeHandle_to_own_agent_parameter_service, "SafeController");
-
-    if(!nodeHandle_for_safeController.getParam("defaultSetpoint", default_setpoint))
-    {
-        ROS_ERROR_STREAM("The StudentGUI could not find parameter 'defaultSetpoint', as called from main(...)");
-    }
-
-    // Copy the default setpoint into respective text fields of the GUI
-    ui->current_setpoint_x->setText(QString::number(default_setpoint[0]));
-    ui->current_setpoint_y->setText(QString::number(default_setpoint[1]));
-    ui->current_setpoint_z->setText(QString::number(default_setpoint[2]));
-    ui->current_setpoint_yaw->setText(QString::number(default_setpoint[3]));
-
-
-    disableGUI();
-    highlightSafeControllerTab();
-    ui->label_battery->setStyleSheet("QLabel { color : red; }");
-    m_battery_state = BATTERY_STATE_NORMAL;
-
-    ui->error_label->setStyleSheet("QLabel { color : red; }");
-    ui->error_label->clear();
-
-    initialize_custom_setpoint();
-}
-
-
-MainWindow::~MainWindow()
-{
-    delete ui;
-}
-
-void MainWindow::disableGUI()
-{
-    ui->motors_OFF_button->setEnabled(false);
-    ui->take_off_button->setEnabled(false);
-    ui->land_button->setEnabled(false);
-}
-
-void MainWindow::enableGUI()
-{
-    ui->motors_OFF_button->setEnabled(true);
-    if(m_battery_state == BATTERY_STATE_NORMAL)
-    {
-        ui->take_off_button->setEnabled(true);
-        ui->land_button->setEnabled(true);
-    }
-}
-
-void MainWindow::highlightSafeControllerTab()
-{
-    ui->tabWidget->tabBar()->setTabTextColor(0, Qt::green);
-    ui->tabWidget->tabBar()->setTabTextColor(1, Qt::black);
-}
-void MainWindow::highlightCustomControllerTab()
-{
-    ui->tabWidget->tabBar()->setTabTextColor(0, Qt::black);
-    ui->tabWidget->tabBar()->setTabTextColor(1, Qt::green);
-}
-
-void MainWindow::DBChangedCallback(const std_msgs::Int32& msg)
-{
-    loadCrazyflieContext();
-    ROS_INFO("context reloaded in student_GUI");
-}
-
-void MainWindow::controllerUsedChangedCallback(const std_msgs::Int32& msg)
-{
-    switch(msg.data)
-    {
-        case SAFE_CONTROLLER:
-            highlightSafeControllerTab();
-            break;
-        case CUSTOM_CONTROLLER:
-            highlightCustomControllerTab();
-            break;
-        default:
-            break;
-    }
-}
-
-void MainWindow::safeSetpointCallback(const Setpoint& newSetpoint)
-{
-    m_safe_setpoint = newSetpoint;
-    // here we get the new setpoint, need to update it in GUI
-    ui->current_setpoint_x->setText(QString::number(newSetpoint.x, 'f', 3));
-    ui->current_setpoint_y->setText(QString::number(newSetpoint.y, 'f', 3));
-    ui->current_setpoint_z->setText(QString::number(newSetpoint.z, 'f', 3));
-    ui->current_setpoint_yaw->setText(QString::number(newSetpoint.yaw * RAD2DEG, 'f', 1));
-}
-
-void MainWindow::customSetpointCallback(const Setpoint& newSetpoint)
-{
-    m_custom_setpoint = newSetpoint;
-    // here we get the new setpoint, need to update it in GUI
-    ui->current_setpoint_x_2->setText(QString::number(newSetpoint.x, 'f', 3));
-    ui->current_setpoint_y_2->setText(QString::number(newSetpoint.y, 'f', 3));
-    ui->current_setpoint_z_2->setText(QString::number(newSetpoint.z, 'f', 3));
-    ui->current_setpoint_yaw_2->setText(QString::number(newSetpoint.yaw * RAD2DEG, 'f', 1));
-}
-
-void MainWindow::flyingStateChangedCallback(const std_msgs::Int32& msg)
-{
-    QString qstr = "Flying State: ";
-    switch(msg.data)
-    {
-        case STATE_MOTORS_OFF:
-            qstr.append("Motors OFF");
-            break;
-        case STATE_TAKE_OFF:
-            qstr.append("Take OFF");
-            break;
-        case STATE_FLYING:
-            qstr.append("Flying");
-            break;
-        case STATE_LAND:
-            qstr.append("Land");
-            break;
-        default:
-            break;
-    }
-    ui->flying_state_label->setText(qstr);
-}
-
-void MainWindow::batteryStateChangedCallback(const std_msgs::Int32& msg)
-{
-    // switch case with unabling buttons motors off, take off, etc... when battery is low
-    QString qstr = "";
-    switch(msg.data)
-    {
-        case BATTERY_STATE_LOW:
-            qstr.append("Low Battery!");
-            ui->take_off_button->setEnabled(false);
-            ui->land_button->setEnabled(false);
-            // ui->groupBox_4->setEnabled(false);
-
-            ui->label_battery->setText(qstr);
-            m_battery_state = BATTERY_STATE_LOW;
-            break;
-        case BATTERY_STATE_NORMAL:
-            // ui->groupBox_4->setEnabled(true);
-            ui->take_off_button->setEnabled(true);
-            ui->land_button->setEnabled(true);
-
-            ui->label_battery->clear();
-            m_battery_state = BATTERY_STATE_NORMAL;
-            break;
-        default:
-            break;
-    }
-}
-
-
-void MainWindow::setCrazyRadioStatus(int radio_status)
-{
-    // add more things whenever the status is changed
-    switch(radio_status)
-    {
-        case CONNECTED:
-            // change icon, the rest of the GUI is available now
-            ui->connected_disconnected_label->setText("Crazyradio status: Connected!");
-            enableGUI();
-            break;
-        case CONNECTING:
-            // change icon
-            ui->connected_disconnected_label->setText("Crazyradio status: Connecting...");
-            break;
-        case DISCONNECTED:
-            // change icon, the rest of the GUI is disabled
-            ui->connected_disconnected_label->setText("Crazyradio status: Disconnected");
-            disableGUI();
-            break;
-        default:
-    		ROS_ERROR_STREAM("unexpected radio status: " << m_radio_status);
-            break;
-    }
-    this->m_radio_status = radio_status;
-}
-
-float MainWindow::fromVoltageToPercent(float voltage)
-{
-    int num_cutoffs = m_cutoff_voltages.size();
-    float hysteresis = 0.05;
-
-    while(m_battery_level < num_cutoffs && voltage >= m_cutoff_voltages[m_battery_level])
-    {
-        ++m_battery_level;
-    }
-    while(m_battery_level > 0 && voltage < m_cutoff_voltages[m_battery_level - 1] - hysteresis)
-    {
-        --m_battery_level;
-    }
-
-    float percentage = 100.0 * m_battery_level/num_cutoffs;
-
-    // should not hapen, but just in case...
-    if(percentage > 100)
-        percentage = 100;
-    if(percentage < 0)
-        percentage = 0;
-
-    return percentage;
-}
-
-void MainWindow::updateBatteryVoltage(float battery_voltage)
-{
-    m_battery_voltage = battery_voltage;
-    // Need to take voltage, display it and transform it to percentage
-    // int percentage = (int) fromVoltageToPercent(m_battery_voltage);
-
-    QString qstr = "";
-    qstr.append(QString::number(battery_voltage, 'f', 2));
-    qstr.append(" V");
-    ui->voltage_field->setText(qstr);
-}
-
-void MainWindow::CFBatteryCallback(const std_msgs::Float32& msg)
-{
-    updateBatteryVoltage(msg.data);
-}
-
-void MainWindow::crazyRadioStatusCallback(const std_msgs::Int32& msg)
-{
-    ROS_INFO("Callback CrazyRadioStatus called");
-    this->setCrazyRadioStatus(msg.data);
-}
-
-void MainWindow::loadCrazyflieContext()
-{
-	CMQuery contextCall;
-	contextCall.request.studentID = m_student_id;
-	ROS_INFO_STREAM("StudentID:" << m_student_id);
-
-	centralManager.waitForExistence(ros::Duration(-1));
-
-	if(centralManager.call(contextCall))
-    {
-		m_context = contextCall.response.crazyflieContext;
-		ROS_INFO_STREAM("CrazyflieContext:\n" << m_context);
-        // we now have the m_context variable with the current context. Put CF Name in label
-
-        QString qstr = "StudentID ";
-        qstr.append(QString::number(m_student_id));
-        qstr.append(" connected to CF ");
-        qstr.append(QString::fromStdString(m_context.crazyflieName));
-        ui->groupBox->setTitle(qstr);
-	}
-    else
-    {
-		ROS_ERROR("Failed to load context");
-	}
-
-	ros::NodeHandle nh("CrazyRadio");
-	nh.setParam("crazyFlieAddress", m_context.crazyflieAddress);
-}
-
-void MainWindow::coordinatesToLocal(CrazyflieData& cf)
-{
-	AreaBounds area = m_context.localArea;
-	float originX = (area.xmin + area.xmax) / 2.0;
-	float originY = (area.ymin + area.ymax) / 2.0;
-    // change Z origin to zero, i.e., to the table height, zero of global coordinates, instead of middle of the box
-    float originZ = 0.0;
-	// float originZ = (area.zmin + area.zmax) / 2.0;
-
-	cf.x -= originX;
-	cf.y -= originY;
-	cf.z -= originZ;
-}
-
-
-void MainWindow::updateNewViconData(const ptrToMessage& p_msg) //connected to newViconData, from node
-{
-    for(std::vector<CrazyflieData>::const_iterator it = p_msg->crazyflies.begin(); it != p_msg->crazyflies.end(); ++it)
-    {
-		CrazyflieData global = *it;
-        if(global.crazyflieName == m_context.crazyflieName)
-        {
-            CrazyflieData local = global;
-            coordinatesToLocal(local);
-
-            // now we have the local coordinates, put them in the labels
-            ui->current_x->setText(QString::number(local.x, 'f', 3));
-            ui->current_y->setText(QString::number(local.y, 'f', 3));
-            ui->current_z->setText(QString::number(local.z, 'f', 3));
-            ui->current_yaw->setText(QString::number(local.yaw * RAD2DEG, 'f', 1));
-            ui->current_pitch->setText(QString::number(local.pitch * RAD2DEG, 'f', 1));
-            ui->current_roll->setText(QString::number(local.roll * RAD2DEG, 'f', 1));
-
-            ui->current_x_2->setText(QString::number(local.x, 'f', 3));
-            ui->current_y_2->setText(QString::number(local.y, 'f', 3));
-            ui->current_z_2->setText(QString::number(local.z, 'f', 3));
-            ui->current_yaw_2->setText(QString::number(local.yaw * RAD2DEG, 'f', 1));
-            ui->current_pitch_2->setText(QString::number(local.pitch * RAD2DEG, 'f', 1));
-            ui->current_roll_2->setText(QString::number(local.roll * RAD2DEG, 'f', 1));
-
-            // also update diff
-            ui->diff_x->setText(QString::number(m_safe_setpoint.x - local.x, 'f', 3));
-            ui->diff_y->setText(QString::number(m_safe_setpoint.y - local.y, 'f', 3));
-            ui->diff_z->setText(QString::number(m_safe_setpoint.z - local.z, 'f', 3));
-            ui->diff_yaw->setText(QString::number((m_safe_setpoint.yaw - local.yaw) * RAD2DEG, 'f', 1));
-
-            ui->diff_x_2->setText(QString::number(m_custom_setpoint.x - local.x, 'f', 3));
-            ui->diff_y_2->setText(QString::number(m_custom_setpoint.y - local.y, 'f', 3));
-            ui->diff_z_2->setText(QString::number(m_custom_setpoint.z - local.z, 'f', 3));
-            ui->diff_yaw_2->setText(QString::number((m_custom_setpoint.yaw - local.yaw) * RAD2DEG, 'f', 1));
-        }
-    }
-}
-
-void MainWindow::on_RF_Connect_button_clicked()
-{
-    std_msgs::Int32 msg;
-    msg.data = CMD_RECONNECT;
-    this->crazyRadioCommandPublisher.publish(msg);
-    ROS_INFO("command reconnect published");
-}
-
-void MainWindow::on_take_off_button_clicked()
-{
-    std_msgs::Int32 msg;
-    msg.data = CMD_CRAZYFLY_TAKE_OFF;
-    this->PPSClientCommandPublisher.publish(msg);
-}
-
-void MainWindow::on_land_button_clicked()
-{
-    std_msgs::Int32 msg;
-    msg.data = CMD_CRAZYFLY_LAND;
-    this->PPSClientCommandPublisher.publish(msg);
-}
-
-void MainWindow::on_motors_OFF_button_clicked()
-{
-    std_msgs::Int32 msg;
-    msg.data = CMD_CRAZYFLY_MOTORS_OFF;
-    this->PPSClientCommandPublisher.publish(msg);
-}
-
-void MainWindow::on_set_setpoint_button_clicked()
-{
-    Setpoint msg_setpoint;
-
-    // initialize setpoint to previous one
-
-    msg_setpoint.x = (ui->current_setpoint_x->text()).toFloat();
-    msg_setpoint.y = (ui->current_setpoint_y->text()).toFloat();
-    msg_setpoint.z = (ui->current_setpoint_z->text()).toFloat();
-    msg_setpoint.yaw = (ui->current_setpoint_yaw->text()).toFloat();
-
-    if(!ui->new_setpoint_x->text().isEmpty())
-        msg_setpoint.x = (ui->new_setpoint_x->text()).toFloat();
-
-    if(!ui->new_setpoint_y->text().isEmpty())
-        msg_setpoint.y = (ui->new_setpoint_y->text()).toFloat();
-
-    if(!ui->new_setpoint_z->text().isEmpty())
-        msg_setpoint.z = (ui->new_setpoint_z->text()).toFloat();
-
-    if(!ui->new_setpoint_yaw->text().isEmpty())
-        msg_setpoint.yaw = (ui->new_setpoint_yaw->text()).toFloat() * DEG2RAD;
-
-
-    if(!setpointInsideBox(msg_setpoint, m_context))
-    {
-        ROS_INFO("Corrected setpoint, was out of bounds");
-
-        // correct the setpoint given the box size
-        msg_setpoint = correctSetpointBox(msg_setpoint, m_context);
-        ui->error_label->setText("Setpoint is outside safety box");
-    }
-    else
-    {
-        ui->error_label->clear();
-    }
-
-    this->controllerSetpointPublisher.publish(msg_setpoint);
-
-    ROS_INFO_STREAM("Setpoint change clicked with:" << msg_setpoint.x << ", "<< msg_setpoint.y << ", "<< msg_setpoint.z << ", "<< msg_setpoint.yaw);
-}
-
-void MainWindow::initialize_custom_setpoint()
-{
-    Setpoint msg_setpoint;
-    msg_setpoint.x = 0;
-    msg_setpoint.y = 0;
-    msg_setpoint.z = 0.4;
-    msg_setpoint.yaw = 0;
-
-    this->customSetpointPublisher.publish(msg_setpoint);
-}
-
-void MainWindow::on_set_setpoint_button_2_clicked()
-{
-    Setpoint msg_setpoint;
-
-    msg_setpoint.x = (ui->current_setpoint_x_2->text()).toFloat();
-    msg_setpoint.y = (ui->current_setpoint_y_2->text()).toFloat();
-    msg_setpoint.z = (ui->current_setpoint_z_2->text()).toFloat();
-    msg_setpoint.yaw = (ui->current_setpoint_yaw_2->text()).toFloat();
-
-    if(!ui->new_setpoint_x_2->text().isEmpty())
-        msg_setpoint.x = (ui->new_setpoint_x_2->text()).toFloat();
-    if(!ui->new_setpoint_y_2->text().isEmpty())
-        msg_setpoint.y = (ui->new_setpoint_y_2->text()).toFloat();
-    if(!ui->new_setpoint_z_2->text().isEmpty())
-        msg_setpoint.z = (ui->new_setpoint_z_2->text()).toFloat();
-    if(!ui->new_setpoint_yaw_2->text().isEmpty())
-        msg_setpoint.yaw = (ui->new_setpoint_yaw_2->text()).toFloat() * DEG2RAD;
-
-    this->customSetpointPublisher.publish(msg_setpoint);
-
-    ROS_INFO_STREAM("Setpoint change clicked with:" << msg_setpoint.x << ", "<< msg_setpoint.y << ", "<< msg_setpoint.z << ", "<< msg_setpoint.yaw);
-}
-
-void MainWindow::on_RF_disconnect_button_clicked()
-{
-    std_msgs::Int32 msg;
-    msg.data = CMD_DISCONNECT;
-    this->crazyRadioCommandPublisher.publish(msg);
-    ROS_INFO("command disconnect published");
-}
-
-
-
-
-void MainWindow::on_load_safe_yaml_button_clicked()
-{
-    // Set the "load safe yaml" button to be disabled
-    ui->load_safe_yaml_button->setEnabled(false);
-
-    // Send a message requesting the parameters from the YAML
-    // file to be reloaded for the safe controller
-    std_msgs::Int32 msg;
-    msg.data = LOAD_YAML_SAFE_CONTROLLER_AGENT;
-    this->requestLoadControllerYamlPublisher.publish(msg);
-    ROS_INFO("Request load of safe controller YAML published");
-
-    // Start a timer which will enable the button in its callback
-    // > This is required because the agent node waits some time between
-    //   re-loading the values from the YAML file and then assigning then
-    //   to the local variable of the agent.
-    // > Thus we use this timer to prevent the user from clicking the
-    //   button in the GUI repeatedly.
-    ros::NodeHandle nodeHandle("~");
-    m_timer_yaml_file_for_safe_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::safeYamlFileTimerCallback, this, true);
-}
-
-void MainWindow::safeYamlFileTimerCallback(const ros::TimerEvent&)
-{
-    // Enble the "load safe yaml" button again
-    ui->load_safe_yaml_button->setEnabled(true);
-}
-
-
-
-
-
-void MainWindow::on_load_custom_yaml_button_clicked()
-{
-    // Set the "load custom yaml" button to be disabled
-    ui->load_custom_yaml_button->setEnabled(false);
-
-    // Send a message requesting the parameters from the YAML
-    // file to be reloaded for the custom controller
-    std_msgs::Int32 msg;
-    msg.data = LOAD_YAML_CUSTOM_CONTROLLER_AGENT;
-    this->requestLoadControllerYamlPublisher.publish(msg);
-    ROS_INFO("Request load of custom controller YAML published");
-
-    // Start a timer which will enable the button in its callback
-    // > This is required because the agent node waits some time between
-    //   re-loading the values from the YAML file and then assigning then
-    //   to the local variable of the agent.
-    // > Thus we use this timer to prevent the user from clicking the
-    //   button in the GUI repeatedly.
-    ros::NodeHandle nodeHandle("~");
-    m_timer_yaml_file_for_custom_controlller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::customYamlFileTimerCallback, this, true);    
-}
-
-void MainWindow::customYamlFileTimerCallback(const ros::TimerEvent&)
-{
-    // Enble the "load custom yaml" button again
-    ui->load_custom_yaml_button->setEnabled(true);
-}
-
-
-
-
-
-void MainWindow::requestLoadControllerYaml_from_my_GUI_Callback(const std_msgs::Int32& msg)
-{
-    // Extract from the "msg" for which controller the YAML
-    // parameters should be loaded
-    int controller_to_load_yaml = msg.data;
-
-    // Create the "nodeHandle" needed in the switch cases below
-    ros::NodeHandle nodeHandle("~");
-
-    // Switch between loading for the different controllers
-    switch(controller_to_load_yaml)
-    {
-        case LOAD_YAML_SAFE_CONTROLLER_AGENT:
-        case LOAD_YAML_SAFE_CONTROLLER_COORDINATOR:
-            // Set the "load safe yaml" button to be disabled
-            ui->load_safe_yaml_button->setEnabled(false);
-
-            // Start a timer which will enable the button in its callback
-            // > This is required because the agent node waits some time between
-            //   re-loading the values from the YAML file and then assigning then
-            //   to the local variable of the agent.
-            // > Thus we use this timer to prevent the user from clicking the
-            //   button in the GUI repeatedly.
-            m_timer_yaml_file_for_safe_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::safeYamlFileTimerCallback, this, true);
-
-            break;
-
-        case LOAD_YAML_CUSTOM_CONTROLLER_AGENT:
-        case LOAD_YAML_CUSTOM_CONTROLLER_COORDINATOR:
-            // Set the "load custom yaml" button to be disabled
-            ui->load_custom_yaml_button->setEnabled(false);
-
-            // Start a timer which will enable the button in its callback
-            // > This is required because the agent node waits some time between
-            //   re-loading the values from the YAML file and then assigning then
-            //   to the local variable of the agent.
-            // > Thus we use this timer to prevent the user from clicking the
-            //   button in the GUI repeatedly.
-            m_timer_yaml_file_for_custom_controlller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::customYamlFileTimerCallback, this, true);    
-
-            break;
-
-        default:
-            ROS_INFO("Unknown 'all controllers to load yaml' command, thus nothing will be disabled");
-            break;
-    }
-}
-
-
-
-
-void MainWindow::on_en_custom_controller_clicked()
-{
-    std_msgs::Int32 msg;
-    msg.data = CMD_USE_CUSTOM_CONTROLLER;
-    this->PPSClientCommandPublisher.publish(msg);
-}
-
-
-void MainWindow::on_en_safe_controller_clicked()
-{
-    std_msgs::Int32 msg;
-    msg.data = CMD_USE_SAFE_CONTROLLER;
-    this->PPSClientCommandPublisher.publish(msg);
-}
-
-void MainWindow::on_customButton_1_clicked()
-{
-    CustomButton msg_custom_button;
-    msg_custom_button.button_index = 1;
-    msg_custom_button.command_code = 0;
-    this->PPSClientStudentCustomButtonPublisher.publish(msg_custom_button);
-
-    ROS_INFO("Custom button 1 pressed");
-}
-
-void MainWindow::on_customButton_2_clicked()
-{
-    CustomButton msg_custom_button;
-    msg_custom_button.button_index = 2;
-    msg_custom_button.command_code = 0;
-    this->PPSClientStudentCustomButtonPublisher.publish(msg_custom_button);
-    ROS_INFO("Custom button 2 pressed");
-}
-
-void MainWindow::on_customButton_3_clicked()
-{
-    CustomButton msg_custom_button;
-    msg_custom_button.button_index = 3;
-    msg_custom_button.command_code = (ui->custom_command_3->text()).toFloat();
-    this->PPSClientStudentCustomButtonPublisher.publish(msg_custom_button);
-    ROS_INFO("Custom button 3 pressed");
-}
-
-Setpoint MainWindow::correctSetpointBox(Setpoint setpoint, CrazyflieContext context)
-{
-    Setpoint corrected_setpoint;
-    corrected_setpoint =  setpoint;
-
-    float x_size = context.localArea.xmax - context.localArea.xmin;
-    float y_size = context.localArea.ymax - context.localArea.ymin;
-    float z_size = context.localArea.zmax - context.localArea.zmin;
-
-    if(setpoint.x > x_size/2)
-        corrected_setpoint.x = x_size/2;
-    if(setpoint.y > y_size/2)
-        corrected_setpoint.y = y_size/2;
-    if(setpoint.z > z_size)
-        corrected_setpoint.z = z_size;
-
-    if(setpoint.x < -x_size/2)
-        corrected_setpoint.x = -x_size/2;
-    if(setpoint.y < -y_size/2)
-        corrected_setpoint.y = -y_size/2;
-    if(setpoint.z < 0)
-        corrected_setpoint.z = 0;
-
-    return corrected_setpoint;
-}
-
-bool MainWindow::setpointInsideBox(Setpoint setpoint, CrazyflieContext context)
-{
-
-    float x_size = context.localArea.xmax - context.localArea.xmin;
-    float y_size = context.localArea.ymax - context.localArea.ymin;
-    float z_size = context.localArea.zmax - context.localArea.zmin;
-    //position check
-	if((setpoint.x < -x_size/2) or (setpoint.x > x_size/2)) {
-		ROS_INFO_STREAM("x outside safety box");
-		return false;
-	}
-	if((setpoint.y < -y_size/2) or (setpoint.y > y_size/2)) {
-		ROS_INFO_STREAM("y outside safety box");
-		return false;
-	}
-	if((setpoint.z < 0) or (setpoint.z > z_size)) {
-		ROS_INFO_STREAM("z outside safety box");
-		return false;
-	}
-
-	return true;
-}
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/coordinatorGUI/src/MainWindow.ui b/pps_ws/src/d_fall_pps/GUI_Qt/coordinatorGUI/src/MainWindow.ui
deleted file mode 100644
index 64e3f3428eea12575294400a5107d7b4abc76162..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/GUI_Qt/coordinatorGUI/src/MainWindow.ui
+++ /dev/null
@@ -1,2064 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<ui version="4.0">
- <class>MainWindow</class>
- <widget class="QMainWindow" name="MainWindow">
-  <property name="geometry">
-   <rect>
-    <x>0</x>
-    <y>0</y>
-    <width>1092</width>
-    <height>701</height>
-   </rect>
-  </property>
-  <property name="sizePolicy">
-   <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
-    <horstretch>0</horstretch>
-    <verstretch>0</verstretch>
-   </sizepolicy>
-  </property>
-  <property name="maximumSize">
-   <size>
-    <width>1500</width>
-    <height>1000</height>
-   </size>
-  </property>
-  <property name="windowTitle">
-   <string>MainWindow</string>
-  </property>
-  <widget class="QWidget" name="centralWidget">
-   <layout class="QGridLayout" name="gridLayout">
-    <item row="1" column="1">
-     <layout class="QVBoxLayout" name="verticalLayout_4">
-      <property name="leftMargin">
-       <number>6</number>
-      </property>
-      <property name="topMargin">
-       <number>6</number>
-      </property>
-      <property name="rightMargin">
-       <number>6</number>
-      </property>
-      <property name="bottomMargin">
-       <number>6</number>
-      </property>
-      <item>
-       <widget class="QGroupBox" name="groupBox">
-        <property name="sizePolicy">
-         <sizepolicy hsizetype="MinimumExpanding" vsizetype="Preferred">
-          <horstretch>0</horstretch>
-          <verstretch>0</verstretch>
-         </sizepolicy>
-        </property>
-        <property name="font">
-         <font>
-          <pointsize>18</pointsize>
-         </font>
-        </property>
-        <property name="title">
-         <string>StudentID # connected to CF #</string>
-        </property>
-        <property name="alignment">
-         <set>Qt::AlignCenter</set>
-        </property>
-        <layout class="QGridLayout" name="gridLayout_2"/>
-       </widget>
-      </item>
-      <item>
-       <widget class="Line" name="line_9">
-        <property name="orientation">
-         <enum>Qt::Horizontal</enum>
-        </property>
-       </widget>
-      </item>
-      <item>
-       <layout class="QVBoxLayout" name="verticalLayout">
-        <property name="leftMargin">
-         <number>6</number>
-        </property>
-        <property name="topMargin">
-         <number>6</number>
-        </property>
-        <property name="rightMargin">
-         <number>6</number>
-        </property>
-        <property name="bottomMargin">
-         <number>6</number>
-        </property>
-        <item>
-         <layout class="QHBoxLayout" name="horizontalLayout_2">
-          <property name="leftMargin">
-           <number>6</number>
-          </property>
-          <property name="topMargin">
-           <number>6</number>
-          </property>
-          <property name="rightMargin">
-           <number>6</number>
-          </property>
-          <property name="bottomMargin">
-           <number>6</number>
-          </property>
-          <item>
-           <widget class="QLabel" name="connected_disconnected_label">
-            <property name="font">
-             <font>
-              <pointsize>16</pointsize>
-              <italic>true</italic>
-             </font>
-            </property>
-            <property name="text">
-             <string>Crazyradio status:</string>
-            </property>
-           </widget>
-          </item>
-          <item>
-           <widget class="QLabel" name="raw_voltage_label">
-            <property name="sizePolicy">
-             <sizepolicy hsizetype="Preferred" vsizetype="Preferred">
-              <horstretch>0</horstretch>
-              <verstretch>0</verstretch>
-             </sizepolicy>
-            </property>
-            <property name="font">
-             <font>
-              <pointsize>16</pointsize>
-              <italic>false</italic>
-             </font>
-            </property>
-            <property name="text">
-             <string>Voltage: </string>
-            </property>
-           </widget>
-          </item>
-          <item>
-           <widget class="QLabel" name="flying_state_label">
-            <property name="font">
-             <font>
-              <pointsize>16</pointsize>
-             </font>
-            </property>
-            <property name="text">
-             <string>FlyingState</string>
-            </property>
-            <property name="alignment">
-             <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
-            </property>
-           </widget>
-          </item>
-         </layout>
-        </item>
-        <item>
-         <layout class="QHBoxLayout" name="horizontalLayout">
-          <property name="leftMargin">
-           <number>6</number>
-          </property>
-          <property name="topMargin">
-           <number>6</number>
-          </property>
-          <property name="rightMargin">
-           <number>6</number>
-          </property>
-          <property name="bottomMargin">
-           <number>6</number>
-          </property>
-          <item>
-           <widget class="QPushButton" name="RF_disconnect_button">
-            <property name="sizePolicy">
-             <sizepolicy hsizetype="Minimum" vsizetype="MinimumExpanding">
-              <horstretch>0</horstretch>
-              <verstretch>0</verstretch>
-             </sizepolicy>
-            </property>
-            <property name="maximumSize">
-             <size>
-              <width>750</width>
-              <height>100</height>
-             </size>
-            </property>
-            <property name="font">
-             <font>
-              <pointsize>16</pointsize>
-             </font>
-            </property>
-            <property name="text">
-             <string>Disconnect</string>
-            </property>
-           </widget>
-          </item>
-          <item>
-           <widget class="QPushButton" name="RF_Connect_button">
-            <property name="sizePolicy">
-             <sizepolicy hsizetype="Minimum" vsizetype="MinimumExpanding">
-              <horstretch>0</horstretch>
-              <verstretch>0</verstretch>
-             </sizepolicy>
-            </property>
-            <property name="maximumSize">
-             <size>
-              <width>750</width>
-              <height>100</height>
-             </size>
-            </property>
-            <property name="font">
-             <font>
-              <pointsize>16</pointsize>
-             </font>
-            </property>
-            <property name="text">
-             <string>Connect</string>
-            </property>
-           </widget>
-          </item>
-          <item>
-           <layout class="QVBoxLayout" name="verticalLayout_8">
-            <item>
-             <widget class="QLineEdit" name="voltage_field">
-              <property name="sizePolicy">
-               <sizepolicy hsizetype="Minimum" vsizetype="MinimumExpanding">
-                <horstretch>0</horstretch>
-                <verstretch>0</verstretch>
-               </sizepolicy>
-              </property>
-              <property name="maximumSize">
-               <size>
-                <width>16777215</width>
-                <height>50</height>
-               </size>
-              </property>
-              <property name="font">
-               <font>
-                <pointsize>16</pointsize>
-               </font>
-              </property>
-              <property name="readOnly">
-               <bool>true</bool>
-              </property>
-             </widget>
-            </item>
-            <item>
-             <widget class="QLabel" name="label_battery">
-              <property name="sizePolicy">
-               <sizepolicy hsizetype="Minimum" vsizetype="MinimumExpanding">
-                <horstretch>0</horstretch>
-                <verstretch>0</verstretch>
-               </sizepolicy>
-              </property>
-              <property name="maximumSize">
-               <size>
-                <width>16777215</width>
-                <height>50</height>
-               </size>
-              </property>
-              <property name="font">
-               <font>
-                <pointsize>16</pointsize>
-                <italic>true</italic>
-               </font>
-              </property>
-              <property name="text">
-               <string/>
-              </property>
-             </widget>
-            </item>
-           </layout>
-          </item>
-          <item>
-           <widget class="QPushButton" name="take_off_button">
-            <property name="sizePolicy">
-             <sizepolicy hsizetype="Minimum" vsizetype="MinimumExpanding">
-              <horstretch>0</horstretch>
-              <verstretch>0</verstretch>
-             </sizepolicy>
-            </property>
-            <property name="maximumSize">
-             <size>
-              <width>16777215</width>
-              <height>100</height>
-             </size>
-            </property>
-            <property name="font">
-             <font>
-              <pointsize>16</pointsize>
-             </font>
-            </property>
-            <property name="text">
-             <string>Take Off</string>
-            </property>
-           </widget>
-          </item>
-          <item>
-           <widget class="QPushButton" name="land_button">
-            <property name="sizePolicy">
-             <sizepolicy hsizetype="Minimum" vsizetype="MinimumExpanding">
-              <horstretch>0</horstretch>
-              <verstretch>0</verstretch>
-             </sizepolicy>
-            </property>
-            <property name="maximumSize">
-             <size>
-              <width>16777215</width>
-              <height>100</height>
-             </size>
-            </property>
-            <property name="font">
-             <font>
-              <pointsize>16</pointsize>
-             </font>
-            </property>
-            <property name="text">
-             <string>Land</string>
-            </property>
-           </widget>
-          </item>
-          <item>
-           <widget class="QPushButton" name="motors_OFF_button">
-            <property name="sizePolicy">
-             <sizepolicy hsizetype="Minimum" vsizetype="MinimumExpanding">
-              <horstretch>0</horstretch>
-              <verstretch>0</verstretch>
-             </sizepolicy>
-            </property>
-            <property name="maximumSize">
-             <size>
-              <width>16777215</width>
-              <height>100</height>
-             </size>
-            </property>
-            <property name="font">
-             <font>
-              <pointsize>16</pointsize>
-              <weight>75</weight>
-              <bold>true</bold>
-             </font>
-            </property>
-            <property name="text">
-             <string>Motors OFF</string>
-            </property>
-           </widget>
-          </item>
-         </layout>
-        </item>
-       </layout>
-      </item>
-      <item>
-       <widget class="Line" name="line_8">
-        <property name="orientation">
-         <enum>Qt::Horizontal</enum>
-        </property>
-       </widget>
-      </item>
-      <item>
-       <layout class="QHBoxLayout" name="horizontalLayout_8">
-        <property name="leftMargin">
-         <number>6</number>
-        </property>
-        <property name="topMargin">
-         <number>6</number>
-        </property>
-        <property name="rightMargin">
-         <number>6</number>
-        </property>
-        <property name="bottomMargin">
-         <number>6</number>
-        </property>
-        <item>
-         <widget class="QTabWidget" name="tabWidget">
-          <property name="font">
-           <font>
-            <pointsize>16</pointsize>
-           </font>
-          </property>
-          <property name="currentIndex">
-           <number>1</number>
-          </property>
-          <property name="usesScrollButtons">
-           <bool>true</bool>
-          </property>
-          <property name="tabsClosable">
-           <bool>false</bool>
-          </property>
-          <widget class="QWidget" name="tab_3">
-           <property name="font">
-            <font>
-             <pointsize>14</pointsize>
-            </font>
-           </property>
-           <attribute name="title">
-            <string>   Safe   </string>
-           </attribute>
-           <layout class="QGridLayout" name="gridLayout_5">
-            <item row="0" column="0">
-             <widget class="QGroupBox" name="groupBox_2">
-              <property name="title">
-               <string/>
-              </property>
-              <layout class="QGridLayout" name="gridLayout_3">
-               <item row="6" column="1">
-                <widget class="QLineEdit" name="current_roll">
-                 <property name="sizePolicy">
-                  <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                   <horstretch>0</horstretch>
-                   <verstretch>0</verstretch>
-                  </sizepolicy>
-                 </property>
-                 <property name="font">
-                  <font>
-                   <pointsize>14</pointsize>
-                  </font>
-                 </property>
-                 <property name="readOnly">
-                  <bool>true</bool>
-                 </property>
-                </widget>
-               </item>
-               <item row="2" column="1">
-                <widget class="QLineEdit" name="current_y">
-                 <property name="sizePolicy">
-                  <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                   <horstretch>0</horstretch>
-                   <verstretch>0</verstretch>
-                  </sizepolicy>
-                 </property>
-                 <property name="font">
-                  <font>
-                   <pointsize>14</pointsize>
-                  </font>
-                 </property>
-                 <property name="readOnly">
-                  <bool>true</bool>
-                 </property>
-                </widget>
-               </item>
-               <item row="4" column="1">
-                <widget class="QLineEdit" name="current_yaw">
-                 <property name="sizePolicy">
-                  <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                   <horstretch>0</horstretch>
-                   <verstretch>0</verstretch>
-                  </sizepolicy>
-                 </property>
-                 <property name="font">
-                  <font>
-                   <pointsize>14</pointsize>
-                  </font>
-                 </property>
-                 <property name="readOnly">
-                  <bool>true</bool>
-                 </property>
-                </widget>
-               </item>
-               <item row="3" column="1">
-                <widget class="QLineEdit" name="current_z">
-                 <property name="sizePolicy">
-                  <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                   <horstretch>0</horstretch>
-                   <verstretch>0</verstretch>
-                  </sizepolicy>
-                 </property>
-                 <property name="font">
-                  <font>
-                   <pointsize>14</pointsize>
-                  </font>
-                 </property>
-                 <property name="readOnly">
-                  <bool>true</bool>
-                 </property>
-                </widget>
-               </item>
-               <item row="1" column="0">
-                <widget class="QLabel" name="current_x_label">
-                 <property name="sizePolicy">
-                  <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                   <horstretch>0</horstretch>
-                   <verstretch>0</verstretch>
-                  </sizepolicy>
-                 </property>
-                 <property name="font">
-                  <font>
-                   <pointsize>14</pointsize>
-                  </font>
-                 </property>
-                 <property name="text">
-                  <string>x [m] =</string>
-                 </property>
-                </widget>
-               </item>
-               <item row="3" column="0">
-                <widget class="QLabel" name="current_z_label">
-                 <property name="sizePolicy">
-                  <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                   <horstretch>0</horstretch>
-                   <verstretch>0</verstretch>
-                  </sizepolicy>
-                 </property>
-                 <property name="font">
-                  <font>
-                   <pointsize>14</pointsize>
-                  </font>
-                 </property>
-                 <property name="text">
-                  <string>z [m] =</string>
-                 </property>
-                </widget>
-               </item>
-               <item row="2" column="0">
-                <widget class="QLabel" name="current_y_label">
-                 <property name="sizePolicy">
-                  <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                   <horstretch>0</horstretch>
-                   <verstretch>0</verstretch>
-                  </sizepolicy>
-                 </property>
-                 <property name="font">
-                  <font>
-                   <pointsize>14</pointsize>
-                  </font>
-                 </property>
-                 <property name="text">
-                  <string>y [m] =</string>
-                 </property>
-                </widget>
-               </item>
-               <item row="4" column="0">
-                <widget class="QLabel" name="current_yaw_label">
-                 <property name="sizePolicy">
-                  <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                   <horstretch>0</horstretch>
-                   <verstretch>0</verstretch>
-                  </sizepolicy>
-                 </property>
-                 <property name="font">
-                  <font>
-                   <pointsize>14</pointsize>
-                  </font>
-                 </property>
-                 <property name="text">
-                  <string>yaw [deg] = </string>
-                 </property>
-                </widget>
-               </item>
-               <item row="1" column="1">
-                <widget class="QLineEdit" name="current_x">
-                 <property name="sizePolicy">
-                  <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                   <horstretch>0</horstretch>
-                   <verstretch>0</verstretch>
-                  </sizepolicy>
-                 </property>
-                 <property name="font">
-                  <font>
-                   <pointsize>14</pointsize>
-                  </font>
-                 </property>
-                 <property name="readOnly">
-                  <bool>true</bool>
-                 </property>
-                </widget>
-               </item>
-               <item row="5" column="0">
-                <widget class="QLabel" name="current_pitch_label">
-                 <property name="sizePolicy">
-                  <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                   <horstretch>0</horstretch>
-                   <verstretch>0</verstretch>
-                  </sizepolicy>
-                 </property>
-                 <property name="font">
-                  <font>
-                   <pointsize>14</pointsize>
-                  </font>
-                 </property>
-                 <property name="text">
-                  <string>pitch [deg] =</string>
-                 </property>
-                </widget>
-               </item>
-               <item row="6" column="0">
-                <widget class="QLabel" name="current_roll_label">
-                 <property name="sizePolicy">
-                  <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                   <horstretch>0</horstretch>
-                   <verstretch>0</verstretch>
-                  </sizepolicy>
-                 </property>
-                 <property name="font">
-                  <font>
-                   <pointsize>14</pointsize>
-                  </font>
-                 </property>
-                 <property name="text">
-                  <string>roll [deg] =</string>
-                 </property>
-                </widget>
-               </item>
-               <item row="5" column="1">
-                <widget class="QLineEdit" name="current_pitch">
-                 <property name="sizePolicy">
-                  <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                   <horstretch>0</horstretch>
-                   <verstretch>0</verstretch>
-                  </sizepolicy>
-                 </property>
-                 <property name="font">
-                  <font>
-                   <pointsize>14</pointsize>
-                  </font>
-                 </property>
-                 <property name="readOnly">
-                  <bool>true</bool>
-                 </property>
-                </widget>
-               </item>
-               <item row="0" column="1">
-                <widget class="QLabel" name="label_4">
-                 <property name="sizePolicy">
-                  <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                   <horstretch>0</horstretch>
-                   <verstretch>0</verstretch>
-                  </sizepolicy>
-                 </property>
-                 <property name="font">
-                  <font>
-                   <pointsize>14</pointsize>
-                  </font>
-                 </property>
-                 <property name="text">
-                  <string>Current</string>
-                 </property>
-                 <property name="alignment">
-                  <set>Qt::AlignCenter</set>
-                 </property>
-                </widget>
-               </item>
-               <item row="1" column="2">
-                <widget class="QLineEdit" name="diff_x">
-                 <property name="sizePolicy">
-                  <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                   <horstretch>0</horstretch>
-                   <verstretch>0</verstretch>
-                  </sizepolicy>
-                 </property>
-                 <property name="font">
-                  <font>
-                   <pointsize>14</pointsize>
-                  </font>
-                 </property>
-                 <property name="readOnly">
-                  <bool>true</bool>
-                 </property>
-                </widget>
-               </item>
-               <item row="2" column="2">
-                <widget class="QLineEdit" name="diff_y">
-                 <property name="sizePolicy">
-                  <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                   <horstretch>0</horstretch>
-                   <verstretch>0</verstretch>
-                  </sizepolicy>
-                 </property>
-                 <property name="font">
-                  <font>
-                   <pointsize>14</pointsize>
-                  </font>
-                 </property>
-                 <property name="readOnly">
-                  <bool>true</bool>
-                 </property>
-                </widget>
-               </item>
-               <item row="3" column="2">
-                <widget class="QLineEdit" name="diff_z">
-                 <property name="sizePolicy">
-                  <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                   <horstretch>0</horstretch>
-                   <verstretch>0</verstretch>
-                  </sizepolicy>
-                 </property>
-                 <property name="font">
-                  <font>
-                   <pointsize>14</pointsize>
-                  </font>
-                 </property>
-                 <property name="readOnly">
-                  <bool>true</bool>
-                 </property>
-                </widget>
-               </item>
-               <item row="4" column="2">
-                <widget class="QLineEdit" name="diff_yaw">
-                 <property name="sizePolicy">
-                  <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                   <horstretch>0</horstretch>
-                   <verstretch>0</verstretch>
-                  </sizepolicy>
-                 </property>
-                 <property name="font">
-                  <font>
-                   <pointsize>14</pointsize>
-                  </font>
-                 </property>
-                 <property name="readOnly">
-                  <bool>true</bool>
-                 </property>
-                </widget>
-               </item>
-               <item row="0" column="2">
-                <widget class="QLabel" name="label_5">
-                 <property name="sizePolicy">
-                  <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                   <horstretch>0</horstretch>
-                   <verstretch>0</verstretch>
-                  </sizepolicy>
-                 </property>
-                 <property name="font">
-                  <font>
-                   <pointsize>14</pointsize>
-                  </font>
-                 </property>
-                 <property name="text">
-                  <string>Difference</string>
-                 </property>
-                 <property name="alignment">
-                  <set>Qt::AlignCenter</set>
-                 </property>
-                </widget>
-               </item>
-              </layout>
-             </widget>
-            </item>
-            <item row="0" column="1">
-             <widget class="QGroupBox" name="groupBox_3">
-              <property name="title">
-               <string/>
-              </property>
-              <layout class="QGridLayout" name="gridLayout_4">
-               <item row="2" column="2">
-                <widget class="QLineEdit" name="new_setpoint_y">
-                 <property name="sizePolicy">
-                  <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                   <horstretch>0</horstretch>
-                   <verstretch>0</verstretch>
-                  </sizepolicy>
-                 </property>
-                 <property name="font">
-                  <font>
-                   <pointsize>14</pointsize>
-                  </font>
-                 </property>
-                </widget>
-               </item>
-               <item row="4" column="2">
-                <widget class="QLineEdit" name="new_setpoint_yaw">
-                 <property name="sizePolicy">
-                  <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                   <horstretch>0</horstretch>
-                   <verstretch>0</verstretch>
-                  </sizepolicy>
-                 </property>
-                 <property name="font">
-                  <font>
-                   <pointsize>14</pointsize>
-                  </font>
-                 </property>
-                </widget>
-               </item>
-               <item row="1" column="2">
-                <widget class="QLineEdit" name="new_setpoint_x">
-                 <property name="sizePolicy">
-                  <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                   <horstretch>0</horstretch>
-                   <verstretch>0</verstretch>
-                  </sizepolicy>
-                 </property>
-                 <property name="font">
-                  <font>
-                   <pointsize>14</pointsize>
-                  </font>
-                 </property>
-                </widget>
-               </item>
-               <item row="2" column="1">
-                <widget class="QLineEdit" name="current_setpoint_y">
-                 <property name="sizePolicy">
-                  <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                   <horstretch>0</horstretch>
-                   <verstretch>0</verstretch>
-                  </sizepolicy>
-                 </property>
-                 <property name="font">
-                  <font>
-                   <pointsize>14</pointsize>
-                  </font>
-                 </property>
-                 <property name="readOnly">
-                  <bool>true</bool>
-                 </property>
-                </widget>
-               </item>
-               <item row="1" column="1">
-                <widget class="QLineEdit" name="current_setpoint_x">
-                 <property name="sizePolicy">
-                  <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                   <horstretch>0</horstretch>
-                   <verstretch>0</verstretch>
-                  </sizepolicy>
-                 </property>
-                 <property name="font">
-                  <font>
-                   <pointsize>14</pointsize>
-                  </font>
-                 </property>
-                 <property name="readOnly">
-                  <bool>true</bool>
-                 </property>
-                </widget>
-               </item>
-               <item row="4" column="0">
-                <widget class="QLabel" name="label_11">
-                 <property name="sizePolicy">
-                  <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                   <horstretch>0</horstretch>
-                   <verstretch>0</verstretch>
-                  </sizepolicy>
-                 </property>
-                 <property name="font">
-                  <font>
-                   <pointsize>14</pointsize>
-                  </font>
-                 </property>
-                 <property name="text">
-                  <string>yaw [deg] =</string>
-                 </property>
-                </widget>
-               </item>
-               <item row="0" column="1">
-                <widget class="QLabel" name="label_12">
-                 <property name="sizePolicy">
-                  <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                   <horstretch>0</horstretch>
-                   <verstretch>0</verstretch>
-                  </sizepolicy>
-                 </property>
-                 <property name="font">
-                  <font>
-                   <pointsize>14</pointsize>
-                  </font>
-                 </property>
-                 <property name="text">
-                  <string>Current</string>
-                 </property>
-                 <property name="alignment">
-                  <set>Qt::AlignCenter</set>
-                 </property>
-                </widget>
-               </item>
-               <item row="1" column="0">
-                <widget class="QLabel" name="label_7">
-                 <property name="sizePolicy">
-                  <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                   <horstretch>0</horstretch>
-                   <verstretch>0</verstretch>
-                  </sizepolicy>
-                 </property>
-                 <property name="font">
-                  <font>
-                   <pointsize>14</pointsize>
-                  </font>
-                 </property>
-                 <property name="text">
-                  <string>x [m] =</string>
-                 </property>
-                </widget>
-               </item>
-               <item row="3" column="1">
-                <widget class="QLineEdit" name="current_setpoint_z">
-                 <property name="sizePolicy">
-                  <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                   <horstretch>0</horstretch>
-                   <verstretch>0</verstretch>
-                  </sizepolicy>
-                 </property>
-                 <property name="font">
-                  <font>
-                   <pointsize>14</pointsize>
-                  </font>
-                 </property>
-                 <property name="readOnly">
-                  <bool>true</bool>
-                 </property>
-                </widget>
-               </item>
-               <item row="3" column="0">
-                <widget class="QLabel" name="label_9">
-                 <property name="sizePolicy">
-                  <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                   <horstretch>0</horstretch>
-                   <verstretch>0</verstretch>
-                  </sizepolicy>
-                 </property>
-                 <property name="font">
-                  <font>
-                   <pointsize>14</pointsize>
-                  </font>
-                 </property>
-                 <property name="text">
-                  <string>z [m] =</string>
-                 </property>
-                </widget>
-               </item>
-               <item row="2" column="0">
-                <widget class="QLabel" name="label_8">
-                 <property name="sizePolicy">
-                  <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                   <horstretch>0</horstretch>
-                   <verstretch>0</verstretch>
-                  </sizepolicy>
-                 </property>
-                 <property name="font">
-                  <font>
-                   <pointsize>14</pointsize>
-                  </font>
-                 </property>
-                 <property name="text">
-                  <string>y [m] =</string>
-                 </property>
-                </widget>
-               </item>
-               <item row="4" column="1">
-                <widget class="QLineEdit" name="current_setpoint_yaw">
-                 <property name="sizePolicy">
-                  <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                   <horstretch>0</horstretch>
-                   <verstretch>0</verstretch>
-                  </sizepolicy>
-                 </property>
-                 <property name="font">
-                  <font>
-                   <pointsize>14</pointsize>
-                  </font>
-                 </property>
-                 <property name="readOnly">
-                  <bool>true</bool>
-                 </property>
-                </widget>
-               </item>
-               <item row="5" column="2">
-                <widget class="QPushButton" name="set_setpoint_button">
-                 <property name="sizePolicy">
-                  <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                   <horstretch>0</horstretch>
-                   <verstretch>0</verstretch>
-                  </sizepolicy>
-                 </property>
-                 <property name="font">
-                  <font>
-                   <pointsize>14</pointsize>
-                  </font>
-                 </property>
-                 <property name="text">
-                  <string>Set setpoint</string>
-                 </property>
-                </widget>
-               </item>
-               <item row="3" column="2">
-                <widget class="QLineEdit" name="new_setpoint_z">
-                 <property name="sizePolicy">
-                  <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                   <horstretch>0</horstretch>
-                   <verstretch>0</verstretch>
-                  </sizepolicy>
-                 </property>
-                 <property name="font">
-                  <font>
-                   <pointsize>14</pointsize>
-                  </font>
-                 </property>
-                </widget>
-               </item>
-               <item row="0" column="0">
-                <widget class="QLabel" name="label_3">
-                 <property name="sizePolicy">
-                  <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                   <horstretch>0</horstretch>
-                   <verstretch>0</verstretch>
-                  </sizepolicy>
-                 </property>
-                 <property name="font">
-                  <font>
-                   <pointsize>14</pointsize>
-                  </font>
-                 </property>
-                 <property name="text">
-                  <string>Setpoint:</string>
-                 </property>
-                </widget>
-               </item>
-               <item row="0" column="2">
-                <widget class="QLabel" name="label_13">
-                 <property name="sizePolicy">
-                  <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                   <horstretch>0</horstretch>
-                   <verstretch>0</verstretch>
-                  </sizepolicy>
-                 </property>
-                 <property name="font">
-                  <font>
-                   <pointsize>14</pointsize>
-                  </font>
-                 </property>
-                 <property name="text">
-                  <string>New</string>
-                 </property>
-                 <property name="alignment">
-                  <set>Qt::AlignCenter</set>
-                 </property>
-                </widget>
-               </item>
-               <item row="5" column="1">
-                <widget class="QLabel" name="error_label">
-                 <property name="text">
-                  <string/>
-                 </property>
-                </widget>
-               </item>
-              </layout>
-             </widget>
-            </item>
-           </layout>
-          </widget>
-          <widget class="QWidget" name="tab_4">
-           <attribute name="title">
-            <string>  Demo  </string>
-           </attribute>
-           <layout class="QGridLayout" name="gridLayout_9">
-            <item row="0" column="0">
-             <layout class="QVBoxLayout" name="verticalLayout_3">
-              <property name="leftMargin">
-               <number>6</number>
-              </property>
-              <property name="topMargin">
-               <number>6</number>
-              </property>
-              <property name="rightMargin">
-               <number>6</number>
-              </property>
-              <property name="bottomMargin">
-               <number>6</number>
-              </property>
-              <item>
-               <layout class="QHBoxLayout" name="horizontalLayout_3">
-                <property name="leftMargin">
-                 <number>6</number>
-                </property>
-                <property name="topMargin">
-                 <number>6</number>
-                </property>
-                <property name="rightMargin">
-                 <number>6</number>
-                </property>
-                <property name="bottomMargin">
-                 <number>6</number>
-                </property>
-                <item>
-                 <widget class="QGroupBox" name="groupBox_5">
-                  <property name="title">
-                   <string/>
-                  </property>
-                  <layout class="QGridLayout" name="gridLayout_7">
-                   <item row="1" column="2">
-                    <widget class="QLineEdit" name="diff_x_2">
-                     <property name="sizePolicy">
-                      <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                       <horstretch>0</horstretch>
-                       <verstretch>0</verstretch>
-                      </sizepolicy>
-                     </property>
-                     <property name="font">
-                      <font>
-                       <pointsize>14</pointsize>
-                      </font>
-                     </property>
-                     <property name="readOnly">
-                      <bool>true</bool>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="6" column="1">
-                    <widget class="QLineEdit" name="current_roll_2">
-                     <property name="sizePolicy">
-                      <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                       <horstretch>0</horstretch>
-                       <verstretch>0</verstretch>
-                      </sizepolicy>
-                     </property>
-                     <property name="font">
-                      <font>
-                       <pointsize>14</pointsize>
-                      </font>
-                     </property>
-                     <property name="readOnly">
-                      <bool>true</bool>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="2" column="1">
-                    <widget class="QLineEdit" name="current_y_2">
-                     <property name="sizePolicy">
-                      <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                       <horstretch>0</horstretch>
-                       <verstretch>0</verstretch>
-                      </sizepolicy>
-                     </property>
-                     <property name="font">
-                      <font>
-                       <pointsize>14</pointsize>
-                      </font>
-                     </property>
-                     <property name="readOnly">
-                      <bool>true</bool>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="4" column="1">
-                    <widget class="QLineEdit" name="current_yaw_2">
-                     <property name="sizePolicy">
-                      <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                       <horstretch>0</horstretch>
-                       <verstretch>0</verstretch>
-                      </sizepolicy>
-                     </property>
-                     <property name="font">
-                      <font>
-                       <pointsize>14</pointsize>
-                      </font>
-                     </property>
-                     <property name="readOnly">
-                      <bool>true</bool>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="3" column="1">
-                    <widget class="QLineEdit" name="current_z_2">
-                     <property name="sizePolicy">
-                      <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                       <horstretch>0</horstretch>
-                       <verstretch>0</verstretch>
-                      </sizepolicy>
-                     </property>
-                     <property name="font">
-                      <font>
-                       <pointsize>14</pointsize>
-                      </font>
-                     </property>
-                     <property name="readOnly">
-                      <bool>true</bool>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="1" column="0">
-                    <widget class="QLabel" name="current_x_label_2">
-                     <property name="sizePolicy">
-                      <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                       <horstretch>0</horstretch>
-                       <verstretch>0</verstretch>
-                      </sizepolicy>
-                     </property>
-                     <property name="font">
-                      <font>
-                       <pointsize>14</pointsize>
-                      </font>
-                     </property>
-                     <property name="text">
-                      <string>x [m] =</string>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="3" column="0">
-                    <widget class="QLabel" name="current_z_label_2">
-                     <property name="sizePolicy">
-                      <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                       <horstretch>0</horstretch>
-                       <verstretch>0</verstretch>
-                      </sizepolicy>
-                     </property>
-                     <property name="font">
-                      <font>
-                       <pointsize>14</pointsize>
-                      </font>
-                     </property>
-                     <property name="text">
-                      <string>z [m] =</string>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="1" column="1">
-                    <widget class="QLineEdit" name="current_x_2">
-                     <property name="sizePolicy">
-                      <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                       <horstretch>0</horstretch>
-                       <verstretch>0</verstretch>
-                      </sizepolicy>
-                     </property>
-                     <property name="font">
-                      <font>
-                       <pointsize>14</pointsize>
-                      </font>
-                     </property>
-                     <property name="readOnly">
-                      <bool>true</bool>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="2" column="0">
-                    <widget class="QLabel" name="current_y_label_2">
-                     <property name="font">
-                      <font>
-                       <pointsize>14</pointsize>
-                      </font>
-                     </property>
-                     <property name="text">
-                      <string>y [m] =</string>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="4" column="0">
-                    <widget class="QLabel" name="current_yaw_label_2">
-                     <property name="sizePolicy">
-                      <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                       <horstretch>0</horstretch>
-                       <verstretch>0</verstretch>
-                      </sizepolicy>
-                     </property>
-                     <property name="font">
-                      <font>
-                       <pointsize>14</pointsize>
-                      </font>
-                     </property>
-                     <property name="text">
-                      <string>yaw [deg] = </string>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="5" column="1">
-                    <widget class="QLineEdit" name="current_pitch_2">
-                     <property name="sizePolicy">
-                      <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                       <horstretch>0</horstretch>
-                       <verstretch>0</verstretch>
-                      </sizepolicy>
-                     </property>
-                     <property name="font">
-                      <font>
-                       <pointsize>14</pointsize>
-                      </font>
-                     </property>
-                     <property name="readOnly">
-                      <bool>true</bool>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="5" column="0">
-                    <widget class="QLabel" name="current_pitch_label_2">
-                     <property name="sizePolicy">
-                      <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                       <horstretch>0</horstretch>
-                       <verstretch>0</verstretch>
-                      </sizepolicy>
-                     </property>
-                     <property name="font">
-                      <font>
-                       <pointsize>14</pointsize>
-                      </font>
-                     </property>
-                     <property name="text">
-                      <string>pitch [deg] =</string>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="6" column="0">
-                    <widget class="QLabel" name="current_roll_label_2">
-                     <property name="sizePolicy">
-                      <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                       <horstretch>0</horstretch>
-                       <verstretch>0</verstretch>
-                      </sizepolicy>
-                     </property>
-                     <property name="font">
-                      <font>
-                       <pointsize>14</pointsize>
-                      </font>
-                     </property>
-                     <property name="text">
-                      <string>roll [deg] =</string>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="0" column="1">
-                    <widget class="QLabel" name="label_6">
-                     <property name="sizePolicy">
-                      <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                       <horstretch>0</horstretch>
-                       <verstretch>0</verstretch>
-                      </sizepolicy>
-                     </property>
-                     <property name="font">
-                      <font>
-                       <pointsize>14</pointsize>
-                      </font>
-                     </property>
-                     <property name="text">
-                      <string>Current</string>
-                     </property>
-                     <property name="alignment">
-                      <set>Qt::AlignCenter</set>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="3" column="2">
-                    <widget class="QLineEdit" name="diff_z_2">
-                     <property name="sizePolicy">
-                      <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                       <horstretch>0</horstretch>
-                       <verstretch>0</verstretch>
-                      </sizepolicy>
-                     </property>
-                     <property name="font">
-                      <font>
-                       <pointsize>14</pointsize>
-                      </font>
-                     </property>
-                     <property name="readOnly">
-                      <bool>true</bool>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="2" column="2">
-                    <widget class="QLineEdit" name="diff_y_2">
-                     <property name="sizePolicy">
-                      <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                       <horstretch>0</horstretch>
-                       <verstretch>0</verstretch>
-                      </sizepolicy>
-                     </property>
-                     <property name="font">
-                      <font>
-                       <pointsize>14</pointsize>
-                      </font>
-                     </property>
-                     <property name="readOnly">
-                      <bool>true</bool>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="4" column="2">
-                    <widget class="QLineEdit" name="diff_yaw_2">
-                     <property name="sizePolicy">
-                      <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                       <horstretch>0</horstretch>
-                       <verstretch>0</verstretch>
-                      </sizepolicy>
-                     </property>
-                     <property name="font">
-                      <font>
-                       <pointsize>14</pointsize>
-                      </font>
-                     </property>
-                     <property name="readOnly">
-                      <bool>true</bool>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="0" column="2">
-                    <widget class="QLabel" name="label_10">
-                     <property name="sizePolicy">
-                      <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                       <horstretch>0</horstretch>
-                       <verstretch>0</verstretch>
-                      </sizepolicy>
-                     </property>
-                     <property name="font">
-                      <font>
-                       <pointsize>14</pointsize>
-                      </font>
-                     </property>
-                     <property name="text">
-                      <string>Difference</string>
-                     </property>
-                     <property name="alignment">
-                      <set>Qt::AlignCenter</set>
-                     </property>
-                    </widget>
-                   </item>
-                  </layout>
-                 </widget>
-                </item>
-                <item>
-                 <widget class="QGroupBox" name="groupBox_6">
-                  <property name="title">
-                   <string/>
-                  </property>
-                  <layout class="QGridLayout" name="gridLayout_8">
-                   <item row="2" column="2">
-                    <widget class="QLineEdit" name="new_setpoint_y_2">
-                     <property name="sizePolicy">
-                      <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                       <horstretch>0</horstretch>
-                       <verstretch>0</verstretch>
-                      </sizepolicy>
-                     </property>
-                     <property name="font">
-                      <font>
-                       <pointsize>14</pointsize>
-                      </font>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="4" column="2">
-                    <widget class="QLineEdit" name="new_setpoint_yaw_2">
-                     <property name="sizePolicy">
-                      <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                       <horstretch>0</horstretch>
-                       <verstretch>0</verstretch>
-                      </sizepolicy>
-                     </property>
-                     <property name="font">
-                      <font>
-                       <pointsize>14</pointsize>
-                      </font>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="1" column="2">
-                    <widget class="QLineEdit" name="new_setpoint_x_2">
-                     <property name="sizePolicy">
-                      <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                       <horstretch>0</horstretch>
-                       <verstretch>0</verstretch>
-                      </sizepolicy>
-                     </property>
-                     <property name="font">
-                      <font>
-                       <pointsize>14</pointsize>
-                      </font>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="2" column="1">
-                    <widget class="QLineEdit" name="current_setpoint_y_2">
-                     <property name="sizePolicy">
-                      <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                       <horstretch>0</horstretch>
-                       <verstretch>0</verstretch>
-                      </sizepolicy>
-                     </property>
-                     <property name="font">
-                      <font>
-                       <pointsize>14</pointsize>
-                      </font>
-                     </property>
-                     <property name="readOnly">
-                      <bool>true</bool>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="1" column="1">
-                    <widget class="QLineEdit" name="current_setpoint_x_2">
-                     <property name="sizePolicy">
-                      <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                       <horstretch>0</horstretch>
-                       <verstretch>0</verstretch>
-                      </sizepolicy>
-                     </property>
-                     <property name="font">
-                      <font>
-                       <pointsize>14</pointsize>
-                      </font>
-                     </property>
-                     <property name="readOnly">
-                      <bool>true</bool>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="4" column="0">
-                    <widget class="QLabel" name="label_14">
-                     <property name="sizePolicy">
-                      <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                       <horstretch>0</horstretch>
-                       <verstretch>0</verstretch>
-                      </sizepolicy>
-                     </property>
-                     <property name="font">
-                      <font>
-                       <pointsize>14</pointsize>
-                      </font>
-                     </property>
-                     <property name="text">
-                      <string>yaw [deg] =</string>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="0" column="1">
-                    <widget class="QLabel" name="label_15">
-                     <property name="sizePolicy">
-                      <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                       <horstretch>0</horstretch>
-                       <verstretch>0</verstretch>
-                      </sizepolicy>
-                     </property>
-                     <property name="font">
-                      <font>
-                       <pointsize>14</pointsize>
-                      </font>
-                     </property>
-                     <property name="text">
-                      <string>Current</string>
-                     </property>
-                     <property name="alignment">
-                      <set>Qt::AlignCenter</set>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="1" column="0">
-                    <widget class="QLabel" name="label_16">
-                     <property name="sizePolicy">
-                      <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                       <horstretch>0</horstretch>
-                       <verstretch>0</verstretch>
-                      </sizepolicy>
-                     </property>
-                     <property name="font">
-                      <font>
-                       <pointsize>14</pointsize>
-                      </font>
-                     </property>
-                     <property name="text">
-                      <string>x [m] =</string>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="3" column="0">
-                    <widget class="QLabel" name="label_17">
-                     <property name="sizePolicy">
-                      <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                       <horstretch>0</horstretch>
-                       <verstretch>0</verstretch>
-                      </sizepolicy>
-                     </property>
-                     <property name="font">
-                      <font>
-                       <pointsize>14</pointsize>
-                      </font>
-                     </property>
-                     <property name="text">
-                      <string>z [m] =</string>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="2" column="0">
-                    <widget class="QLabel" name="label_18">
-                     <property name="sizePolicy">
-                      <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                       <horstretch>0</horstretch>
-                       <verstretch>0</verstretch>
-                      </sizepolicy>
-                     </property>
-                     <property name="font">
-                      <font>
-                       <pointsize>14</pointsize>
-                      </font>
-                     </property>
-                     <property name="text">
-                      <string>y [m] =</string>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="3" column="1">
-                    <widget class="QLineEdit" name="current_setpoint_z_2">
-                     <property name="sizePolicy">
-                      <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                       <horstretch>0</horstretch>
-                       <verstretch>0</verstretch>
-                      </sizepolicy>
-                     </property>
-                     <property name="font">
-                      <font>
-                       <pointsize>14</pointsize>
-                      </font>
-                     </property>
-                     <property name="readOnly">
-                      <bool>true</bool>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="5" column="2">
-                    <widget class="QPushButton" name="set_setpoint_button_2">
-                     <property name="sizePolicy">
-                      <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                       <horstretch>0</horstretch>
-                       <verstretch>0</verstretch>
-                      </sizepolicy>
-                     </property>
-                     <property name="font">
-                      <font>
-                       <pointsize>14</pointsize>
-                      </font>
-                     </property>
-                     <property name="text">
-                      <string>Set setpoint</string>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="4" column="1">
-                    <widget class="QLineEdit" name="current_setpoint_yaw_2">
-                     <property name="sizePolicy">
-                      <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                       <horstretch>0</horstretch>
-                       <verstretch>0</verstretch>
-                      </sizepolicy>
-                     </property>
-                     <property name="font">
-                      <font>
-                       <pointsize>14</pointsize>
-                      </font>
-                     </property>
-                     <property name="readOnly">
-                      <bool>true</bool>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="3" column="2">
-                    <widget class="QLineEdit" name="new_setpoint_z_2">
-                     <property name="sizePolicy">
-                      <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                       <horstretch>0</horstretch>
-                       <verstretch>0</verstretch>
-                      </sizepolicy>
-                     </property>
-                     <property name="font">
-                      <font>
-                       <pointsize>14</pointsize>
-                      </font>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="0" column="0">
-                    <widget class="QLabel" name="label_19">
-                     <property name="sizePolicy">
-                      <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                       <horstretch>0</horstretch>
-                       <verstretch>0</verstretch>
-                      </sizepolicy>
-                     </property>
-                     <property name="font">
-                      <font>
-                       <pointsize>14</pointsize>
-                      </font>
-                     </property>
-                     <property name="text">
-                      <string>Setpoint:</string>
-                     </property>
-                    </widget>
-                   </item>
-                   <item row="0" column="2">
-                    <widget class="QLabel" name="label_20">
-                     <property name="sizePolicy">
-                      <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-                       <horstretch>0</horstretch>
-                       <verstretch>0</verstretch>
-                      </sizepolicy>
-                     </property>
-                     <property name="font">
-                      <font>
-                       <pointsize>14</pointsize>
-                      </font>
-                     </property>
-                     <property name="text">
-                      <string>New</string>
-                     </property>
-                     <property name="alignment">
-                      <set>Qt::AlignCenter</set>
-                     </property>
-                    </widget>
-                   </item>
-                  </layout>
-                 </widget>
-                </item>
-               </layout>
-              </item>
-              <item>
-               <layout class="QHBoxLayout" name="horizontalLayout_5">
-                <property name="leftMargin">
-                 <number>6</number>
-                </property>
-                <property name="topMargin">
-                 <number>6</number>
-                </property>
-                <property name="rightMargin">
-                 <number>6</number>
-                </property>
-                <property name="bottomMargin">
-                 <number>6</number>
-                </property>
-                <item>
-                 <widget class="QPushButton" name="customButton_1">
-                  <property name="sizePolicy">
-                   <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
-                    <horstretch>0</horstretch>
-                    <verstretch>0</verstretch>
-                   </sizepolicy>
-                  </property>
-                  <property name="font">
-                   <font>
-                    <pointsize>14</pointsize>
-                   </font>
-                  </property>
-                  <property name="text">
-                   <string>Custom Command 1</string>
-                  </property>
-                 </widget>
-                </item>
-                <item>
-                 <widget class="QPushButton" name="customButton_2">
-                  <property name="sizePolicy">
-                   <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
-                    <horstretch>0</horstretch>
-                    <verstretch>0</verstretch>
-                   </sizepolicy>
-                  </property>
-                  <property name="font">
-                   <font>
-                    <pointsize>14</pointsize>
-                   </font>
-                  </property>
-                  <property name="text">
-                   <string>Custom Command 2</string>
-                  </property>
-                 </widget>
-                </item>
-                <item>
-                 <widget class="QPushButton" name="customButton_3">
-                  <property name="sizePolicy">
-                   <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
-                    <horstretch>0</horstretch>
-                    <verstretch>0</verstretch>
-                   </sizepolicy>
-                  </property>
-                  <property name="font">
-                   <font>
-                    <pointsize>14</pointsize>
-                   </font>
-                  </property>
-                  <property name="text">
-                   <string>Custom Command 3</string>
-                  </property>
-                 </widget>
-                </item>
-                <item>
-                 <widget class="QLineEdit" name="custom_command_3">
-                  <property name="font">
-                   <font>
-                    <pointsize>14</pointsize>
-                   </font>
-                  </property>
-                 </widget>
-                </item>
-               </layout>
-              </item>
-             </layout>
-            </item>
-           </layout>
-          </widget>
-         </widget>
-        </item>
-        <item>
-         <widget class="Line" name="line">
-          <property name="orientation">
-           <enum>Qt::Vertical</enum>
-          </property>
-         </widget>
-        </item>
-        <item>
-         <layout class="QGridLayout" name="gridLayout_17">
-          <property name="leftMargin">
-           <number>6</number>
-          </property>
-          <property name="topMargin">
-           <number>6</number>
-          </property>
-          <property name="rightMargin">
-           <number>6</number>
-          </property>
-          <property name="bottomMargin">
-           <number>6</number>
-          </property>
-          <item row="0" column="2">
-           <widget class="QLabel" name="label_2">
-            <property name="sizePolicy">
-             <sizepolicy hsizetype="Preferred" vsizetype="Minimum">
-              <horstretch>0</horstretch>
-              <verstretch>0</verstretch>
-             </sizepolicy>
-            </property>
-            <property name="font">
-             <font>
-              <pointsize>16</pointsize>
-             </font>
-            </property>
-            <property name="text">
-             <string>Load</string>
-            </property>
-            <property name="alignment">
-             <set>Qt::AlignCenter</set>
-            </property>
-           </widget>
-          </item>
-          <item row="0" column="0">
-           <widget class="QLabel" name="label_21">
-            <property name="sizePolicy">
-             <sizepolicy hsizetype="Preferred" vsizetype="Minimum">
-              <horstretch>0</horstretch>
-              <verstretch>0</verstretch>
-             </sizepolicy>
-            </property>
-            <property name="font">
-             <font>
-              <pointsize>16</pointsize>
-             </font>
-            </property>
-            <property name="text">
-             <string>Controller</string>
-            </property>
-            <property name="alignment">
-             <set>Qt::AlignCenter</set>
-            </property>
-           </widget>
-          </item>
-          <item row="1" column="0">
-           <widget class="QLabel" name="label">
-            <property name="sizePolicy">
-             <sizepolicy hsizetype="Preferred" vsizetype="Minimum">
-              <horstretch>0</horstretch>
-              <verstretch>0</verstretch>
-             </sizepolicy>
-            </property>
-            <property name="font">
-             <font>
-              <pointsize>16</pointsize>
-              <weight>75</weight>
-              <bold>true</bold>
-             </font>
-            </property>
-            <property name="text">
-             <string>Enable</string>
-            </property>
-            <property name="alignment">
-             <set>Qt::AlignCenter</set>
-            </property>
-           </widget>
-          </item>
-          <item row="4" column="0">
-           <widget class="QPushButton" name="en_custom_controller">
-            <property name="sizePolicy">
-             <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-              <horstretch>0</horstretch>
-              <verstretch>0</verstretch>
-             </sizepolicy>
-            </property>
-            <property name="maximumSize">
-             <size>
-              <width>16777215</width>
-              <height>50</height>
-             </size>
-            </property>
-            <property name="font">
-             <font>
-              <pointsize>14</pointsize>
-             </font>
-            </property>
-            <property name="text">
-             <string>Demo</string>
-            </property>
-           </widget>
-          </item>
-          <item row="5" column="0">
-           <spacer name="verticalSpacer">
-            <property name="orientation">
-             <enum>Qt::Vertical</enum>
-            </property>
-            <property name="sizeHint" stdset="0">
-             <size>
-              <width>20</width>
-              <height>40</height>
-             </size>
-            </property>
-           </spacer>
-          </item>
-          <item row="3" column="2">
-           <widget class="QPushButton" name="load_safe_yaml_button">
-            <property name="sizePolicy">
-             <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-              <horstretch>0</horstretch>
-              <verstretch>0</verstretch>
-             </sizepolicy>
-            </property>
-            <property name="maximumSize">
-             <size>
-              <width>16777215</width>
-              <height>50</height>
-             </size>
-            </property>
-            <property name="font">
-             <font>
-              <pointsize>14</pointsize>
-             </font>
-            </property>
-            <property name="text">
-             <string>Safe</string>
-            </property>
-           </widget>
-          </item>
-          <item row="3" column="0">
-           <widget class="QPushButton" name="en_safe_controller">
-            <property name="sizePolicy">
-             <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-              <horstretch>0</horstretch>
-              <verstretch>0</verstretch>
-             </sizepolicy>
-            </property>
-            <property name="maximumSize">
-             <size>
-              <width>16777215</width>
-              <height>50</height>
-             </size>
-            </property>
-            <property name="font">
-             <font>
-              <pointsize>14</pointsize>
-             </font>
-            </property>
-            <property name="text">
-             <string>Safe</string>
-            </property>
-           </widget>
-          </item>
-          <item row="4" column="2">
-           <widget class="QPushButton" name="load_custom_yaml_button">
-            <property name="sizePolicy">
-             <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
-              <horstretch>0</horstretch>
-              <verstretch>0</verstretch>
-             </sizepolicy>
-            </property>
-            <property name="maximumSize">
-             <size>
-              <width>16777215</width>
-              <height>50</height>
-             </size>
-            </property>
-            <property name="font">
-             <font>
-              <pointsize>14</pointsize>
-             </font>
-            </property>
-            <property name="text">
-             <string>Demo</string>
-            </property>
-           </widget>
-          </item>
-          <item row="1" column="2">
-           <widget class="QLabel" name="label_22">
-            <property name="sizePolicy">
-             <sizepolicy hsizetype="Preferred" vsizetype="Minimum">
-              <horstretch>0</horstretch>
-              <verstretch>0</verstretch>
-             </sizepolicy>
-            </property>
-            <property name="font">
-             <font>
-              <pointsize>16</pointsize>
-              <weight>75</weight>
-              <bold>true</bold>
-             </font>
-            </property>
-            <property name="text">
-             <string>YAML</string>
-            </property>
-            <property name="alignment">
-             <set>Qt::AlignCenter</set>
-            </property>
-           </widget>
-          </item>
-          <item row="3" column="1">
-           <widget class="Line" name="line_3">
-            <property name="orientation">
-             <enum>Qt::Vertical</enum>
-            </property>
-           </widget>
-          </item>
-          <item row="4" column="1">
-           <widget class="Line" name="line_4">
-            <property name="orientation">
-             <enum>Qt::Vertical</enum>
-            </property>
-           </widget>
-          </item>
-          <item row="5" column="1">
-           <widget class="Line" name="line_5">
-            <property name="orientation">
-             <enum>Qt::Vertical</enum>
-            </property>
-           </widget>
-          </item>
-          <item row="1" column="1">
-           <widget class="Line" name="line_6">
-            <property name="orientation">
-             <enum>Qt::Vertical</enum>
-            </property>
-           </widget>
-          </item>
-          <item row="0" column="1">
-           <widget class="Line" name="line_7">
-            <property name="orientation">
-             <enum>Qt::Vertical</enum>
-            </property>
-           </widget>
-          </item>
-         </layout>
-        </item>
-        <item>
-         <widget class="Line" name="line_2">
-          <property name="orientation">
-           <enum>Qt::Vertical</enum>
-          </property>
-         </widget>
-        </item>
-       </layout>
-      </item>
-     </layout>
-    </item>
-   </layout>
-  </widget>
-  <widget class="QMenuBar" name="menuBar">
-   <property name="geometry">
-    <rect>
-     <x>0</x>
-     <y>0</y>
-     <width>1092</width>
-     <height>25</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"/>
- <resources/>
- <connections/>
-</ui>
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/coordinatorGUI/src/main.cpp b/pps_ws/src/d_fall_pps/GUI_Qt/coordinatorGUI/src/main.cpp
deleted file mode 100644
index 27bdde984af32065c2c7c03ddef660376e51004a..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/GUI_Qt/coordinatorGUI/src/main.cpp
+++ /dev/null
@@ -1,42 +0,0 @@
-//    Copyright (C) 2017, ETH Zurich, D-ITET, Angel Romero
-//
-//    This file is part of D-FaLL-System.
-//    
-//    D-FaLL-System is free software: you can redistribute it and/or modify
-//    it under the terms of the GNU General Public License as published by
-//    the Free Software Foundation, either version 3 of the License, or
-//    (at your option) any later version.
-//    
-//    D-FaLL-System is distributed in the hope that it will be useful,
-//    but WITHOUT ANY WARRANTY; without even the implied warranty of
-//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-//    GNU General Public License for more details.
-//    
-//    You should have received a copy of the GNU General Public License
-//    along with D-FaLL-System.  If not, see <http://www.gnu.org/licenses/>.
-//    
-//
-//    ----------------------------------------------------------------------------------
-//    DDDD        FFFFF        L     L           SSSS  Y   Y   SSSS  TTTTT  EEEEE  M   M
-//    D   D       F      aaa   L     L          S       Y Y   S        T    E      MM MM
-//    D   D  ---  FFFF  a   a  L     L     ---   SSS     Y     SSS     T    EEE    M M M
-//    D   D       F     a  aa  L     L              S    Y        S    T    E      M   M
-//    DDDD        F      aa a  LLLL  LLLL       SSSS     Y    SSSS     T    EEEEE  M   M
-//
-//
-//    DESCRIPTION:
-//    Main program of the Student's GUI
-//
-//    ----------------------------------------------------------------------------------
-
-
-#include "MainWindow.h"
-#include <QApplication>
-
-int main(int argc, char *argv[])
-{
-    QApplication a(argc, argv);
-    MainWindow w(argc, argv);
-    w.show();
-    return a.exec();
-}
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/coordinatorGUI/src/rosNodeThread.cpp b/pps_ws/src/d_fall_pps/GUI_Qt/coordinatorGUI/src/rosNodeThread.cpp
deleted file mode 100644
index ce3b6182fe6e8576106c2a52292e9f5d81f6eae6..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/GUI_Qt/coordinatorGUI/src/rosNodeThread.cpp
+++ /dev/null
@@ -1,114 +0,0 @@
-//    Copyright (C) 2017, ETH Zurich, D-ITET, Angel Romero
-//
-//    This file is part of D-FaLL-System.
-//    
-//    D-FaLL-System is free software: you can redistribute it and/or modify
-//    it under the terms of the GNU General Public License as published by
-//    the Free Software Foundation, either version 3 of the License, or
-//    (at your option) any later version.
-//    
-//    D-FaLL-System is distributed in the hope that it will be useful,
-//    but WITHOUT ANY WARRANTY; without even the implied warranty of
-//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-//    GNU General Public License for more details.
-//    
-//    You should have received a copy of the GNU General Public License
-//    along with D-FaLL-System.  If not, see <http://www.gnu.org/licenses/>.
-//    
-//
-//    ----------------------------------------------------------------------------------
-//    DDDD        FFFFF        L     L           SSSS  Y   Y   SSSS  TTTTT  EEEEE  M   M
-//    D   D       F      aaa   L     L          S       Y Y   S        T    E      MM MM
-//    D   D  ---  FFFF  a   a  L     L     ---   SSS     Y     SSS     T    EEE    M M M
-//    D   D       F     a  aa  L     L              S    Y        S    T    E      M   M
-//    DDDD        F      aa a  LLLL  LLLL       SSSS     Y    SSSS     T    EEEEE  M   M
-//
-//
-//    DESCRIPTION:
-//    Takes care of creating a ros node thread.
-//
-//    ----------------------------------------------------------------------------------
-
-
-#include "rosNodeThread.h"
-
-#include "d_fall_pps/CMRead.h"
-#include "d_fall_pps/CMUpdate.h"
-#include "d_fall_pps/CMCommand.h"
-
-
-rosNodeThread::rosNodeThread(int argc, char** pArgv, const char * node_name, QObject* parent)
-    :   QObject(parent),
-        m_Init_argc(argc),
-        m_pInit_argv(pArgv),
-        m_node_name(node_name)
-
-{
-    /** Constructor for the node thread **/
-}
-
-rosNodeThread::~rosNodeThread()
-{
-    if (ros::isStarted())
-    {
-        ros::shutdown();
-        ros::waitForShutdown();
-    } // end if
-    m_pThread->wait();
-} // end destructor
-
-bool rosNodeThread::init()
-{
-    m_pThread = new QThread();
-    this->moveToThread(m_pThread); // QObject method
-
-    connect(m_pThread, SIGNAL(started()), this, SLOT(run()));
-    ros::init(m_Init_argc, m_pInit_argv, m_node_name); // student_GUI is the name of this node
-
-    if (!ros::master::check())
-    {
-        ROS_ERROR("Master not found, please check teacher computer is running and try again");
-        return false;           // do not start without ros.
-    }
-
-    ros::start();
-    ros::Time::init();
-    ros::NodeHandle nh("~");
-
-    m_vicon_subscriber = nh.subscribe("/ViconDataPublisher/ViconData", 100, &rosNodeThread::messageCallback, this);
-
-    // clients for db services:
-    m_read_db_client = nh.serviceClient<CMRead>("/CentralManagerService/Read", false);
-    m_update_db_client = nh.serviceClient<CMUpdate>("/CentralManagerService/Update", false);
-    m_command_db_client = nh.serviceClient<CMCommand>("/CentralManagerService/Command", false);
-
-    m_pThread->start();
-    return true;
-} // set up the thread
-
-void rosNodeThread::messageCallback(const ptrToMessage& p_msg) // When a message arrives to the topic, this callback is executed
-{
-    emit newViconData(p_msg);   //pass the message to other places
-}
-
-void rosNodeThread::run()
-{
-    ros::Rate loop_rate(100);
-    QMutex * pMutex;
-    while (ros::ok())
-    {
-        pMutex = new QMutex();
-
-        // geometry_msgs::Twist cmd_msg;
-        pMutex->lock();
-        // cmd_msg.linear.x = m_speed;
-        // cmd_msg.angular.z = m_angle;
-        pMutex->unlock();
-        // ROS_INFO("RUNNING");
-
-        // sim_velocity.publish(cmd_msg);
-        ros::spinOnce();
-        loop_rate.sleep();
-        delete pMutex;
-    } // do ros things.
-}