diff --git a/pps_ws/src/d_fall_pps/CMakeLists.txt b/pps_ws/src/d_fall_pps/CMakeLists.txt
index a7bf2d91b3d142b568509e5d4efc270bf221a6fb..abda2b2087bf3a35f97a0a0d8f95c601273f5e66 100755
--- a/pps_ws/src/d_fall_pps/CMakeLists.txt
+++ b/pps_ws/src/d_fall_pps/CMakeLists.txt
@@ -42,6 +42,10 @@ 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)
 
@@ -85,6 +89,18 @@ 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
 ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
@@ -199,8 +215,9 @@ generate_messages(
 ## CATKIN_DEPENDS: catkin_packages dependent projects also need
 ## DEPENDS: system dependencies of this project that dependent projects also need
 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 ${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
@@ -215,7 +232,8 @@ catkin_package(
 # include_directories(include)
 include_directories(
   ${MY_GUI_LIB_PATH_INC}            # GUI -- include directory inside GUI folder
-  ${STUDENT_GUI_LIB_PATH_INC}            # StudentGUI -- 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
 )
@@ -269,6 +287,13 @@ 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
 add_executable(my_GUI ${MY_CPP_SOURCES_GUI} ${UIS_HDRS_GUI} ${SRC_MOC_HDRS_GUI} ${MY_RESOURCE_FILE_RCC})
@@ -278,6 +303,10 @@ 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)
+
 
 
 add_dependencies(ViconDataPublisher d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
@@ -293,6 +322,9 @@ 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
@@ -333,9 +365,12 @@ target_link_libraries(my_GUI ${catkin_LIBRARIES})
 
 # StudentGUI -- link 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
new file mode 100644
index 0000000000000000000000000000000000000000..4f43aedbed97bb2800fd5eac5501c59fad890f7b
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/coordinatorGUI/coordinatorGUI.pro
@@ -0,0 +1,25 @@
+#-------------------------------------------------
+#
+# 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
new file mode 100644
index 0000000000000000000000000000000000000000..be4888a000e4ac892adc5da572790d1404c9a286
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/coordinatorGUI/include/MainWindow.h
@@ -0,0 +1,217 @@
+//    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
new file mode 100644
index 0000000000000000000000000000000000000000..193561248a04e927fc8b337f9384bbf6e33b07e8
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/coordinatorGUI/include/rosNodeThread.h
@@ -0,0 +1,93 @@
+//    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
new file mode 100644
index 0000000000000000000000000000000000000000..d86a3dd007e7c682d4b1049f6f46d01f54b9a2a1
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/coordinatorGUI/src/MainWindow.cpp
@@ -0,0 +1,756 @@
+//    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
new file mode 100644
index 0000000000000000000000000000000000000000..64e3f3428eea12575294400a5107d7b4abc76162
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/coordinatorGUI/src/MainWindow.ui
@@ -0,0 +1,2064 @@
+<?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
new file mode 100644
index 0000000000000000000000000000000000000000..27bdde984af32065c2c7c03ddef660376e51004a
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/coordinatorGUI/src/main.cpp
@@ -0,0 +1,42 @@
+//    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
new file mode 100644
index 0000000000000000000000000000000000000000..ce3b6182fe6e8576106c2a52292e9f5d81f6eae6
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/coordinatorGUI/src/rosNodeThread.cpp
@@ -0,0 +1,114 @@
+//    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.
+}
diff --git a/pps_ws/src/d_fall_pps/launch/Coordinator.launch b/pps_ws/src/d_fall_pps/launch/Coordinator.launch
new file mode 100755
index 0000000000000000000000000000000000000000..0bc6d34e2dc7164f431fa80f5e4063d2772a2273
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/launch/Coordinator.launch
@@ -0,0 +1,7 @@
+<launch>
+
+	<!-- COORDINATOR GUI -->
+    <node pkg="d_fall_pps" name="coordinator_GUI" output="screen" type="coordinator_GUI">
+	</node>
+
+</launch>