diff --git a/pps install/99-crazyflie.rules b/install/99-crazyflie.rules
similarity index 100%
rename from pps install/99-crazyflie.rules
rename to install/99-crazyflie.rules
diff --git a/pps install/99-crazyradio.rules b/install/99-crazyradio.rules
similarity index 100%
rename from pps install/99-crazyradio.rules
rename to install/99-crazyradio.rules
diff --git a/pps install/pps_install.sh b/install/pps_install.sh
similarity index 100%
rename from pps install/pps_install.sh
rename to install/pps_install.sh
diff --git a/pps_ws/src/d_fall_pps/CMakeLists.txt b/pps_ws/src/d_fall_pps/CMakeLists.txt
index 3e92c2eb1509ae3176e774a68ff1bbbff0bb5dc5..065f356282490ed3a1ae91a0e8d40d6a16dfe2f5 100755
--- a/pps_ws/src/d_fall_pps/CMakeLists.txt
+++ b/pps_ws/src/d_fall_pps/CMakeLists.txt
@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.0)
project(d_fall_pps)
## Add support for C++11, supported in ROS Kinetic and newer
@@ -15,7 +15,27 @@ find_package(catkin REQUIRED COMPONENTS
genmsg
rosbag
roslib
-)
+ )
+
+find_package (Eigen3 3.3 NO_MODULE)
+
+
+# VICON DATASTREAM SDK PACKAGE
+# Ensure that the cache variable where the result would be stored is clear
+message(STATUS "NOTE: Prior to being unset: VICON_LIBRARY = " ${VICON_LIBRARY})
+unset(VICON_LIBRARY CACHE)
+message(STATUS "NOTE: After being unset: VICON_LIBRARY = " ${VICON_LIBRARY})
+
+# Find the Vicon Data Stream SDK Pacakge
+find_library(VICON_LIBRARY ViconDataStreamSDK_CPP PATHS lib/vicon NO_DEFAULT_PATH)
+message(STATUS "NOTE: After calling find_library(): VICON_LIBRARY = " ${VICON_LIBRARY})
+
+# Let the user know if the library was found or not
+if(VICON_LIBRARY)
+ message(STATUS "NOTE: the Vicon Data Stream SDK library was found")
+else()
+ message(STATUS "NOTE: the Vicon Data Stream SDK library was NOT found")
+endif()
# GUI -- Add precompiler definitions to include ROS things in GUI compilation
@@ -34,6 +54,7 @@ find_package(Qt5Core REQUIRED)
find_package(Qt5Gui REQUIRED)
find_package(Qt5Svg REQUIRED)
+
# GUI -- Add src and includes
set(MY_GUI_LIB_PATH_SRC ${PROJECT_SOURCE_DIR}/GUI_Qt/CrazyFlyGUI/src)
set(MY_GUI_LIB_PATH_INC ${PROJECT_SOURCE_DIR}/GUI_Qt/CrazyFlyGUI/include)
@@ -85,6 +106,7 @@ 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})
+
## 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
@@ -134,8 +156,8 @@ add_message_files(
CrazyflieDB.msg
#----------------------------------------------------------------------
DebugMsg.msg
- CustomControllerYAML.msg
CustomButton.msg
+ ViconSubscribeObjectName.msg
)
## Generate services in the 'srv' folder
@@ -146,6 +168,7 @@ add_message_files(
# )
add_service_files(
FILES
+ LoadYamlFiles.srv
Controller.srv
CMRead.srv
CMQuery.srv
@@ -200,8 +223,8 @@ 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
LIBRARIES
CATKIN_DEPENDS roscpp rospy std_msgs rosbag roslib
DEPENDS
@@ -216,9 +239,10 @@ 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
${catkin_INCLUDE_DIRS}
include
+ include/nodes
)
## Declare a C++ library
@@ -235,15 +259,20 @@ include_directories(
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/d_fall_pps_node.cpp)
-add_executable(ViconDataPublisher src/ViconDataPublisher.cpp)
-add_executable(PPSClient src/PPSClient.cpp)
-add_executable(SafeControllerService src/SafeControllerService.cpp)
-add_executable(CustomControllerService src/CustomControllerService.cpp)
-add_executable(CentralManagerService src/CentralManagerService.cpp src/CrazyflieIO.cpp)
-add_executable(ParameterService src/ParameterService.cpp)
-add_executable(CircleControllerService src/CircleControllerService.cpp)
-add_executable(FollowCrazyflieService src/FollowCrazyflieService.cpp)
-add_executable(FollowN_1Service src/FollowN_1Service.cpp)
+
+if(VICON_LIBRARY)
+ add_executable(ViconDataPublisher src/nodes/ViconDataPublisher.cpp)
+endif()
+
+add_executable(PPSClient src/nodes/PPSClient.cpp)
+add_executable(SafeControllerService src/nodes/SafeControllerService.cpp)
+add_executable(DemoControllerService src/nodes/DemoControllerService.cpp)
+add_executable(StudentControllerService src/nodes/StudentControllerService.cpp)
+add_executable(MpcControllerService src/nodes/MpcControllerService.cpp)
+add_executable(RemoteControllerService src/nodes/RemoteControllerService.cpp)
+add_executable(TuningControllerService src/nodes/TuningControllerService.cpp)
+add_executable(CentralManagerService src/nodes/CentralManagerService.cpp src/CrazyflieIO.cpp)
+add_executable(ParameterService src/nodes/ParameterService.cpp)
@@ -274,6 +303,7 @@ set(MY_CPP_SOURCES_STUDENT_GUI # compilation of sources
)
+
# GUI -- Add executables here
add_executable(my_GUI ${MY_CPP_SOURCES_GUI} ${UIS_HDRS_GUI} ${SRC_MOC_HDRS_GUI} ${MY_RESOURCE_FILE_RCC})
qt5_use_modules(my_GUI Widgets)
@@ -284,15 +314,19 @@ qt5_use_modules(student_GUI Widgets)
-add_dependencies(ViconDataPublisher d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
-add_dependencies(PPSClient d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
-add_dependencies(SafeControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
-add_dependencies(CustomControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
-add_dependencies(CentralManagerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
-add_dependencies(ParameterService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
-add_dependencies(CircleControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
-add_dependencies(FollowCrazyflieService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
-add_dependencies(FollowN_1Service d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+if(VICON_LIBRARY)
+ add_dependencies(ViconDataPublisher d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+endif()
+
+add_dependencies(PPSClient d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(SafeControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(DemoControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(StudentControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(MpcControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(RemoteControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(TuningControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(CentralManagerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(ParameterService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
# GUI-- dependencies
add_dependencies(my_GUI d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
@@ -315,26 +349,27 @@ add_dependencies(student_GUI d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_
# ${catkin_LIBRARIES}
# )
-find_library(VICON_LIBRARY ViconDataStreamSDK_CPP HINTS lib/vicon)
-
-target_link_libraries(ViconDataPublisher ${catkin_LIBRARIES})
-target_link_libraries(ViconDataPublisher ${VICON_LIBRARY})
-
-target_link_libraries(PPSClient ${catkin_LIBRARIES})
+# find_library(VICON_LIBRARY ViconDataStreamSDK_CPP HINTS lib/vicon)
-target_link_libraries(SafeControllerService ${catkin_LIBRARIES})
+if(VICON_LIBRARY)
+ target_link_libraries(ViconDataPublisher ${catkin_LIBRARIES})
+ target_link_libraries(ViconDataPublisher ${VICON_LIBRARY})
+endif()
-target_link_libraries(CustomControllerService ${catkin_LIBRARIES})
+target_link_libraries(PPSClient ${catkin_LIBRARIES})
+target_link_libraries(SafeControllerService ${catkin_LIBRARIES})
+target_link_libraries(DemoControllerService ${catkin_LIBRARIES})
+target_link_libraries(StudentControllerService ${catkin_LIBRARIES})
+if(Eigen3_FOUND)
+ target_link_libraries(MpcControllerService ${catkin_LIBRARIES} Eigen3::Eigen)
+else()
+ target_link_libraries(MpcControllerService ${catkin_LIBRARIES})
+endif()
+target_link_libraries(RemoteControllerService ${catkin_LIBRARIES})
+target_link_libraries(TuningControllerService ${catkin_LIBRARIES})
+target_link_libraries(CentralManagerService ${catkin_LIBRARIES})
+target_link_libraries(ParameterService ${catkin_LIBRARIES})
-target_link_libraries(CentralManagerService ${catkin_LIBRARIES})
-
-target_link_libraries(ParameterService ${catkin_LIBRARIES})
-
-target_link_libraries(CircleControllerService ${catkin_LIBRARIES})
-
-target_link_libraries(FollowCrazyflieService ${catkin_LIBRARIES})
-
-target_link_libraries(FollowN_1Service ${catkin_LIBRARIES})
# GUI -- link libraries
target_link_libraries(my_GUI Qt5::Widgets) # GUI -- let my_GUI have acesss to Qt stuff
@@ -346,7 +381,6 @@ 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})
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/mainguiwindow.cpp b/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/mainguiwindow.cpp
index 7bb024784a2180f2d788138a63eacab83475ffe2..f0c79b5bc2d296ed2428f666833301a1bd780886 100755
--- a/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/mainguiwindow.cpp
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/mainguiwindow.cpp
@@ -44,6 +44,7 @@
#include <QMetaType>
#include <QDir>
#include <regex>
+#include <QShortcut>
#ifdef CATKIN_MAKE
#include "d_fall_pps/UnlabeledMarker.h"
@@ -51,7 +52,7 @@
#include "d_fall_pps/CrazyflieEntry.h"
#include "d_fall_pps/CMUpdate.h"
#include "d_fall_pps/CMCommand.h"
-#include "CentralManagerService.h"
+#include "nodes/CentralManagerService.h"
#include <ros/ros.h>
#include <ros/network.h>
@@ -223,6 +224,13 @@ void MainGUIWindow::_init()
ui->checkBox_vicon_highlight_markers->setEnabled(false);
+ // Add keyboard shortcuts
+ // > for "all motors off", press the space bar
+ ui->all_motors_off_button->setShortcut(tr("Space"));
+ // > for "kill GUI node", press "CTRL+C" while the GUI window is the focus
+ QShortcut* close_GUI_shortcut = new QShortcut(QKeySequence(tr("CTRL+C")), this, SLOT(close()));
+
+
#ifdef CATKIN_MAKE
_rosNodeThread->init();
qRegisterMetaType<ptrToMessage>("ptrToMessage");
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h
index be4888a000e4ac892adc5da572790d1404c9a286..dfcedc29d3a6afb1bbdd22ca649c2def28c44af8 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h
@@ -43,11 +43,17 @@
#include "d_fall_pps/CrazyflieContext.h"
#include "d_fall_pps/CrazyflieData.h"
#include "d_fall_pps/Setpoint.h"
+#include "d_fall_pps/ViconSubscribeObjectName.h"
// Types of controllers being used:
-#define SAFE_CONTROLLER 0
-#define CUSTOM_CONTROLLER 1
+#define SAFE_CONTROLLER 1
+#define DEMO_CONTROLLER 2
+#define STUDENT_CONTROLLER 3
+#define MPC_CONTROLLER 4
+#define REMOTE_CONTROLLER 5
+#define TUNING_CONTROLLER 6
+
// Commands for CrazyRadio
#define CMD_RECONNECT 0
@@ -62,11 +68,16 @@
// 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
+#define CMD_USE_SAFE_CONTROLLER 1
+#define CMD_USE_DEMO_CONTROLLER 2
+#define CMD_USE_STUDENT_CONTROLLER 3
+#define CMD_USE_MPC_CONTROLLER 4
+#define CMD_USE_REMOTE_CONTROLLER 5
+#define CMD_USE_TUNING_CONTROLLER 6
+
+#define CMD_CRAZYFLY_TAKE_OFF 11
+#define CMD_CRAZYFLY_LAND 12
+#define CMD_CRAZYFLY_MOTORS_OFF 13
// Flying States
#define STATE_MOTORS_OFF 1
@@ -80,9 +91,18 @@
// 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
+#define LOAD_YAML_DEMO_CONTROLLER_AGENT 2
+#define LOAD_YAML_STUDENT_CONTROLLER_AGENT 3
+#define LOAD_YAML_MPC_CONTROLLER_AGENT 4
+#define LOAD_YAML_REMOTE_CONTROLLER_AGENT 5
+#define LOAD_YAML_TUNING_CONTROLLER_AGENT 6
+
+#define LOAD_YAML_SAFE_CONTROLLER_COORDINATOR 11
+#define LOAD_YAML_DEMO_CONTROLLER_COORDINATOR 12
+#define LOAD_YAML_STUDENT_CONTROLLER_COORDINATOR 13
+#define LOAD_YAML_MPC_CONTROLLER_COORDINATOR 14
+#define LOAD_YAML_REMOTE_CONTROLLER_COORDINATOR 15
+#define LOAD_YAML_TUNING_CONTROLLER_COORDINATOR 16
// Universal constants
#define PI 3.141592653589
@@ -104,29 +124,66 @@ public:
private slots:
void updateNewViconData(const ptrToMessage& p_msg);
+
+ // # RF Crazyradio Connect Disconnect
void on_RF_Connect_button_clicked();
+ void on_RF_disconnect_button_clicked();
+ // # Take off, lanf, motors off
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();
+ // # Setpoint
+ void on_set_setpoint_button_safe_clicked();
+ void on_set_setpoint_button_demo_clicked();
+ void on_set_setpoint_button_student_clicked();
+ void on_set_setpoint_button_mpc_clicked();
- void on_load_custom_yaml_button_clicked();
+ // # Load Yaml when acting as the GUI for an Agent
void on_load_safe_yaml_button_clicked();
+ void on_load_demo_yaml_button_clicked();
+ void on_load_student_yaml_button_clicked();
+ void on_load_mpc_yaml_button_clicked();
+ void on_load_remote_yaml_button_clicked();
+ void on_load_tuning_yaml_button_clicked();
+
+ // # Enable controllers
+ void on_enable_safe_controller_clicked();
+ void on_enable_demo_controller_clicked();
+ void on_enable_student_controller_clicked();
+ void on_enable_mpc_controller_clicked();
+ void on_enable_remote_controller_clicked();
+ void on_enable_tuning_controller_clicked();
+
+
+
+ void on_demoButton_1_clicked();
+ void on_demoButton_2_clicked();
+ void on_demoButton_3_clicked();
+
+ void on_studentButton_1_clicked();
+ void on_studentButton_2_clicked();
+ void on_studentButton_3_clicked();
+
+ // Buttons within the REMOTE controller tab
+ void on_remote_subscribe_button_clicked();
+ void on_remote_unsubscribe_button_clicked();
+ void on_remote_activate_button_clicked();
+ void on_remote_deactivate_button_clicked();
+
+ // Buttons within the TUNING controller tab
+ void on_tuning_test_horizontal_button_clicked();
+ void on_tuning_test_vertical_button_clicked();
+ void on_tuning_test_heading_button_clicked();
+ void on_tuning_test_all_button_clicked();
+ void on_tuning_test_circle_button_clicked();
+ void on_tuning_slider_horizontal_valueChanged(int value);
+ void on_tuning_slider_vertical_valueChanged(int value);
+ void on_tuning_slider_heading_valueChanged(int value);
- 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;
@@ -138,13 +195,20 @@ private:
std::string m_ros_namespace;
ros::Timer m_timer_yaml_file_for_safe_controller;
- ros::Timer m_timer_yaml_file_for_custom_controlller;
+ ros::Timer m_timer_yaml_file_for_demo_controller;
+ ros::Timer m_timer_yaml_file_for_student_controller;
+ ros::Timer m_timer_yaml_file_for_mpc_controller;
+ ros::Timer m_timer_yaml_file_for_remote_controller;
+ ros::Timer m_timer_yaml_file_for_tuning_controller;
+
int m_student_id;
CrazyflieContext m_context;
Setpoint m_safe_setpoint;
- Setpoint m_custom_setpoint;
+ Setpoint m_demo_setpoint;
+ Setpoint m_student_setpoint;
+ Setpoint m_mpc_setpoint;
int m_battery_state;
@@ -158,10 +222,36 @@ private:
ros::Publisher controllerSetpointPublisher;
ros::Subscriber safeSetpointSubscriber;
- ros::Publisher customSetpointPublisher;
- ros::Subscriber customSetpointSubscriber;
-
- ros::Publisher PPSClientStudentCustomButtonPublisher;
+ // SUBSCRIBERS AND PUBLISHERS:
+ // > For the Demo Controller SETPOINTS
+ ros::Publisher demoSetpointPublisher;
+ ros::Subscriber demoSetpointSubscriber;
+ // > For the Student Controller SETPOINTS
+ ros::Publisher studentSetpointPublisher;
+ ros::Subscriber studentSetpointSubscriber;
+ // > For the MPC Controller SETPOINTS
+ ros::Publisher mpcSetpointPublisher;
+ ros::Subscriber mpcSetpointSubscriber;
+
+ // > For the Remote Controller subscribe action
+ ros::Publisher remoteSubscribePublisher;
+ // > For the Remote Controller activate action
+ ros::Publisher remoteActivatePublisher;
+ // > For the Remote Controller data
+ ros::Subscriber remoteDataSubscriber;
+ // > For the Remote Control setpoint
+ ros::Subscriber remoteControlSetpointSubscriber;
+
+ // > For the TUNING CONTROLLER "test" button publisher
+ ros::Publisher tuningActivateTestPublisher;
+ // > For the TUNING CONTOLLER "gain" sliders
+ ros::Publisher tuningHorizontalGainPublisher;
+ ros::Publisher tuningVerticalGainPublisher;
+ ros::Publisher tuningHeadingGainPublisher;
+
+
+ ros::Publisher demoCustomButtonPublisher;
+ ros::Publisher studentCustomButtonPublisher;
ros::Subscriber DBChangedSubscriber;
@@ -186,11 +276,29 @@ private:
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 demoSetpointCallback(const Setpoint& newSetpoint);
+ void studentSetpointCallback(const Setpoint& newSetpoint);
+ void mpcSetpointCallback(const Setpoint& newSetpoint);
+
+
+ void remoteDataCallback(const CrazyflieData& objectData);
+ void remoteControlSetpointCallback(const CrazyflieData& setpointData);
+
+
void DBChangedCallback(const std_msgs::Int32& msg);
- void customYamlFileTimerCallback(const ros::TimerEvent&);
+
+ // # Load Yaml when acting as the GUI for an Agent
void safeYamlFileTimerCallback(const ros::TimerEvent&);
+ void demoYamlFileTimerCallback(const ros::TimerEvent&);
+ void studentYamlFileTimerCallback(const ros::TimerEvent&);
+ void mpcYamlFileTimerCallback(const ros::TimerEvent&);
+ void remoteYamlFileTimerCallback(const ros::TimerEvent&);
+ void tuningYamlFileTimerCallback(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);
@@ -200,13 +308,20 @@ private:
void setCrazyRadioStatus(int radio_status);
void loadCrazyflieContext();
void coordinatesToLocal(CrazyflieData& cf);
- void initialize_custom_setpoint();
+
+ void initialize_demo_setpoint();
+ void initialize_student_setpoint();
+ void initialize_mpc_setpoint();
void disableGUI();
void enableGUI();
void highlightSafeControllerTab();
- void highlightCustomControllerTab();
+ void highlightDemoControllerTab();
+ void highlightStudentControllerTab();
+ void highlightMpcControllerTab();
+ void highlightRemoteControllerTab();
+ void highlightTuningControllerTab();
bool setpointInsideBox(Setpoint setpoint, CrazyflieContext context);
Setpoint correctSetpointBox(Setpoint setpoint, CrazyflieContext context);
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp
index d86a3dd007e7c682d4b1049f6f46d01f54b9a2a1..41cb4ba7dae86dd1ae0832916cdb4f6182cbcf7a 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp
@@ -33,6 +33,7 @@
#include "MainWindow.h"
#include "ui_MainWindow.h"
#include <string>
+#include <QShortcut>
#include <ros/ros.h>
#include <ros/network.h>
@@ -58,15 +59,46 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
setCrazyRadioStatus(DISCONNECTED);
m_ros_namespace = ros::this_node::getNamespace();
- ROS_INFO("Student GUI node namespace: %s", m_ros_namespace.c_str());
+ 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 AND PUBLISHERS:
+ // > For the Demo Controller SETPOINTS and CUSTOM COMMANDS
+ demoSetpointPublisher = nodeHandle.advertise<Setpoint>("DemoControllerService/Setpoint", 1);
+ demoSetpointSubscriber = nodeHandle.subscribe("DemoControllerService/Setpoint", 1, &MainWindow::demoSetpointCallback, this);
+ demoCustomButtonPublisher = nodeHandle.advertise<CustomButton>("DemoControllerService/GUIButton", 1);
+ // > For the Student Controller SETPOINTS and CUSTOM COMMANDS
+ studentSetpointPublisher = nodeHandle.advertise<Setpoint>("StudentControllerService/Setpoint", 1);
+ studentSetpointSubscriber = nodeHandle.subscribe("StudentControllerService/Setpoint", 1, &MainWindow::studentSetpointCallback, this);
+ studentCustomButtonPublisher = nodeHandle.advertise<CustomButton>("StudentControllerService/GUIButton", 1);
+ // > For the MPC Controller SETPOINTS
+ mpcSetpointPublisher = nodeHandle.advertise<Setpoint>("MpcControllerService/Setpoint", 1);
+ mpcSetpointSubscriber = nodeHandle.subscribe("MpcControllerService/Setpoint", 1, &MainWindow::mpcSetpointCallback, this);
+
+
+ // > For the Remote Controller subscribe action
+ remoteSubscribePublisher = nodeHandle.advertise<ViconSubscribeObjectName>("RemoteControllerService/ViconSubscribeObjectName", 1);
+ // > For the Remote Controller activate action
+ remoteActivatePublisher = nodeHandle.advertise<std_msgs::Int32>("RemoteControllerService/Activate", 1);
+ // > For the Remote Controller data
+ remoteDataSubscriber = nodeHandle.subscribe("RemoteControllerService/RemoteData", 1, &MainWindow::remoteDataCallback, this);;
+ // > For the Remote Controller data
+ remoteControlSetpointSubscriber = nodeHandle.subscribe("RemoteControllerService/RemoteControlSetpoint", 1, &MainWindow::remoteControlSetpointCallback, this);;
+
+
+ // > For the TUNING CONTROLLER "test" button publisher
+ tuningActivateTestPublisher = nodeHandle.advertise<std_msgs::Int32>("TuningControllerService/ActivateTest", 1);
+ // > For the TUNING CONTOLLER "gain" sliders
+ tuningHorizontalGainPublisher = nodeHandle.advertise<std_msgs::Int32>("TuningControllerService/HorizontalGain", 1);
+ tuningVerticalGainPublisher = nodeHandle.advertise<std_msgs::Int32>("TuningControllerService/VerticalGain", 1);
+ tuningHeadingGainPublisher = nodeHandle.advertise<std_msgs::Int32>("TuningControllerService/HeadingGain", 1);
+
+
// subscribers
crazyRadioStatusSubscriber = nodeHandle.subscribe("CrazyRadio/CrazyRadioStatus", 1, &MainWindow::crazyRadioStatusCallback, this);
@@ -91,9 +123,7 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
//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);
+ PPSClientCommandPublisher = nh_PPSClient.advertise<std_msgs::Int32>("Command", 1);
// > For publishing a message that requests the
@@ -108,9 +138,9 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
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))
+ if(!nh_PPSClient.getParam("agentID", m_student_id))
{
- ROS_ERROR("Failed to get studentID");
+ ROS_ERROR("Failed to get agentID");
}
// Then, Central manager
@@ -131,10 +161,10 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
}
// 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]));
+ ui->current_setpoint_x_safe->setText(QString::number(default_setpoint[0]));
+ ui->current_setpoint_y_safe->setText(QString::number(default_setpoint[1]));
+ ui->current_setpoint_z_safe->setText(QString::number(default_setpoint[2]));
+ ui->current_setpoint_yaw_safe->setText(QString::number(default_setpoint[3]));
disableGUI();
@@ -145,7 +175,16 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
ui->error_label->setStyleSheet("QLabel { color : red; }");
ui->error_label->clear();
- initialize_custom_setpoint();
+ // Add keyboard shortcuts
+ // > for "all motors off", press the space bar
+ ui->motors_OFF_button->setShortcut(tr("Space"));
+ // > for "kill GUI node", press "CTRL+C" while the GUI window is the focus
+ QShortcut* close_GUI_shortcut = new QShortcut(QKeySequence(tr("CTRL+C")), this, SLOT(close()));
+
+
+ initialize_demo_setpoint();
+ initialize_student_setpoint();
+ initialize_mpc_setpoint();
}
@@ -175,11 +214,55 @@ void MainWindow::highlightSafeControllerTab()
{
ui->tabWidget->tabBar()->setTabTextColor(0, Qt::green);
ui->tabWidget->tabBar()->setTabTextColor(1, Qt::black);
+ ui->tabWidget->tabBar()->setTabTextColor(2, Qt::black);
+ ui->tabWidget->tabBar()->setTabTextColor(3, Qt::black);
+ ui->tabWidget->tabBar()->setTabTextColor(4, Qt::black);
+ ui->tabWidget->tabBar()->setTabTextColor(5, Qt::black);
}
-void MainWindow::highlightCustomControllerTab()
+void MainWindow::highlightDemoControllerTab()
{
ui->tabWidget->tabBar()->setTabTextColor(0, Qt::black);
ui->tabWidget->tabBar()->setTabTextColor(1, Qt::green);
+ ui->tabWidget->tabBar()->setTabTextColor(2, Qt::black);
+ ui->tabWidget->tabBar()->setTabTextColor(3, Qt::black);
+ ui->tabWidget->tabBar()->setTabTextColor(4, Qt::black);
+ ui->tabWidget->tabBar()->setTabTextColor(5, Qt::black);
+}
+void MainWindow::highlightStudentControllerTab()
+{
+ ui->tabWidget->tabBar()->setTabTextColor(0, Qt::black);
+ ui->tabWidget->tabBar()->setTabTextColor(1, Qt::black);
+ ui->tabWidget->tabBar()->setTabTextColor(2, Qt::green);
+ ui->tabWidget->tabBar()->setTabTextColor(3, Qt::black);
+ ui->tabWidget->tabBar()->setTabTextColor(4, Qt::black);
+ ui->tabWidget->tabBar()->setTabTextColor(5, Qt::black);
+}
+void MainWindow::highlightMpcControllerTab()
+{
+ ui->tabWidget->tabBar()->setTabTextColor(0, Qt::black);
+ ui->tabWidget->tabBar()->setTabTextColor(1, Qt::black);
+ ui->tabWidget->tabBar()->setTabTextColor(2, Qt::black);
+ ui->tabWidget->tabBar()->setTabTextColor(3, Qt::green);
+ ui->tabWidget->tabBar()->setTabTextColor(4, Qt::black);
+ ui->tabWidget->tabBar()->setTabTextColor(5, Qt::black);
+}
+void MainWindow::highlightRemoteControllerTab()
+{
+ ui->tabWidget->tabBar()->setTabTextColor(0, Qt::black);
+ ui->tabWidget->tabBar()->setTabTextColor(1, Qt::black);
+ ui->tabWidget->tabBar()->setTabTextColor(2, Qt::black);
+ ui->tabWidget->tabBar()->setTabTextColor(3, Qt::black);
+ ui->tabWidget->tabBar()->setTabTextColor(4, Qt::green);
+ ui->tabWidget->tabBar()->setTabTextColor(5, Qt::black);
+}
+void MainWindow::highlightTuningControllerTab()
+{
+ ui->tabWidget->tabBar()->setTabTextColor(0, Qt::black);
+ ui->tabWidget->tabBar()->setTabTextColor(1, Qt::black);
+ ui->tabWidget->tabBar()->setTabTextColor(2, Qt::black);
+ ui->tabWidget->tabBar()->setTabTextColor(3, Qt::black);
+ ui->tabWidget->tabBar()->setTabTextColor(4, Qt::black);
+ ui->tabWidget->tabBar()->setTabTextColor(5, Qt::green);
}
void MainWindow::DBChangedCallback(const std_msgs::Int32& msg)
@@ -195,8 +278,20 @@ void MainWindow::controllerUsedChangedCallback(const std_msgs::Int32& msg)
case SAFE_CONTROLLER:
highlightSafeControllerTab();
break;
- case CUSTOM_CONTROLLER:
- highlightCustomControllerTab();
+ case DEMO_CONTROLLER:
+ highlightDemoControllerTab();
+ break;
+ case STUDENT_CONTROLLER:
+ highlightStudentControllerTab();
+ break;
+ case MPC_CONTROLLER:
+ highlightMpcControllerTab();
+ break;
+ case REMOTE_CONTROLLER:
+ highlightRemoteControllerTab();
+ break;
+ case TUNING_CONTROLLER:
+ highlightTuningControllerTab();
break;
default:
break;
@@ -207,20 +302,40 @@ 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));
+ ui->current_setpoint_x_safe->setText(QString::number(newSetpoint.x, 'f', 3));
+ ui->current_setpoint_y_safe->setText(QString::number(newSetpoint.y, 'f', 3));
+ ui->current_setpoint_z_safe->setText(QString::number(newSetpoint.z, 'f', 3));
+ ui->current_setpoint_yaw_safe->setText(QString::number(newSetpoint.yaw * RAD2DEG, 'f', 1));
+}
+
+void MainWindow::demoSetpointCallback(const Setpoint& newSetpoint)
+{
+ m_demo_setpoint = newSetpoint;
+ // here we get the new setpoint, need to update it in GUI
+ ui->current_setpoint_x_demo->setText(QString::number(newSetpoint.x, 'f', 3));
+ ui->current_setpoint_y_demo->setText(QString::number(newSetpoint.y, 'f', 3));
+ ui->current_setpoint_z_demo->setText(QString::number(newSetpoint.z, 'f', 3));
+ ui->current_setpoint_yaw_demo->setText(QString::number(newSetpoint.yaw * RAD2DEG, 'f', 1));
+}
+
+void MainWindow::studentSetpointCallback(const Setpoint& newSetpoint)
+{
+ m_student_setpoint = newSetpoint;
+ // here we get the new setpoint, need to update it in GUI
+ ui->current_setpoint_x_student->setText(QString::number(newSetpoint.x, 'f', 3));
+ ui->current_setpoint_y_student->setText(QString::number(newSetpoint.y, 'f', 3));
+ ui->current_setpoint_z_student->setText(QString::number(newSetpoint.z, 'f', 3));
+ ui->current_setpoint_yaw_student->setText(QString::number(newSetpoint.yaw * RAD2DEG, 'f', 1));
}
-void MainWindow::customSetpointCallback(const Setpoint& newSetpoint)
+void MainWindow::mpcSetpointCallback(const Setpoint& newSetpoint)
{
- m_custom_setpoint = newSetpoint;
+ m_mpc_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));
+ ui->current_setpoint_x_mpc->setText(QString::number(newSetpoint.x, 'f', 3));
+ ui->current_setpoint_y_mpc->setText(QString::number(newSetpoint.y, 'f', 3));
+ ui->current_setpoint_z_mpc->setText(QString::number(newSetpoint.z, 'f', 3));
+ ui->current_setpoint_yaw_mpc->setText(QString::number(newSetpoint.yaw * RAD2DEG, 'f', 1));
}
void MainWindow::flyingStateChangedCallback(const std_msgs::Int32& msg)
@@ -404,34 +519,38 @@ void MainWindow::updateNewViconData(const ptrToMessage& p_msg) //connected to ne
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));
+ ui->current_x_safe->setText(QString::number(local.x, 'f', 3));
+ ui->current_y_safe->setText(QString::number(local.y, 'f', 3));
+ ui->current_z_safe->setText(QString::number(local.z, 'f', 3));
+ ui->current_yaw_safe->setText(QString::number(local.yaw * RAD2DEG, 'f', 1));
+ ui->current_pitch_safe->setText(QString::number(local.pitch * RAD2DEG, 'f', 1));
+ ui->current_roll_safe->setText(QString::number(local.roll * RAD2DEG, 'f', 1));
+
+ ui->current_x_demo->setText(QString::number(local.x, 'f', 3));
+ ui->current_y_demo->setText(QString::number(local.y, 'f', 3));
+ ui->current_z_demo->setText(QString::number(local.z, 'f', 3));
+ ui->current_yaw_demo->setText(QString::number(local.yaw * RAD2DEG, 'f', 1));
+ ui->current_pitch_demo->setText(QString::number(local.pitch * RAD2DEG, 'f', 1));
+ ui->current_roll_demo->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));
+ ui->diff_x_safe->setText(QString::number(m_safe_setpoint.x - local.x, 'f', 3));
+ ui->diff_y_safe->setText(QString::number(m_safe_setpoint.y - local.y, 'f', 3));
+ ui->diff_z_safe->setText(QString::number(m_safe_setpoint.z - local.z, 'f', 3));
+ ui->diff_yaw_safe->setText(QString::number((m_safe_setpoint.yaw - local.yaw) * RAD2DEG, 'f', 1));
+
+ ui->diff_x_demo->setText(QString::number(m_demo_setpoint.x - local.x, 'f', 3));
+ ui->diff_y_demo->setText(QString::number(m_demo_setpoint.y - local.y, 'f', 3));
+ ui->diff_z_demo->setText(QString::number(m_demo_setpoint.z - local.z, 'f', 3));
+ ui->diff_yaw_demo->setText(QString::number((m_demo_setpoint.yaw - local.yaw) * RAD2DEG, 'f', 1));
}
}
}
+
+
+// ----------------------------------------------------------------------------------
+// # RF Crazyradio Connect Disconnect
void MainWindow::on_RF_Connect_button_clicked()
{
std_msgs::Int32 msg;
@@ -440,6 +559,18 @@ void MainWindow::on_RF_Connect_button_clicked()
ROS_INFO("command reconnect published");
}
+void MainWindow::on_RF_disconnect_button_clicked()
+{
+ std_msgs::Int32 msg;
+ msg.data = CMD_DISCONNECT;
+ this->crazyRadioCommandPublisher.publish(msg);
+ ROS_INFO("command disconnect published");
+}
+
+
+
+// ----------------------------------------------------------------------------------
+// # Take off, lanf, motors off
void MainWindow::on_take_off_button_clicked()
{
std_msgs::Int32 msg;
@@ -461,28 +592,32 @@ void MainWindow::on_motors_OFF_button_clicked()
this->PPSClientCommandPublisher.publish(msg);
}
-void MainWindow::on_set_setpoint_button_clicked()
+
+
+// ----------------------------------------------------------------------------------
+// # Setpoint
+void MainWindow::on_set_setpoint_button_safe_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();
+ msg_setpoint.x = (ui->current_setpoint_x_safe->text()).toFloat();
+ msg_setpoint.y = (ui->current_setpoint_y_safe->text()).toFloat();
+ msg_setpoint.z = (ui->current_setpoint_z_safe->text()).toFloat();
+ msg_setpoint.yaw = (ui->current_setpoint_yaw_safe->text()).toFloat();
- if(!ui->new_setpoint_x->text().isEmpty())
- msg_setpoint.x = (ui->new_setpoint_x->text()).toFloat();
+ if(!ui->new_setpoint_x_safe->text().isEmpty())
+ msg_setpoint.x = (ui->new_setpoint_x_safe->text()).toFloat();
- if(!ui->new_setpoint_y->text().isEmpty())
- msg_setpoint.y = (ui->new_setpoint_y->text()).toFloat();
+ if(!ui->new_setpoint_y_safe->text().isEmpty())
+ msg_setpoint.y = (ui->new_setpoint_y_safe->text()).toFloat();
- if(!ui->new_setpoint_z->text().isEmpty())
- msg_setpoint.z = (ui->new_setpoint_z->text()).toFloat();
+ if(!ui->new_setpoint_z_safe->text().isEmpty())
+ msg_setpoint.z = (ui->new_setpoint_z_safe->text()).toFloat();
- if(!ui->new_setpoint_yaw->text().isEmpty())
- msg_setpoint.yaw = (ui->new_setpoint_yaw->text()).toFloat() * DEG2RAD;
+ if(!ui->new_setpoint_yaw_safe->text().isEmpty())
+ msg_setpoint.yaw = (ui->new_setpoint_yaw_safe->text()).toFloat() * DEG2RAD;
if(!setpointInsideBox(msg_setpoint, m_context))
@@ -503,7 +638,7 @@ void MainWindow::on_set_setpoint_button_clicked()
ROS_INFO_STREAM("Setpoint change clicked with:" << msg_setpoint.x << ", "<< msg_setpoint.y << ", "<< msg_setpoint.z << ", "<< msg_setpoint.yaw);
}
-void MainWindow::initialize_custom_setpoint()
+void MainWindow::initialize_demo_setpoint()
{
Setpoint msg_setpoint;
msg_setpoint.x = 0;
@@ -511,43 +646,103 @@ void MainWindow::initialize_custom_setpoint()
msg_setpoint.z = 0.4;
msg_setpoint.yaw = 0;
- this->customSetpointPublisher.publish(msg_setpoint);
+ this->demoSetpointPublisher.publish(msg_setpoint);
}
-void MainWindow::on_set_setpoint_button_2_clicked()
+void MainWindow::initialize_student_setpoint()
{
Setpoint msg_setpoint;
+ msg_setpoint.x = 0;
+ msg_setpoint.y = 0;
+ msg_setpoint.z = 0.4;
+ msg_setpoint.yaw = 0;
- 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();
+ this->studentSetpointPublisher.publish(msg_setpoint);
+}
- 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;
+void MainWindow::initialize_mpc_setpoint()
+{
+ Setpoint msg_setpoint;
+ msg_setpoint.x = 0;
+ msg_setpoint.y = 0;
+ msg_setpoint.z = 0.4;
+ msg_setpoint.yaw = 0;
+
+ this->mpcSetpointPublisher.publish(msg_setpoint);
+}
+
+void MainWindow::on_set_setpoint_button_demo_clicked()
+{
+ Setpoint msg_setpoint;
+
+ msg_setpoint.x = (ui->current_setpoint_x_demo->text()).toFloat();
+ msg_setpoint.y = (ui->current_setpoint_y_demo->text()).toFloat();
+ msg_setpoint.z = (ui->current_setpoint_z_demo->text()).toFloat();
+ msg_setpoint.yaw = (ui->current_setpoint_yaw_demo->text()).toFloat();
- this->customSetpointPublisher.publish(msg_setpoint);
+ if(!ui->new_setpoint_x_demo->text().isEmpty())
+ msg_setpoint.x = (ui->new_setpoint_x_demo->text()).toFloat();
+ if(!ui->new_setpoint_y_demo->text().isEmpty())
+ msg_setpoint.y = (ui->new_setpoint_y_demo->text()).toFloat();
+ if(!ui->new_setpoint_z_demo->text().isEmpty())
+ msg_setpoint.z = (ui->new_setpoint_z_demo->text()).toFloat();
+ if(!ui->new_setpoint_yaw_demo->text().isEmpty())
+ msg_setpoint.yaw = (ui->new_setpoint_yaw_demo->text()).toFloat() * DEG2RAD;
+
+ this->demoSetpointPublisher.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()
+void MainWindow::on_set_setpoint_button_student_clicked()
{
- std_msgs::Int32 msg;
- msg.data = CMD_DISCONNECT;
- this->crazyRadioCommandPublisher.publish(msg);
- ROS_INFO("command disconnect published");
+ Setpoint msg_setpoint;
+
+ msg_setpoint.x = (ui->current_setpoint_x_student->text()).toFloat();
+ msg_setpoint.y = (ui->current_setpoint_y_student->text()).toFloat();
+ msg_setpoint.z = (ui->current_setpoint_z_student->text()).toFloat();
+ msg_setpoint.yaw = (ui->current_setpoint_yaw_student->text()).toFloat();
+
+ if(!ui->new_setpoint_x_student->text().isEmpty())
+ msg_setpoint.x = (ui->new_setpoint_x_student->text()).toFloat();
+ if(!ui->new_setpoint_y_student->text().isEmpty())
+ msg_setpoint.y = (ui->new_setpoint_y_student->text()).toFloat();
+ if(!ui->new_setpoint_z_student->text().isEmpty())
+ msg_setpoint.z = (ui->new_setpoint_z_student->text()).toFloat();
+ if(!ui->new_setpoint_yaw_student->text().isEmpty())
+ msg_setpoint.yaw = (ui->new_setpoint_yaw_student->text()).toFloat() * DEG2RAD;
+
+ this->studentSetpointPublisher.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_set_setpoint_button_mpc_clicked()
+{
+ Setpoint msg_setpoint;
+
+ msg_setpoint.x = (ui->current_setpoint_x_mpc->text()).toFloat();
+ msg_setpoint.y = (ui->current_setpoint_y_mpc->text()).toFloat();
+ msg_setpoint.z = (ui->current_setpoint_z_mpc->text()).toFloat();
+ msg_setpoint.yaw = (ui->current_setpoint_yaw_mpc->text()).toFloat();
+ if(!ui->new_setpoint_x_mpc->text().isEmpty())
+ msg_setpoint.x = (ui->new_setpoint_x_mpc->text()).toFloat();
+ if(!ui->new_setpoint_y_mpc->text().isEmpty())
+ msg_setpoint.y = (ui->new_setpoint_y_mpc->text()).toFloat();
+ if(!ui->new_setpoint_z_mpc->text().isEmpty())
+ msg_setpoint.z = (ui->new_setpoint_z_mpc->text()).toFloat();
+ if(!ui->new_setpoint_yaw_mpc->text().isEmpty())
+ msg_setpoint.yaw = (ui->new_setpoint_yaw_mpc->text()).toFloat() * DEG2RAD;
+ this->mpcSetpointPublisher.publish(msg_setpoint);
+ ROS_INFO_STREAM("Setpoint change clicked with:" << msg_setpoint.x << ", "<< msg_setpoint.y << ", "<< msg_setpoint.z << ", "<< msg_setpoint.yaw);
+}
+
+
+// ----------------------------------------------------------------------------------
+// # Load Yaml when acting as the GUI for an Agent
void MainWindow::on_load_safe_yaml_button_clicked()
{
// Set the "load safe yaml" button to be disabled
@@ -578,19 +773,108 @@ void MainWindow::safeYamlFileTimerCallback(const ros::TimerEvent&)
+void MainWindow::on_load_demo_yaml_button_clicked()
+{
+ // Set the "load demo yaml" button to be disabled
+ ui->load_demo_yaml_button->setEnabled(false);
+
+ // Send a message requesting the parameters from the YAML
+ // file to be reloaded for the demo controller
+ std_msgs::Int32 msg;
+ msg.data = LOAD_YAML_DEMO_CONTROLLER_AGENT;
+ this->requestLoadControllerYamlPublisher.publish(msg);
+ ROS_INFO("Request load of demo 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_demo_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::demoYamlFileTimerCallback, this, true);
+}
+
+void MainWindow::demoYamlFileTimerCallback(const ros::TimerEvent&)
+{
+ // Enble the "load demo yaml" button again
+ ui->load_demo_yaml_button->setEnabled(true);
+}
+
+
+
+void MainWindow::on_load_student_yaml_button_clicked()
+{
+ // Set the "load student yaml" button to be disabled
+ ui->load_student_yaml_button->setEnabled(false);
+
+ // Send a message requesting the parameters from the YAML
+ // file to be reloaded for the student controller
+ std_msgs::Int32 msg;
+ msg.data = LOAD_YAML_STUDENT_CONTROLLER_AGENT;
+ this->requestLoadControllerYamlPublisher.publish(msg);
+ ROS_INFO("Request load of student 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_student_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::studentYamlFileTimerCallback, this, true);
+}
+
+void MainWindow::studentYamlFileTimerCallback(const ros::TimerEvent&)
+{
+ // Enble the "load student yaml" button again
+ ui->load_student_yaml_button->setEnabled(true);
+}
+
+
+void MainWindow::on_load_mpc_yaml_button_clicked()
+{
+ // Set the "load mpc yaml" button to be disabled
+ ui->load_mpc_yaml_button->setEnabled(false);
-void MainWindow::on_load_custom_yaml_button_clicked()
+ // Send a message requesting the parameters from the YAML
+ // file to be reloaded for the mpc controller
+ std_msgs::Int32 msg;
+ msg.data = LOAD_YAML_MPC_CONTROLLER_AGENT;
+ this->requestLoadControllerYamlPublisher.publish(msg);
+ ROS_INFO("Request load of mpc 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_mpc_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::mpcYamlFileTimerCallback, this, true);
+}
+
+void MainWindow::mpcYamlFileTimerCallback(const ros::TimerEvent&)
+{
+ // Enble the "load mpc yaml" button again
+ ui->load_mpc_yaml_button->setEnabled(true);
+}
+
+
+
+
+void MainWindow::on_load_remote_yaml_button_clicked()
{
- // Set the "load custom yaml" button to be disabled
- ui->load_custom_yaml_button->setEnabled(false);
+ // Set the "load remote yaml" button to be disabled
+ ui->load_remote_yaml_button->setEnabled(false);
// Send a message requesting the parameters from the YAML
- // file to be reloaded for the custom controller
+ // file to be reloaded for the remote controller
std_msgs::Int32 msg;
- msg.data = LOAD_YAML_CUSTOM_CONTROLLER_AGENT;
+ msg.data = LOAD_YAML_REMOTE_CONTROLLER_AGENT;
this->requestLoadControllerYamlPublisher.publish(msg);
- ROS_INFO("Request load of custom controller YAML published");
+ ROS_INFO("[STUDENT GUI] Request load of remote 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
@@ -599,17 +883,45 @@ void MainWindow::on_load_custom_yaml_button_clicked()
// > 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);
+ m_timer_yaml_file_for_remote_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::remoteYamlFileTimerCallback, this, true);
}
-void MainWindow::customYamlFileTimerCallback(const ros::TimerEvent&)
+void MainWindow::remoteYamlFileTimerCallback(const ros::TimerEvent&)
{
- // Enble the "load custom yaml" button again
- ui->load_custom_yaml_button->setEnabled(true);
+ // Enble the "load remote yaml" button again
+ ui->load_remote_yaml_button->setEnabled(true);
}
+void MainWindow::on_load_tuning_yaml_button_clicked()
+{
+ // Set the "load tuning yaml" button to be disabled
+ ui->load_tuning_yaml_button->setEnabled(false);
+
+ // Send a message requesting the parameters from the YAML
+ // file to be reloaded for the tuning controller
+ std_msgs::Int32 msg;
+ msg.data = LOAD_YAML_TUNING_CONTROLLER_AGENT;
+ this->requestLoadControllerYamlPublisher.publish(msg);
+ ROS_INFO("[STUDENT GUI] Request load of tuning 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_tuning_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::tuningYamlFileTimerCallback, this, true);
+}
+
+void MainWindow::tuningYamlFileTimerCallback(const ros::TimerEvent&)
+{
+ // Enble the "load tuning yaml" button again
+ ui->load_tuning_yaml_button->setEnabled(true);
+}
+
void MainWindow::requestLoadControllerYaml_from_my_GUI_Callback(const std_msgs::Int32& msg)
@@ -639,10 +951,70 @@ void MainWindow::requestLoadControllerYaml_from_my_GUI_Callback(const std_msgs::
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);
+ case LOAD_YAML_DEMO_CONTROLLER_AGENT:
+ case LOAD_YAML_DEMO_CONTROLLER_COORDINATOR:
+ // Set the "load demo yaml" button to be disabled
+ ui->load_demo_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_demo_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::demoYamlFileTimerCallback, this, true);
+
+ break;
+
+ case LOAD_YAML_STUDENT_CONTROLLER_AGENT:
+ case LOAD_YAML_STUDENT_CONTROLLER_COORDINATOR:
+ // Set the "load student yaml" button to be disabled
+ ui->load_student_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_student_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::studentYamlFileTimerCallback, this, true);
+
+ break;
+
+ case LOAD_YAML_MPC_CONTROLLER_AGENT:
+ case LOAD_YAML_MPC_CONTROLLER_COORDINATOR:
+ // Set the "load mpc yaml" button to be disabled
+ ui->load_mpc_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_mpc_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::mpcYamlFileTimerCallback, this, true);
+
+ break;
+
+ case LOAD_YAML_REMOTE_CONTROLLER_AGENT:
+ case LOAD_YAML_REMOTE_CONTROLLER_COORDINATOR:
+ // Set the "load remote yaml" button to be disabled
+ ui->load_remote_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_remote_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::remoteYamlFileTimerCallback, this, true);
+
+ break;
+
+ case LOAD_YAML_TUNING_CONTROLLER_AGENT:
+ case LOAD_YAML_TUNING_CONTROLLER_COORDINATOR:
+ // Set the "load tuning yaml" button to be disabled
+ ui->load_tuning_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
@@ -650,7 +1022,7 @@ void MainWindow::requestLoadControllerYaml_from_my_GUI_Callback(const std_msgs::
// 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);
+ m_timer_yaml_file_for_tuning_controller = nodeHandle.createTimer(ros::Duration(1.5), &MainWindow::tuningYamlFileTimerCallback, this, true);
break;
@@ -663,49 +1035,116 @@ void MainWindow::requestLoadControllerYaml_from_my_GUI_Callback(const std_msgs::
-void MainWindow::on_en_custom_controller_clicked()
+
+// # Enable controllers
+void MainWindow::on_enable_safe_controller_clicked()
{
std_msgs::Int32 msg;
- msg.data = CMD_USE_CUSTOM_CONTROLLER;
+ msg.data = CMD_USE_SAFE_CONTROLLER;
this->PPSClientCommandPublisher.publish(msg);
}
+void MainWindow::on_enable_demo_controller_clicked()
+{
+ std_msgs::Int32 msg;
+ msg.data = CMD_USE_DEMO_CONTROLLER;
+ this->PPSClientCommandPublisher.publish(msg);
+}
-void MainWindow::on_en_safe_controller_clicked()
+void MainWindow::on_enable_student_controller_clicked()
{
std_msgs::Int32 msg;
- msg.data = CMD_USE_SAFE_CONTROLLER;
+ msg.data = CMD_USE_STUDENT_CONTROLLER;
+ this->PPSClientCommandPublisher.publish(msg);
+}
+
+void MainWindow::on_enable_mpc_controller_clicked()
+{
+ std_msgs::Int32 msg;
+ msg.data = CMD_USE_MPC_CONTROLLER;
+ this->PPSClientCommandPublisher.publish(msg);
+}
+
+void MainWindow::on_enable_remote_controller_clicked()
+{
+ std_msgs::Int32 msg;
+ msg.data = CMD_USE_REMOTE_CONTROLLER;
this->PPSClientCommandPublisher.publish(msg);
}
-void MainWindow::on_customButton_1_clicked()
+void MainWindow::on_enable_tuning_controller_clicked()
+{
+ std_msgs::Int32 msg;
+ msg.data = CMD_USE_TUNING_CONTROLLER;
+ this->PPSClientCommandPublisher.publish(msg);
+}
+
+
+
+// # Custom command buttons - FOR DEMO CONTROLLER
+void MainWindow::on_demoButton_1_clicked()
+{
+ CustomButton msg_custom_button;
+ msg_custom_button.button_index = 1;
+ msg_custom_button.command_code = 0;
+ this->demoCustomButtonPublisher.publish(msg_custom_button);
+
+ ROS_INFO("Demo button 1 pressed in GUI");
+}
+
+void MainWindow::on_demoButton_2_clicked()
+{
+ CustomButton msg_custom_button;
+ msg_custom_button.button_index = 2;
+ msg_custom_button.command_code = 0;
+ this->demoCustomButtonPublisher.publish(msg_custom_button);
+ ROS_INFO("Demo button 2 pressed in GUI");
+}
+
+void MainWindow::on_demoButton_3_clicked()
+{
+ CustomButton msg_custom_button;
+ msg_custom_button.button_index = 3;
+ msg_custom_button.command_code = (ui->demoField_3->text()).toFloat();
+ this->demoCustomButtonPublisher.publish(msg_custom_button);
+ ROS_INFO("Demo button 3 pressed in GUI");
+}
+
+
+
+
+
+// # Custom command buttons - FOR STUDENT CONTROLLER
+void MainWindow::on_studentButton_1_clicked()
{
CustomButton msg_custom_button;
msg_custom_button.button_index = 1;
msg_custom_button.command_code = 0;
- this->PPSClientStudentCustomButtonPublisher.publish(msg_custom_button);
+ this->studentCustomButtonPublisher.publish(msg_custom_button);
- ROS_INFO("Custom button 1 pressed");
+ ROS_INFO("Student button 1 pressed in GUI");
}
-void MainWindow::on_customButton_2_clicked()
+void MainWindow::on_studentButton_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");
+ this->studentCustomButtonPublisher.publish(msg_custom_button);
+ ROS_INFO("Student button 2 pressed in GUI");
}
-void MainWindow::on_customButton_3_clicked()
+void MainWindow::on_studentButton_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");
+ msg_custom_button.command_code = (ui->studentField_3->text()).toFloat();
+ this->studentCustomButtonPublisher.publish(msg_custom_button);
+ ROS_INFO("Student button 3 pressed in GUI");
}
+
+
Setpoint MainWindow::correctSetpointBox(Setpoint setpoint, CrazyflieContext context)
{
Setpoint corrected_setpoint;
@@ -754,3 +1193,184 @@ bool MainWindow::setpointInsideBox(Setpoint setpoint, CrazyflieContext context)
return true;
}
+
+
+
+
+
+
+
+
+// # Custom buttons for the REMOTE controller service
+void MainWindow::on_remote_subscribe_button_clicked()
+{
+ // Initialise the message
+ ViconSubscribeObjectName msg;
+ // Set the subscribe flag
+ msg.shouldSubscribe = true;
+ // Set the object name
+ msg.objectName = (ui->remote_object_name->text()).toUtf8().constData();
+ // Publish the message
+ this->remoteSubscribePublisher.publish(msg);
+}
+
+void MainWindow::on_remote_unsubscribe_button_clicked()
+{
+ // Initialise the message
+ ViconSubscribeObjectName msg;
+ // Set the subscribe flag
+ msg.shouldSubscribe = false;
+ // Set the object name
+ msg.objectName = (ui->remote_object_name->text()).toUtf8().constData();
+ // Publish the message
+ this->remoteSubscribePublisher.publish(msg);
+}
+
+void MainWindow::on_remote_activate_button_clicked()
+{
+ // Initialise the message
+ std_msgs::Int32 msg;
+ // Set the msg data
+ msg.data = 1;
+ // Publish the message
+ this->remoteActivatePublisher.publish(msg);
+}
+
+void MainWindow::on_remote_deactivate_button_clicked()
+{
+ // Initialise the message
+ std_msgs::Int32 msg;
+ // Set the msg data
+ msg.data = 0;
+ // Publish the message
+ this->remoteActivatePublisher.publish(msg);
+}
+
+void MainWindow::remoteDataCallback(const CrazyflieData& objectData)
+{
+ // Check if the object is occluded
+ if (objectData.occluded)
+ {
+ // Set the column heading label to have a red background
+ // > IMPORTANT: Set the background auto fill property to true
+ ui->remote_data_label->setAutoFillBackground(true);
+ // > Get the pallette currently set for the label
+ QPalette pal = ui->remote_roll_label->palette();
+ // > Set the palette property that will change the background
+ pal.setColor(QPalette::Window, QColor(Qt::red));
+ // > Update the palette for the label
+ ui->remote_data_label->setPalette(pal);
+ }
+ else
+ {
+ // Put the roll, pitch, yaw, and z data into the appropriate fields
+ ui->remote_data_roll ->setText(QString::number( objectData.roll * RAD2DEG, 'f', 1));
+ ui->remote_data_pitch->setText(QString::number( objectData.pitch * RAD2DEG, 'f', 1));
+ ui->remote_data_yaw ->setText(QString::number( objectData.yaw * RAD2DEG, 'f', 1));
+ ui->remote_data_z ->setText(QString::number( objectData.z, 'f', 2));
+
+ // Set the column heading label to have a "normal" background
+ // > IMPORTANT: Set the background auto fill property to true
+ ui->remote_data_label->setAutoFillBackground(false);
+ // > Get the pallette currently set for the roll label
+ QPalette pal = ui->remote_roll_label->palette();
+ // > Update the palette for the column heading label
+ ui->remote_data_label->setPalette(pal);
+ }
+}
+
+void MainWindow::remoteControlSetpointCallback(const CrazyflieData& setpointData)
+{
+ ui->remote_setpoint_roll ->setText(QString::number( setpointData.roll * RAD2DEG, 'f', 1));
+ ui->remote_setpoint_pitch->setText(QString::number( setpointData.pitch * RAD2DEG, 'f', 1));
+ ui->remote_setpoint_yaw ->setText(QString::number( setpointData.yaw * RAD2DEG, 'f', 1));
+ ui->remote_setpoint_z ->setText(QString::number( setpointData.z, 'f', 2));
+}
+
+
+
+
+
+
+// TUNING CONTROLLER TAB
+void MainWindow::on_tuning_test_horizontal_button_clicked()
+{
+ // Initialise the message
+ std_msgs::Int32 msg;
+ // Set the msg data
+ msg.data = 1;
+ // Publish the message
+ this->tuningActivateTestPublisher.publish(msg);
+}
+
+void MainWindow::on_tuning_test_vertical_button_clicked()
+{
+ // Initialise the message
+ std_msgs::Int32 msg;
+ // Set the msg data
+ msg.data = 2;
+ // Publish the message
+ this->tuningActivateTestPublisher.publish(msg);
+}
+
+void MainWindow::on_tuning_test_heading_button_clicked()
+{
+ // Initialise the message
+ std_msgs::Int32 msg;
+ // Set the msg data
+ msg.data = 3;
+ // Publish the message
+ this->tuningActivateTestPublisher.publish(msg);
+}
+
+void MainWindow::on_tuning_test_all_button_clicked()
+{
+ // Initialise the message
+ std_msgs::Int32 msg;
+ // Set the msg data
+ msg.data = 4;
+ // Publish the message
+ this->tuningActivateTestPublisher.publish(msg);
+}
+
+void MainWindow::on_tuning_test_circle_button_clicked()
+{
+ // Initialise the message
+ std_msgs::Int32 msg;
+ // Set the msg data
+ msg.data = 5;
+ // Publish the message
+ this->tuningActivateTestPublisher.publish(msg);
+}
+
+void MainWindow::on_tuning_slider_horizontal_valueChanged(int value)
+{
+ // Initialise the message
+ std_msgs::Int32 msg;
+ // Set the msg data
+ msg.data = value;
+ // Publish the message
+ this->tuningHorizontalGainPublisher.publish(msg);
+}
+
+void MainWindow::on_tuning_slider_vertical_valueChanged(int value)
+{
+ // Initialise the message
+ std_msgs::Int32 msg;
+ // Set the msg data
+ msg.data = value;
+ // Publish the message
+ this->tuningVerticalGainPublisher.publish(msg);
+}
+
+void MainWindow::on_tuning_slider_heading_valueChanged(int value)
+{
+ // Initialise the message
+ std_msgs::Int32 msg;
+ // Set the msg data
+ msg.data = value;
+ // Publish the message
+ this->tuningHeadingGainPublisher.publish(msg);
+}
+
+
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.ui b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.ui
index 88cf08e5ce445fe31c88bde317df0329f9d9a7f2..2babdb4d720500c224343bf3fe36558f0a789b20 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.ui
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.ui
@@ -6,8 +6,8 @@
<rect>
<x>0</x>
<y>0</y>
- <width>1042</width>
- <height>701</height>
+ <width>1500</width>
+ <height>1000</height>
</rect>
</property>
<property name="sizePolicy">
@@ -27,483 +27,2209 @@
</property>
<widget class="QWidget" name="centralWidget">
<layout class="QGridLayout" name="gridLayout">
- <item row="4" column="1" colspan="2">
- <widget class="QGroupBox" name="groupBox_general">
- <property name="title">
- <string/>
+ <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>
- <layout class="QGridLayout" name="gridLayout_10">
- <item row="3" column="1">
- <widget class="QGroupBox" name="groupBox_4">
- <property name="title">
- <string/>
- </property>
- <layout class="QVBoxLayout" name="verticalLayout">
+ <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="QPushButton" name="motors_OFF_button">
+ <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="Minimum" vsizetype="Expanding">
+ <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>Motors OFF</string>
+ <string>Voltage: </string>
</property>
</widget>
</item>
<item>
- <widget class="QPushButton" name="take_off_button">
+ <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="Expanding">
+ <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>Take Off</string>
+ <string>Disconnect</string>
</property>
</widget>
</item>
<item>
- <widget class="QPushButton" name="land_button">
+ <widget class="QPushButton" name="RF_Connect_button">
<property name="sizePolicy">
- <sizepolicy hsizetype="Minimum" vsizetype="Expanding">
+ <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>Land</string>
+ <string>Connect</string>
</property>
</widget>
</item>
<item>
- <widget class="QLabel" name="label_battery">
+ <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>9</pointsize>
- <italic>true</italic>
+ <pointsize>16</pointsize>
</font>
</property>
<property name="text">
- <string/>
+ <string>Take Off</string>
</property>
</widget>
</item>
- </layout>
- </widget>
- </item>
- <item row="1" column="0">
- <widget class="QGroupBox" name="groupBox">
- <property name="sizePolicy">
- <sizepolicy hsizetype="Maximum" 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>
- <layout class="QGridLayout" name="gridLayout_2">
- <item row="1" column="0">
- <widget class="QLabel" name="raw_voltage_label">
+ <item>
+ <widget class="QPushButton" name="land_button">
<property name="sizePolicy">
- <sizepolicy hsizetype="Fixed" vsizetype="Preferred">
+ <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>18</pointsize>
- <italic>false</italic>
+ <pointsize>16</pointsize>
</font>
</property>
<property name="text">
- <string>Raw voltage: </string>
+ <string>Land</string>
</property>
</widget>
</item>
- <item row="1" column="1">
- <widget class="QLineEdit" name="voltage_field">
+ <item>
+ <widget class="QPushButton" name="motors_OFF_button">
<property name="sizePolicy">
- <sizepolicy hsizetype="Maximum" vsizetype="Fixed">
+ <sizepolicy hsizetype="Minimum" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="maximumSize">
<size>
- <width>80</width>
- <height>16777215</height>
+ <width>16777215</width>
+ <height>100</height>
</size>
</property>
- <property name="readOnly">
- <bool>true</bool>
+ <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>
- </widget>
- </item>
- <item row="1" column="1">
- <widget class="QLabel" name="flying_state_label">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Preferred">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="text">
- <string>FlyingState</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="3" column="0">
- <widget class="QTabWidget" name="tabWidget">
- <property name="currentIndex">
- <number>0</number>
- </property>
- <widget class="QWidget" name="tab_3">
- <attribute name="title">
- <string>Safe Controller</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>
+ </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>2</number>
+ </property>
+ <property name="usesScrollButtons">
+ <bool>true</bool>
+ </property>
+ <property name="tabsClosable">
+ <bool>false</bool>
+ </property>
+ <widget class="QWidget" name="tab_safe">
+ <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_safe">
+ <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_safe">
+ <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_safe">
+ <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_safe">
+ <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_safe">
+ <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_safe">
+ <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_safe">
+ <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_safe">
+ <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_safe">
+ <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_safe">
+ <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_safe">
+ <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_safe">
+ <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_safe">
+ <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_safe">
+ <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_safe">
+ <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_safe">
+ <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_safe">
+ <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_safe">
+ <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_safe">
+ <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_demo">
+ <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_demo">
+ <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_demo">
+ <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_demo">
+ <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_demo">
+ <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_demo">
+ <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_demo">
+ <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_demo">
+ <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_demo">
+ <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_demo">
+ <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_demo">
+ <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_demo">
+ <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_demo">
+ <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_demo">
+ <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_demo">
+ <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_demo">
+ <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_demo">
+ <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_demo">
+ <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_demo">
+ <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_demo">
+ <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="demoButton_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>Command 1</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="demoButton_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>Command 2</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="demoButton_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>Command 3</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLineEdit" name="demoField_3">
+ <property name="font">
+ <font>
+ <pointsize>14</pointsize>
+ </font>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </item>
+ </layout>
+ </widget>
+ <widget class="QWidget" name="tab_student">
+ <attribute name="title">
+ <string>Student</string>
+ </attribute>
+ <layout class="QGridLayout" name="gridLayout_11">
+ <item row="0" column="0">
+ <layout class="QGridLayout" name="gridLayout_6">
+ <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="15" column="1">
+ <widget class="QPushButton" name="studentButton_2">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>0</width>
+ <height>40</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>50</height>
+ </size>
</property>
- <property name="readOnly">
- <bool>true</bool>
+ <property name="text">
+ <string>Commnd 2</string>
</property>
</widget>
</item>
- <item row="2" column="1">
- <widget class="QLineEdit" name="current_y">
+ <item row="11" column="0">
+ <widget class="QLabel" name="label_25">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
- <property name="readOnly">
- <bool>true</bool>
+ <property name="font">
+ <font>
+ <pointsize>14</pointsize>
+ </font>
+ </property>
+ <property name="text">
+ <string>yaw [deg] =</string>
</property>
</widget>
</item>
- <item row="4" column="1">
- <widget class="QLineEdit" name="current_yaw">
+ <item row="1" column="2">
+ <widget class="QLabel" name="label_27">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
- <property name="readOnly">
- <bool>true</bool>
+ <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="3" column="1">
- <widget class="QLineEdit" name="current_z">
+ <item row="14" column="2">
+ <widget class="QPushButton" name="set_setpoint_button_student">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
- <property name="readOnly">
- <bool>true</bool>
+ <property name="font">
+ <font>
+ <pointsize>16</pointsize>
+ </font>
+ </property>
+ <property name="text">
+ <string>Set setpoint</string>
</property>
</widget>
</item>
- <item row="1" column="0">
- <widget class="QLabel" name="current_x_label">
+ <item row="1" column="1">
+ <widget class="QLabel" name="label_26">
<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>
+ <string>Current</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
</property>
</widget>
</item>
- <item row="3" column="0">
- <widget class="QLabel" name="current_z_label">
+ <item row="1" column="0">
+ <widget class="QLabel" name="label_28">
<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>
+ <string>Setpoint:</string>
</property>
</widget>
</item>
- <item row="2" column="0">
- <widget class="QLabel" name="current_y_label">
+ <item row="3" column="0">
+ <widget class="QLabel" name="label_29">
<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>
+ <string>x [m] =</string>
</property>
</widget>
</item>
- <item row="4" column="0">
- <widget class="QLabel" name="current_yaw_label">
+ <item row="9" column="0">
+ <widget class="QLabel" name="label_24">
<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>
+ <string>z [m] =</string>
</property>
</widget>
</item>
- <item row="1" column="1">
- <widget class="QLineEdit" name="current_x">
+ <item row="7" column="0">
+ <widget class="QLabel" name="label_23">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
- <property name="readOnly">
- <bool>true</bool>
+ <property name="font">
+ <font>
+ <pointsize>14</pointsize>
+ </font>
+ </property>
+ <property name="text">
+ <string>y [m] =</string>
</property>
</widget>
</item>
- <item row="5" column="0">
- <widget class="QLabel" name="current_pitch_label">
+ <item row="3" column="2">
+ <widget class="QLineEdit" name="new_setpoint_x_student">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
- <property name="text">
- <string>pitch [deg] =</string>
+ <property name="font">
+ <font>
+ <pointsize>14</pointsize>
+ </font>
</property>
</widget>
</item>
- <item row="6" column="0">
- <widget class="QLabel" name="current_roll_label">
+ <item row="11" column="2">
+ <widget class="QLineEdit" name="new_setpoint_yaw_student">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
- <property name="text">
- <string>roll [deg] =</string>
+ <property name="font">
+ <font>
+ <pointsize>14</pointsize>
+ </font>
</property>
</widget>
</item>
- <item row="5" column="1">
- <widget class="QLineEdit" name="current_pitch">
+ <item row="3" column="1">
+ <widget class="QLineEdit" name="current_setpoint_x_student">
<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">
+ <item row="7" column="1">
+ <widget class="QLineEdit" name="current_setpoint_y_student">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
- <property name="text">
- <string>Current</string>
+ <property name="font">
+ <font>
+ <pointsize>14</pointsize>
+ </font>
</property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
+ <property name="readOnly">
+ <bool>true</bool>
</property>
</widget>
</item>
- <item row="1" column="2">
- <widget class="QLineEdit" name="diff_x">
+ <item row="11" column="1">
+ <widget class="QLineEdit" name="current_setpoint_yaw_student">
<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">
+ <item row="9" column="1">
+ <widget class="QLineEdit" name="current_setpoint_z_student">
<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">
+ <item row="9" column="2">
+ <widget class="QLineEdit" name="new_setpoint_z_student">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
- <property name="readOnly">
- <bool>true</bool>
+ <property name="font">
+ <font>
+ <pointsize>14</pointsize>
+ </font>
</property>
</widget>
</item>
- <item row="4" column="2">
- <widget class="QLineEdit" name="diff_yaw">
+ <item row="7" column="2">
+ <widget class="QLineEdit" name="new_setpoint_y_student">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
- <property name="readOnly">
- <bool>true</bool>
+ <property name="font">
+ <font>
+ <pointsize>14</pointsize>
+ </font>
</property>
</widget>
</item>
- <item row="0" column="2">
- <widget class="QLabel" name="label_5">
+ <item row="15" column="0">
+ <widget class="QPushButton" name="studentButton_1">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
- <property name="text">
- <string>Difference</string>
+ <property name="minimumSize">
+ <size>
+ <width>0</width>
+ <height>40</height>
+ </size>
</property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Command 1</string>
</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">
+ <item row="15" column="2">
+ <widget class="QPushButton" name="studentButton_3">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
+ <property name="minimumSize">
+ <size>
+ <width>0</width>
+ <height>40</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Command 3</string>
+ </property>
</widget>
</item>
- <item row="4" column="2">
- <widget class="QLineEdit" name="new_setpoint_yaw">
+ <item row="15" column="3">
+ <widget class="QLineEdit" name="studentField_3">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
+ <property name="minimumSize">
+ <size>
+ <width>0</width>
+ <height>40</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>50</height>
+ </size>
+ </property>
</widget>
</item>
+ </layout>
+ </item>
+ </layout>
+ </widget>
+ <widget class="QWidget" name="tab_mpc">
+ <attribute name="title">
+ <string>MPC</string>
+ </attribute>
+ <layout class="QGridLayout" name="gridLayout_13">
+ <item row="0" column="0">
+ <layout class="QGridLayout" name="gridLayout_12">
+ <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="1" column="2">
- <widget class="QLineEdit" name="new_setpoint_x">
+ <widget class="QLabel" name="label_30">
<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="2" column="1">
- <widget class="QLineEdit" name="current_setpoint_y">
+ <item row="14" column="2">
+ <widget class="QPushButton" name="set_setpoint_button_mpc">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</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="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 name="font">
+ <font>
+ <pointsize>14</pointsize>
+ </font>
</property>
<property name="text">
- <string>yaw [deg] =</string>
+ <string>Set setpoint</string>
</property>
</widget>
</item>
- <item row="0" column="1">
- <widget class="QLabel" name="label_12">
+ <item row="1" column="1">
+ <widget class="QLabel" name="label_31">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
@@ -512,7 +2238,7 @@
</property>
<property name="font">
<font>
- <pointsize>7</pointsize>
+ <pointsize>14</pointsize>
</font>
</property>
<property name="text">
@@ -524,72 +2250,79 @@
</widget>
</item>
<item row="1" column="0">
- <widget class="QLabel" name="label_7">
+ <widget class="QLabel" name="label_32">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</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 name="font">
+ <font>
+ <pointsize>14</pointsize>
+ </font>
</property>
- <property name="readOnly">
- <bool>true</bool>
+ <property name="text">
+ <string>Setpoint:</string>
</property>
</widget>
</item>
<item row="3" column="0">
- <widget class="QLabel" name="label_9">
+ <widget class="QLabel" name="label_33">
<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>
+ <string>x [m] =</string>
</property>
</widget>
</item>
- <item row="2" column="0">
- <widget class="QLabel" name="label_8">
+ <item row="11" column="0">
+ <widget class="QLabel" name="label_34">
<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>
+ <string>yaw [deg] =</string>
</property>
</widget>
</item>
- <item row="4" column="1">
- <widget class="QLineEdit" name="current_setpoint_yaw">
+ <item row="9" column="0">
+ <widget class="QLabel" name="label_35">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
- <property name="readOnly">
- <bool>true</bool>
+ <property name="font">
+ <font>
+ <pointsize>14</pointsize>
+ </font>
+ </property>
+ <property name="text">
+ <string>z [m] =</string>
</property>
</widget>
</item>
- <item row="5" column="2">
- <widget class="QPushButton" name="set_setpoint_button">
+ <item row="7" column="0">
+ <widget class="QLabel" name="label_36">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
@@ -598,39 +2331,46 @@
</property>
<property name="font">
<font>
- <pointsize>7</pointsize>
+ <pointsize>14</pointsize>
</font>
</property>
<property name="text">
- <string>Set setpoint</string>
+ <string>y [m] =</string>
</property>
</widget>
</item>
<item row="3" column="2">
- <widget class="QLineEdit" name="new_setpoint_z">
+ <widget class="QLineEdit" name="new_setpoint_x_mpc">
<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">
+ <item row="11" column="2">
+ <widget class="QLineEdit" name="new_setpoint_yaw_mpc">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
- <property name="text">
- <string>Setpoint:</string>
+ <property name="font">
+ <font>
+ <pointsize>14</pointsize>
+ </font>
</property>
</widget>
</item>
- <item row="0" column="2">
- <widget class="QLabel" name="label_13">
+ <item row="3" column="1">
+ <widget class="QLineEdit" name="current_setpoint_x_mpc">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
@@ -639,660 +2379,1416 @@
</property>
<property name="font">
<font>
- <pointsize>7</pointsize>
+ <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 name="readOnly">
+ <bool>true</bool>
</property>
</widget>
</item>
- </layout>
- </widget>
- </item>
- <item row="1" column="1">
- <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="text">
- <string>Enable Safe Controller</string>
- </property>
- </widget>
- </item>
- <item row="1" column="0">
- <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="text">
- <string>Load Safecontroller YAML file</string>
- </property>
- </widget>
- </item>
- </layout>
- </widget>
- <widget class="QWidget" name="tab_4">
- <attribute name="title">
- <string>Custom Controller</string>
- </attribute>
- <layout class="QGridLayout" name="gridLayout_9">
- <item row="2" column="1">
- <widget class="QPushButton" name="customButton_2">
- <property name="text">
- <string>Custom Command 2</string>
- </property>
- </widget>
- </item>
- <item row="3" column="1">
- <widget class="QPushButton" name="customButton_3">
- <property name="text">
- <string>Custom Command 3</string>
- </property>
- </widget>
- </item>
- <item row="1" column="1">
- <widget class="QPushButton" name="customButton_1">
- <property name="text">
- <string>Custom Command 1</string>
- </property>
- </widget>
- </item>
- <item row="0" column="0">
- <widget class="QGroupBox" name="groupBox_5">
- <property name="title">
- <string/>
- </property>
- <layout class="QGridLayout" name="gridLayout_7">
- <item row="6" column="1">
- <widget class="QLineEdit" name="current_roll_2">
+ <item row="7" column="1">
+ <widget class="QLineEdit" name="current_setpoint_y_mpc">
<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">
+ <item row="11" column="1">
+ <widget class="QLineEdit" name="current_setpoint_yaw_mpc">
<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">
+ <item row="9" column="1">
+ <widget class="QLineEdit" name="current_setpoint_z_mpc">
<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">
+ <item row="9" column="2">
+ <widget class="QLineEdit" name="new_setpoint_z_mpc">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
- <property name="readOnly">
- <bool>true</bool>
+ <property name="font">
+ <font>
+ <pointsize>14</pointsize>
+ </font>
</property>
</widget>
</item>
- <item row="1" column="0">
- <widget class="QLabel" name="current_x_label_2">
+ <item row="7" column="2">
+ <widget class="QLineEdit" name="new_setpoint_y_mpc">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
- <property name="text">
- <string>x [m] =</string>
+ <property name="font">
+ <font>
+ <pointsize>14</pointsize>
+ </font>
</property>
</widget>
</item>
- <item row="3" column="0">
- <widget class="QLabel" name="current_z_label_2">
+ </layout>
+ </item>
+ </layout>
+ </widget>
+ <widget class="QWidget" name="tab_remote">
+ <attribute name="title">
+ <string>Remote</string>
+ </attribute>
+ <layout class="QGridLayout" name="gridLayout_14">
+ <item row="0" column="0">
+ <layout class="QGridLayout" name="gridLayout_10">
+ <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="0">
+ <widget class="QLineEdit" name="remote_object_name">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
- <property name="text">
- <string>z [m] =</string>
- </property>
- </widget>
- </item>
- <item row="2" column="0">
- <widget class="QLabel" name="current_y_label_2">
- <property name="text">
- <string>y [m] =</string>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>50</height>
+ </size>
</property>
</widget>
</item>
- <item row="4" column="0">
- <widget class="QLabel" name="current_yaw_label_2">
+ <item row="5" column="1">
+ <widget class="QLineEdit" name="remote_data_pitch">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
- <property name="text">
- <string>yaw [deg] = </string>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Monospace</family>
+ </font>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
</property>
</widget>
</item>
- <item row="1" column="1">
- <widget class="QLineEdit" name="current_x_2">
+ <item row="0" column="2">
+ <widget class="QPushButton" name="remote_unsubscribe_button">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
- <property name="readOnly">
- <bool>true</bool>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>UN-subscribe</string>
</property>
</widget>
</item>
- <item row="5" column="0">
- <widget class="QLabel" name="current_pitch_label_2">
+ <item row="7" column="2">
+ <widget class="QLineEdit" name="remote_setpoint_z">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
- <property name="text">
- <string>pitch [deg] =</string>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Monospace</family>
+ </font>
</property>
</widget>
</item>
- <item row="6" column="0">
- <widget class="QLabel" name="current_roll_label_2">
+ <item row="0" column="1">
+ <widget class="QPushButton" name="remote_subscribe_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="text">
- <string>roll [deg] =</string>
+ <string>Subscribe</string>
</property>
</widget>
</item>
- <item row="5" column="1">
- <widget class="QLineEdit" name="current_pitch_2">
+ <item row="7" column="1">
+ <widget class="QLineEdit" name="remote_data_z">
<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>
+ <family>Monospace</family>
+ </font>
+ </property>
<property name="readOnly">
<bool>true</bool>
</property>
</widget>
</item>
- <item row="0" column="1">
- <widget class="QLabel" name="label_6">
+ <item row="6" column="2">
+ <widget class="QLineEdit" name="remote_setpoint_yaw">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
- <property name="text">
- <string>Current</string>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>50</height>
+ </size>
</property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
+ <property name="font">
+ <font>
+ <family>Monospace</family>
+ </font>
</property>
</widget>
</item>
- <item row="1" column="2">
- <widget class="QLineEdit" name="diff_x_2">
+ <item row="6" column="1">
+ <widget class="QLineEdit" name="remote_data_yaw">
<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>
+ <family>Monospace</family>
+ </font>
+ </property>
<property name="readOnly">
<bool>true</bool>
</property>
</widget>
</item>
- <item row="2" column="2">
- <widget class="QLineEdit" name="diff_y_2">
+ <item row="1" column="2">
+ <widget class="Line" name="line_12">
+ <property name="lineWidth">
+ <number>5</number>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="0">
+ <widget class="Line" name="line_10">
+ <property name="frameShadow">
+ <enum>QFrame::Sunken</enum>
+ </property>
+ <property name="lineWidth">
+ <number>5</number>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="2">
+ <widget class="QLineEdit" name="remote_setpoint_pitch">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
- <property name="readOnly">
- <bool>true</bool>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Monospace</family>
+ </font>
</property>
</widget>
</item>
- <item row="3" column="2">
- <widget class="QLineEdit" name="diff_z_2">
+ <item row="4" column="1">
+ <widget class="QLineEdit" name="remote_data_roll">
<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>
+ <family>Monospace</family>
+ </font>
+ </property>
<property name="readOnly">
<bool>true</bool>
</property>
</widget>
</item>
+ <item row="1" column="1">
+ <widget class="Line" name="line_11">
+ <property name="lineWidth">
+ <number>5</number>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
<item row="4" column="2">
- <widget class="QLineEdit" name="diff_yaw_2">
+ <widget class="QLineEdit" name="remote_setpoint_roll">
<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>
+ <family>Monospace</family>
+ </font>
+ </property>
<property name="readOnly">
<bool>true</bool>
</property>
</widget>
</item>
- <item row="0" column="2">
- <widget class="QLabel" name="label_10">
+ <item row="2" column="1">
+ <widget class="QLabel" name="remote_data_label">
<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="text">
- <string>Difference</string>
+ <string>Remote</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_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>
+ <widget class="QLabel" name="label_38">
+ <property name="text">
+ <string>Setpoint</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
</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>
+ <item row="4" column="0">
+ <widget class="QLabel" name="remote_roll_label">
+ <property name="text">
+ <string>Roll [deg]</string>
</property>
</widget>
</item>
- <item row="1" column="2">
- <widget class="QLineEdit" name="new_setpoint_x_2">
+ <item row="5" column="0">
+ <widget class="QLabel" name="remote_pitch_label">
+ <property name="text">
+ <string>Pitch [deg]</string>
+ </property>
+ </widget>
+ </item>
+ <item row="6" column="0">
+ <widget class="QLabel" name="remote_yaw_label">
+ <property name="text">
+ <string>Yaw [deg]</string>
+ </property>
+ </widget>
+ </item>
+ <item row="7" column="0">
+ <widget class="QLabel" name="remote_z_label">
+ <property name="text">
+ <string>z [m]</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="0" column="1">
+ <layout class="QVBoxLayout" name="verticalLayout_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>
+ <spacer name="verticalSpacer_2">
+ <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>
+ <widget class="QPushButton" name="remote_activate_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>100</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Activate</string>
+ </property>
</widget>
</item>
- <item row="2" column="1">
- <widget class="QLineEdit" name="current_setpoint_y_2">
+ <item>
+ <widget class="QPushButton" name="remote_deactivate_button">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
- <property name="readOnly">
- <bool>true</bool>
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>100</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>DE-activate</string>
</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>
+ <item>
+ <spacer name="verticalSpacer_3">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
</property>
- <property name="readOnly">
- <bool>true</bool>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>20</width>
+ <height>40</height>
+ </size>
</property>
- </widget>
+ </spacer>
</item>
+ </layout>
+ </item>
+ </layout>
+ </widget>
+ <widget class="QWidget" name="tab">
+ <attribute name="title">
+ <string>Tuning</string>
+ </attribute>
+ <layout class="QGridLayout" name="gridLayout_16">
+ <item row="0" column="0">
+ <layout class="QGridLayout" name="gridLayout_15">
+ <property name="leftMargin">
+ <number>6</number>
+ </property>
+ <property name="topMargin">
+ <number>0</number>
+ </property>
+ <property name="rightMargin">
+ <number>6</number>
+ </property>
+ <property name="bottomMargin">
+ <number>0</number>
+ </property>
<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>
+ <widget class="QLabel" name="label_45">
<property name="text">
- <string>yaw [deg] =</string>
+ <string>Vertical Controller Gain (Vertikal Regler Verstaekung)</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>
+ <item row="2" column="0">
+ <widget class="Line" name="line_13">
+ <property name="lineWidth">
+ <number>5</number>
</property>
- <property name="font">
- <font>
- <pointsize>7</pointsize>
- </font>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
</property>
- <property name="text">
- <string>Current</string>
+ </widget>
+ </item>
+ <item row="2" column="1">
+ <widget class="Line" name="line_15">
+ <property name="lineWidth">
+ <number>5</number>
</property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
</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>
+ <item row="11" column="0">
+ <spacer name="verticalSpacer_4">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
</property>
- <property name="text">
- <string>x [m] =</string>
+ <property name="sizeType">
+ <enum>QSizePolicy::Minimum</enum>
</property>
- </widget>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>20</width>
+ <height>10</height>
+ </size>
+ </property>
+ </spacer>
</item>
- <item row="3" column="0">
- <widget class="QLabel" name="label_17">
+ <item row="1" column="1">
+ <widget class="QPushButton" name="tuning_test_horizontal_button">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
+ <property name="maximumSize">
+ <size>
+ <width>160</width>
+ <height>50</height>
+ </size>
+ </property>
<property name="text">
- <string>z [m] =</string>
+ <string>Test</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>
+ <item row="6" column="1">
+ <widget class="Line" name="line_16">
+ <property name="lineWidth">
+ <number>5</number>
</property>
- <property name="text">
- <string>y [m] =</string>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
</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>
+ <item row="8" column="0">
+ <widget class="QLabel" name="label_46">
+ <property name="text">
+ <string>Heading Controller Gain (Orientierung Regler Verstaekung)</string>
+ </property>
+ </widget>
+ </item>
+ <item row="12" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_10">
+ <property name="leftMargin">
+ <number>6</number>
+ </property>
+ <property name="topMargin">
+ <number>0</number>
+ </property>
+ <property name="rightMargin">
+ <number>6</number>
+ </property>
+ <property name="bottomMargin">
+ <number>0</number>
+ </property>
+ <item>
+ <widget class="QPushButton" name="tuning_test_all_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>0</width>
+ <height>0</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>340</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Test All</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="tuning_test_circle_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>340</width>
+ <height>50</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Fly a circle</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="6" column="0">
+ <widget class="Line" name="line_14">
+ <property name="lineWidth">
+ <number>5</number>
</property>
- <property name="readOnly">
- <bool>true</bool>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
</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>
+ <item row="3" column="0">
+ <spacer name="verticalSpacer_5">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
</property>
- <property name="font">
- <font>
- <pointsize>7</pointsize>
- </font>
+ <property name="sizeType">
+ <enum>QSizePolicy::Minimum</enum>
</property>
- <property name="text">
- <string>Set setpoint</string>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>20</width>
+ <height>5</height>
+ </size>
</property>
- </widget>
+ </spacer>
</item>
- <item row="4" column="1">
- <widget class="QLineEdit" name="current_setpoint_yaw_2">
+ <item row="9" column="1">
+ <widget class="QPushButton" name="tuning_test_heading_button">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
- <property name="readOnly">
- <bool>true</bool>
+ <property name="maximumSize">
+ <size>
+ <width>160</width>
+ <height>50</height>
+ </size>
</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 name="text">
+ <string>Test</string>
</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>
+ <widget class="QLabel" name="label_37">
<property name="text">
- <string>Setpoint:</string>
+ <string>Horizontal Controller Gain (Horizontal Regler Verstaekung)</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
</property>
</widget>
</item>
- <item row="0" column="2">
- <widget class="QLabel" name="label_20">
+ <item row="5" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_4">
+ <property name="leftMargin">
+ <number>6</number>
+ </property>
+ <property name="rightMargin">
+ <number>6</number>
+ </property>
+ <item>
+ <widget class="QLabel" name="label_39">
+ <property name="text">
+ <string>Min</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QSlider" name="tuning_slider_vertical">
+ <property name="maximum">
+ <number>1000</number>
+ </property>
+ <property name="value">
+ <number>100</number>
+ </property>
+ <property name="tracking">
+ <bool>false</bool>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="tickPosition">
+ <enum>QSlider::TicksAbove</enum>
+ </property>
+ <property name="tickInterval">
+ <number>100</number>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="label_40">
+ <property name="text">
+ <string>Max</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="5" column="1">
+ <widget class="QPushButton" name="tuning_test_vertical_button">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
- <property name="font">
- <font>
- <pointsize>7</pointsize>
- </font>
+ <property name="maximumSize">
+ <size>
+ <width>160</width>
+ <height>50</height>
+ </size>
</property>
<property name="text">
- <string>New</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
+ <string>Test</string>
+ </property>
+ </widget>
+ </item>
+ <item row="7" column="0">
+ <spacer name="verticalSpacer_6">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ <property name="sizeType">
+ <enum>QSizePolicy::Minimum</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>20</width>
+ <height>5</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item row="9" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_6">
+ <property name="leftMargin">
+ <number>6</number>
+ </property>
+ <property name="rightMargin">
+ <number>6</number>
+ </property>
+ <item>
+ <widget class="QLabel" name="label_41">
+ <property name="text">
+ <string>Min</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QSlider" name="tuning_slider_heading">
+ <property name="maximum">
+ <number>1000</number>
+ </property>
+ <property name="value">
+ <number>100</number>
+ </property>
+ <property name="tracking">
+ <bool>false</bool>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="tickPosition">
+ <enum>QSlider::TicksAbove</enum>
+ </property>
+ <property name="tickInterval">
+ <number>100</number>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="label_42">
+ <property name="text">
+ <string>Max</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="1" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_7">
+ <property name="leftMargin">
+ <number>6</number>
+ </property>
+ <property name="rightMargin">
+ <number>6</number>
+ </property>
+ <item>
+ <widget class="QLabel" name="label_43">
+ <property name="text">
+ <string>Min</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QSlider" name="tuning_slider_horizontal">
+ <property name="maximum">
+ <number>1000</number>
+ </property>
+ <property name="value">
+ <number>100</number>
+ </property>
+ <property name="tracking">
+ <bool>false</bool>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="tickPosition">
+ <enum>QSlider::TicksAbove</enum>
+ </property>
+ <property name="tickInterval">
+ <number>100</number>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QLabel" name="label_44">
+ <property name="text">
+ <string>Max</string>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="10" column="0">
+ <widget class="Line" name="line_17">
+ <property name="lineWidth">
+ <number>5</number>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="10" column="1">
+ <widget class="Line" name="line_18">
+ <property name="lineWidth">
+ <number>5</number>
+ </property>
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
</property>
</widget>
</item>
</layout>
- </widget>
- </item>
- <item row="4" column="1">
- <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="text">
- <string>Enable Custom Controller</string>
- </property>
- </widget>
- </item>
- <item row="4" column="0">
- <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="text">
- <string>Load CustomController YAML file</string>
- </property>
- </widget>
- </item>
- <item row="3" column="2">
- <widget class="QLineEdit" name="custom_command_3"/>
- </item>
- </layout>
+ </item>
+ </layout>
+ </widget>
</widget>
- </widget>
- </item>
- </layout>
- </widget>
- </item>
- <item row="1" column="1">
- <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="text">
- <string>Disconnect from Crazyflie</string>
- </property>
- </widget>
- </item>
- <item row="0" column="1">
- <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="text">
- <string>Connect to Crazyflie</string>
- </property>
- </widget>
- </item>
- <item row="1" column="2">
- <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="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="6" column="2">
+ <widget class="QPushButton" name="load_mpc_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>MPC</string>
+ </property>
+ </widget>
+ </item>
+ <item row="7" column="2">
+ <widget class="QPushButton" name="load_remote_yaml_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>0</width>
+ <height>0</height>
+ </size>
+ </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>Remote</string>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="2">
+ <widget class="QPushButton" name="load_student_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>Student</string>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="0">
+ <widget class="QPushButton" name="enable_student_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>Student</string>
+ </property>
+ </widget>
+ </item>
+ <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="enable_demo_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="9" 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="enable_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_demo_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="4" column="1">
+ <widget class="Line" name="line_4">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </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="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>
+ <item row="9" column="1">
+ <widget class="Line" name="line_5">
+ <property name="orientation">
+ <enum>Qt::Vertical</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="6" column="0">
+ <widget class="QPushButton" name="enable_mpc_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>MPC</string>
+ </property>
+ </widget>
+ </item>
+ <item row="7" column="0">
+ <widget class="QPushButton" name="enable_remote_controller">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>0</width>
+ <height>0</height>
+ </size>
+ </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>Remote</string>
+ </property>
+ </widget>
+ </item>
+ <item row="8" column="0">
+ <widget class="QPushButton" name="enable_tuning_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>Tuning</string>
+ </property>
+ </widget>
+ </item>
+ <item row="8" column="2">
+ <widget class="QPushButton" name="load_tuning_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>Tuning</string>
+ </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>
@@ -1301,8 +3797,8 @@
<rect>
<x>0</x>
<y>0</y>
- <width>1042</width>
- <height>25</height>
+ <width>1500</width>
+ <height>37</height>
</rect>
</property>
</widget>
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/studentGUI.pro.user.1400dcd b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/studentGUI.pro.user.1400dcd
new file mode 100644
index 0000000000000000000000000000000000000000..8e1edbbfa62e3ca2e0c55ceaefa53f5f983a47e2
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/studentGUI.pro.user.1400dcd
@@ -0,0 +1,271 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE QtCreatorProject>
+<!-- Written by QtCreator 3.5.1, 2017-10-18T15:20:06. -->
+<qtcreator>
+ <data>
+ <variable>EnvironmentId</variable>
+ <value type="QByteArray">{1400dcd4-82c6-466c-a808-34f7a3d4fe21}</value>
+ </data>
+ <data>
+ <variable>ProjectExplorer.Project.ActiveTarget</variable>
+ <value type="int">0</value>
+ </data>
+ <data>
+ <variable>ProjectExplorer.Project.EditorSettings</variable>
+ <valuemap type="QVariantMap">
+ <value type="bool" key="EditorConfiguration.AutoIndent">true</value>
+ <value type="bool" key="EditorConfiguration.AutoSpacesForTabs">false</value>
+ <value type="bool" key="EditorConfiguration.CamelCaseNavigation">true</value>
+ <valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.0">
+ <value type="QString" key="language">Cpp</value>
+ <valuemap type="QVariantMap" key="value">
+ <value type="QByteArray" key="CurrentPreferences">CppGlobal</value>
+ </valuemap>
+ </valuemap>
+ <valuemap type="QVariantMap" key="EditorConfiguration.CodeStyle.1">
+ <value type="QString" key="language">QmlJS</value>
+ <valuemap type="QVariantMap" key="value">
+ <value type="QByteArray" key="CurrentPreferences">QmlJSGlobal</value>
+ </valuemap>
+ </valuemap>
+ <value type="int" key="EditorConfiguration.CodeStyle.Count">2</value>
+ <value type="QByteArray" key="EditorConfiguration.Codec">UTF-8</value>
+ <value type="bool" key="EditorConfiguration.ConstrainTooltips">false</value>
+ <value type="int" key="EditorConfiguration.IndentSize">4</value>
+ <value type="bool" key="EditorConfiguration.KeyboardTooltips">false</value>
+ <value type="int" key="EditorConfiguration.MarginColumn">80</value>
+ <value type="bool" key="EditorConfiguration.MouseHiding">true</value>
+ <value type="bool" key="EditorConfiguration.MouseNavigation">true</value>
+ <value type="int" key="EditorConfiguration.PaddingMode">1</value>
+ <value type="bool" key="EditorConfiguration.ScrollWheelZooming">true</value>
+ <value type="bool" key="EditorConfiguration.ShowMargin">false</value>
+ <value type="int" key="EditorConfiguration.SmartBackspaceBehavior">0</value>
+ <value type="bool" key="EditorConfiguration.SpacesForTabs">true</value>
+ <value type="int" key="EditorConfiguration.TabKeyBehavior">0</value>
+ <value type="int" key="EditorConfiguration.TabSize">8</value>
+ <value type="bool" key="EditorConfiguration.UseGlobal">true</value>
+ <value type="int" key="EditorConfiguration.Utf8BomBehavior">1</value>
+ <value type="bool" key="EditorConfiguration.addFinalNewLine">true</value>
+ <value type="bool" key="EditorConfiguration.cleanIndentation">true</value>
+ <value type="bool" key="EditorConfiguration.cleanWhitespace">true</value>
+ <value type="bool" key="EditorConfiguration.inEntireDocument">false</value>
+ </valuemap>
+ </data>
+ <data>
+ <variable>ProjectExplorer.Project.PluginSettings</variable>
+ <valuemap type="QVariantMap"/>
+ </data>
+ <data>
+ <variable>ProjectExplorer.Project.Target.0</variable>
+ <valuemap type="QVariantMap">
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Desktop</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Desktop</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">{051621a5-413a-4a38-907c-a6d036ac454e}</value>
+ <value type="int" key="ProjectExplorer.Target.ActiveBuildConfiguration">0</value>
+ <value type="int" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
+ <value type="int" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
+ <valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.0">
+ <value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/ppsteacher/work/D-FaLL-System/pps_ws/src/d_fall_pps/GUI_Qt/build-studentGUI-Desktop-Debug</value>
+ <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
+ <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
+ <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">qmake</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">QtProjectManager.QMakeBuildStep</value>
+ <value type="bool" key="QtProjectManager.QMakeBuildStep.LinkQmlDebuggingLibrary">false</value>
+ <value type="bool" key="QtProjectManager.QMakeBuildStep.LinkQmlDebuggingLibraryAuto">true</value>
+ <value type="QString" key="QtProjectManager.QMakeBuildStep.QMakeArguments"></value>
+ <value type="bool" key="QtProjectManager.QMakeBuildStep.QMakeForced">false</value>
+ <value type="bool" key="QtProjectManager.QMakeBuildStep.SeparateDebugInfo">false</value>
+ <value type="bool" key="QtProjectManager.QMakeBuildStep.UseQtQuickCompiler">false</value>
+ </valuemap>
+ <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.1">
+ <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.MakeStep</value>
+ <valuelist type="QVariantList" key="Qt4ProjectManager.MakeStep.AutomaticallyAddedMakeArguments">
+ <value type="QString">-w</value>
+ <value type="QString">-r</value>
+ </valuelist>
+ <value type="bool" key="Qt4ProjectManager.MakeStep.Clean">false</value>
+ <value type="QString" key="Qt4ProjectManager.MakeStep.MakeArguments"></value>
+ <value type="QString" key="Qt4ProjectManager.MakeStep.MakeCommand"></value>
+ </valuemap>
+ <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">2</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Build</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
+ </valuemap>
+ <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
+ <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
+ <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.MakeStep</value>
+ <valuelist type="QVariantList" key="Qt4ProjectManager.MakeStep.AutomaticallyAddedMakeArguments">
+ <value type="QString">-w</value>
+ <value type="QString">-r</value>
+ </valuelist>
+ <value type="bool" key="Qt4ProjectManager.MakeStep.Clean">true</value>
+ <value type="QString" key="Qt4ProjectManager.MakeStep.MakeArguments">clean</value>
+ <value type="QString" key="Qt4ProjectManager.MakeStep.MakeCommand"></value>
+ </valuemap>
+ <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Clean</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
+ </valuemap>
+ <value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
+ <value type="bool" key="ProjectExplorer.BuildConfiguration.ClearSystemEnvironment">false</value>
+ <valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.UserEnvironmentChanges"/>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Debug</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.Qt4BuildConfiguration</value>
+ <value type="int" key="Qt4ProjectManager.Qt4BuildConfiguration.BuildConfiguration">2</value>
+ <value type="bool" key="Qt4ProjectManager.Qt4BuildConfiguration.UseShadowBuild">true</value>
+ </valuemap>
+ <valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.1">
+ <value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/home/ppsteacher/work/D-FaLL-System/pps_ws/src/d_fall_pps/GUI_Qt/build-studentGUI-Desktop-Release</value>
+ <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
+ <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
+ <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">qmake</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">QtProjectManager.QMakeBuildStep</value>
+ <value type="bool" key="QtProjectManager.QMakeBuildStep.LinkQmlDebuggingLibrary">false</value>
+ <value type="bool" key="QtProjectManager.QMakeBuildStep.LinkQmlDebuggingLibraryAuto">true</value>
+ <value type="QString" key="QtProjectManager.QMakeBuildStep.QMakeArguments"></value>
+ <value type="bool" key="QtProjectManager.QMakeBuildStep.QMakeForced">false</value>
+ <value type="bool" key="QtProjectManager.QMakeBuildStep.SeparateDebugInfo">false</value>
+ <value type="bool" key="QtProjectManager.QMakeBuildStep.UseQtQuickCompiler">false</value>
+ </valuemap>
+ <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.1">
+ <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.MakeStep</value>
+ <valuelist type="QVariantList" key="Qt4ProjectManager.MakeStep.AutomaticallyAddedMakeArguments">
+ <value type="QString">-w</value>
+ <value type="QString">-r</value>
+ </valuelist>
+ <value type="bool" key="Qt4ProjectManager.MakeStep.Clean">false</value>
+ <value type="QString" key="Qt4ProjectManager.MakeStep.MakeArguments"></value>
+ <value type="QString" key="Qt4ProjectManager.MakeStep.MakeCommand"></value>
+ </valuemap>
+ <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">2</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Build</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Build</value>
+ </valuemap>
+ <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.1">
+ <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
+ <value type="bool" key="ProjectExplorer.BuildStep.Enabled">true</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Make</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.MakeStep</value>
+ <valuelist type="QVariantList" key="Qt4ProjectManager.MakeStep.AutomaticallyAddedMakeArguments">
+ <value type="QString">-w</value>
+ <value type="QString">-r</value>
+ </valuelist>
+ <value type="bool" key="Qt4ProjectManager.MakeStep.Clean">true</value>
+ <value type="QString" key="Qt4ProjectManager.MakeStep.MakeArguments">clean</value>
+ <value type="QString" key="Qt4ProjectManager.MakeStep.MakeCommand"></value>
+ </valuemap>
+ <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">1</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Clean</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Clean</value>
+ </valuemap>
+ <value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">2</value>
+ <value type="bool" key="ProjectExplorer.BuildConfiguration.ClearSystemEnvironment">false</value>
+ <valuelist type="QVariantList" key="ProjectExplorer.BuildConfiguration.UserEnvironmentChanges"/>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Release</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.Qt4BuildConfiguration</value>
+ <value type="int" key="Qt4ProjectManager.Qt4BuildConfiguration.BuildConfiguration">0</value>
+ <value type="bool" key="Qt4ProjectManager.Qt4BuildConfiguration.UseShadowBuild">true</value>
+ </valuemap>
+ <value type="int" key="ProjectExplorer.Target.BuildConfigurationCount">2</value>
+ <valuemap type="QVariantMap" key="ProjectExplorer.Target.DeployConfiguration.0">
+ <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
+ <value type="int" key="ProjectExplorer.BuildStepList.StepsCount">0</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.BuildSteps.Deploy</value>
+ </valuemap>
+ <value type="int" key="ProjectExplorer.BuildConfiguration.BuildStepListCount">1</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Deploy locally</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">ProjectExplorer.DefaultDeployConfiguration</value>
+ </valuemap>
+ <value type="int" key="ProjectExplorer.Target.DeployConfigurationCount">1</value>
+ <valuemap type="QVariantMap" key="ProjectExplorer.Target.PluginSettings"/>
+ <valuemap type="QVariantMap" key="ProjectExplorer.Target.RunConfiguration.0">
+ <valuelist type="QVariantList" key="Analyzer.Valgrind.AddedSuppressionFiles"/>
+ <value type="bool" key="Analyzer.Valgrind.Callgrind.CollectBusEvents">false</value>
+ <value type="bool" key="Analyzer.Valgrind.Callgrind.CollectSystime">false</value>
+ <value type="bool" key="Analyzer.Valgrind.Callgrind.EnableBranchSim">false</value>
+ <value type="bool" key="Analyzer.Valgrind.Callgrind.EnableCacheSim">false</value>
+ <value type="bool" key="Analyzer.Valgrind.Callgrind.EnableEventToolTips">true</value>
+ <value type="double" key="Analyzer.Valgrind.Callgrind.MinimumCostRatio">0.01</value>
+ <value type="double" key="Analyzer.Valgrind.Callgrind.VisualisationMinimumCostRatio">10</value>
+ <value type="bool" key="Analyzer.Valgrind.FilterExternalIssues">true</value>
+ <value type="int" key="Analyzer.Valgrind.LeakCheckOnFinish">1</value>
+ <value type="int" key="Analyzer.Valgrind.NumCallers">25</value>
+ <valuelist type="QVariantList" key="Analyzer.Valgrind.RemovedSuppressionFiles"/>
+ <value type="int" key="Analyzer.Valgrind.SelfModifyingCodeDetection">1</value>
+ <value type="bool" key="Analyzer.Valgrind.Settings.UseGlobalSettings">true</value>
+ <value type="bool" key="Analyzer.Valgrind.ShowReachable">false</value>
+ <value type="bool" key="Analyzer.Valgrind.TrackOrigins">true</value>
+ <value type="QString" key="Analyzer.Valgrind.ValgrindExecutable">valgrind</value>
+ <valuelist type="QVariantList" key="Analyzer.Valgrind.VisibleErrorKinds">
+ <value type="int">0</value>
+ <value type="int">1</value>
+ <value type="int">2</value>
+ <value type="int">3</value>
+ <value type="int">4</value>
+ <value type="int">5</value>
+ <value type="int">6</value>
+ <value type="int">7</value>
+ <value type="int">8</value>
+ <value type="int">9</value>
+ <value type="int">10</value>
+ <value type="int">11</value>
+ <value type="int">12</value>
+ <value type="int">13</value>
+ <value type="int">14</value>
+ </valuelist>
+ <value type="int" key="PE.EnvironmentAspect.Base">2</value>
+ <valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">studentGUI</value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
+ <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">Qt4ProjectManager.Qt4RunConfiguration:/home/ppsteacher/work/D-FaLL-System/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/studentGUI.pro</value>
+ <value type="QString" key="Qt4ProjectManager.Qt4RunConfiguration.CommandLineArguments"></value>
+ <value type="QString" key="Qt4ProjectManager.Qt4RunConfiguration.ProFile">studentGUI.pro</value>
+ <value type="bool" key="Qt4ProjectManager.Qt4RunConfiguration.UseDyldImageSuffix">false</value>
+ <value type="bool" key="Qt4ProjectManager.Qt4RunConfiguration.UseTerminal">false</value>
+ <value type="QString" key="Qt4ProjectManager.Qt4RunConfiguration.UserWorkingDirectory"></value>
+ <value type="uint" key="RunConfiguration.QmlDebugServerPort">3768</value>
+ <value type="bool" key="RunConfiguration.UseCppDebugger">false</value>
+ <value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
+ <value type="bool" key="RunConfiguration.UseMultiProcess">false</value>
+ <value type="bool" key="RunConfiguration.UseQmlDebugger">false</value>
+ <value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
+ </valuemap>
+ <value type="int" key="ProjectExplorer.Target.RunConfigurationCount">1</value>
+ </valuemap>
+ </data>
+ <data>
+ <variable>ProjectExplorer.Project.TargetCount</variable>
+ <value type="int">1</value>
+ </data>
+ <data>
+ <variable>ProjectExplorer.Project.Updater.FileVersion</variable>
+ <value type="int">18</value>
+ </data>
+ <data>
+ <variable>Version</variable>
+ <value type="int">18</value>
+ </data>
+</qtcreator>
diff --git a/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py b/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
index e68a72fda7576913aec72c09de6d38dd7426128d..8562b558a21c7446598ecffaaa02bea001f802de 100755
--- a/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
+++ b/pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py
@@ -87,11 +87,16 @@ CMD_RECONNECT = 0
CMD_DISCONNECT = 1
# Commands for PPSClient
-CMD_USE_SAFE_CONTROLLER = 1
-CMD_USE_CUSTOM_CONTROLLER = 2
-CMD_CRAZYFLY_TAKE_OFF = 3
-CMD_CRAZYFLY_LAND = 4
-CMD_CRAZYFLY_MOTORS_OFF = 5
+#CMD_USE_SAFE_CONTROLLER = 1
+#CMD_USE_DEMO_CONTROLLER = 2
+#CMD_USE_STUDENT_CONTROLLER = 3
+#CMD_USE_MPC_CONTROLLER = 4
+#CMD_USE_REMOTE_CONTROLLER = 5
+#CMD_USE_TUNING_CONTROLLER = 6
+
+CMD_CRAZYFLY_TAKE_OFF = 11
+CMD_CRAZYFLY_LAND = 12
+CMD_CRAZYFLY_MOTORS_OFF = 13
rp = RosPack()
record_file = rp.get_path('d_fall_pps') + '/LoggingOnboard.bag'
diff --git a/pps_ws/src/d_fall_pps/include/CentralManagerService.h b/pps_ws/src/d_fall_pps/include/CentralManagerService.h
deleted file mode 100644
index 7f6414c17cf613daf03dd7b1178665e633a733a6..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/include/CentralManagerService.h
+++ /dev/null
@@ -1,40 +0,0 @@
-// Copyright (C) 2017, ETH Zurich, D-ITET, Cyrill Burgener, Marco Mueller, Philipp Friedli
-//
-// 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:
-// Header for the service that manages the context of the student groups.
-//
-// ----------------------------------------------------------------------------------
-
-
-//for field command in CMCommand
-#define CMD_SAVE 1
-#define CMD_RELOAD 2
-
-//for field mode in CMUpdate
-#define ENTRY_INSERT_OR_UPDATE 1
-#define ENTRY_REMOVE 2
-
diff --git a/pps_ws/src/d_fall_pps/include/nodes/CentralManagerService.h b/pps_ws/src/d_fall_pps/include/nodes/CentralManagerService.h
new file mode 100644
index 0000000000000000000000000000000000000000..9d7f81e8e9323f5eb9f46c6031e7688f3e9efcdf
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/include/nodes/CentralManagerService.h
@@ -0,0 +1,130 @@
+// Copyright (C) 2017, ETH Zurich, D-ITET, Cyrill Burgener, Marco Mueller, Philipp Friedli
+//
+// 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:
+// Header for the service that manages the context of the student groups.
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// III N N CCCC L U U DDDD EEEEE SSSS
+// I NN N C L U U D D E S
+// I N N N C L U U D D EEE SSS
+// I N NN C L U U D D E S
+// III N N CCCC LLLLL UUU DDDD EEEEE SSSS
+// ----------------------------------------------------------------------------------
+
+#include <stdlib.h>
+#include <ros/ros.h>
+#include "d_fall_pps/CrazyflieContext.h"
+#include "d_fall_pps/CrazyflieDB.h"
+
+#include "d_fall_pps/CMRead.h"
+#include "d_fall_pps/CMQuery.h"
+#include "d_fall_pps/CMUpdate.h"
+#include "d_fall_pps/CMCommand.h"
+
+#include "CrazyflieIO.h"
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// DDDD EEEEE FFFFF III N N EEEEE SSSS
+// D D E F I NN N E S
+// D D EEE FFF I N N N EEE SSS
+// D D E F I N NN E S
+// DDDD EEEEE F III N N EEEEE SSSS
+// ----------------------------------------------------------------------------------
+
+//for field command in CMCommand
+#define CMD_SAVE 1
+#define CMD_RELOAD 2
+
+//for field mode in CMUpdate
+#define ENTRY_INSERT_OR_UPDATE 1
+#define ENTRY_REMOVE 2
+
+// For which controller parameters to load
+#define LOAD_YAML_SAFE_CONTROLLER_AGENT 1
+#define LOAD_YAML_DEMO_CONTROLLER_AGENT 2
+#define LOAD_YAML_SAFE_CONTROLLER_COORDINATOR 3
+#define LOAD_YAML_DEMO_CONTROLLER_COORDINATOR 4
+
+// For which controller parameters and from where to fetch
+#define FETCH_YAML_SAFE_CONTROLLER_FROM_OWN_AGENT 1
+#define FETCH_YAML_DEMO_CONTROLLER_FROM_OWN_AGENT 2
+#define FETCH_YAML_SAFE_CONTROLLER_FROM_COORDINATOR 3
+#define FETCH_YAML_DEMO_CONTROLLER_FROM_COORDINATOR 4
+
+
+using namespace d_fall_pps;
+using namespace std;
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// V V A RRRR III A BBBB L EEEEE SSSS
+// V V A A R R I A A B B L E S
+// V V A A RRRR I A A BBBB L EEE SSS
+// V V AAAAA R R I AAAAA B B L E S
+// V A A R R III A A BBBB LLLLL EEEEE SSSS
+// ----------------------------------------------------------------------------------
+
+CrazyflieDB crazyflieDB;
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// FFFFF U U N N CCCC TTTTT III OOO N N
+// F U U NN N C T I O O NN N
+// FFF U U N N N C T I O O N N N
+// F U U N NN C T I O O N NN
+// F UUU N N CCCC T III OOO N N
+//
+// PPPP RRRR OOO TTTTT OOO TTTTT Y Y PPPP EEEEE SSSS
+// P P R R O O T O O T Y Y P P E S
+// PPPP RRRR O O T O O T Y PPPP EEE SSS
+// P R R O O T O O T Y P E S
+// P R R OOO T OOO T Y P EEEEE SSSS
+// ----------------------------------------------------------------------------------
+
+bool cmRead(CMRead::Request &request, CMRead::Response &response);
+int findEntryByStudID(unsigned int studID);
+bool cmQuery(CMQuery::Request &request, CMQuery::Response &response);
+int findEntryByCF(string name);
+bool cmUpdate(CMUpdate::Request &request, CMUpdate::Response &response);
+bool cmCommand(CMCommand::Request &request, CMCommand::Response &response);
+
diff --git a/pps_ws/src/d_fall_pps/include/CrazyflieIO.h b/pps_ws/src/d_fall_pps/include/nodes/CrazyflieIO.h
similarity index 100%
rename from pps_ws/src/d_fall_pps/include/CrazyflieIO.h
rename to pps_ws/src/d_fall_pps/include/nodes/CrazyflieIO.h
diff --git a/pps_ws/src/d_fall_pps/include/nodes/DemoControllerService.h b/pps_ws/src/d_fall_pps/include/nodes/DemoControllerService.h
new file mode 100644
index 0000000000000000000000000000000000000000..7c0f469f4a949d5e217c943d82d30ff6fae7929f
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/include/nodes/DemoControllerService.h
@@ -0,0 +1,445 @@
+// Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat
+//
+// 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:
+// Place for students to implement their controller
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// III N N CCCC L U U DDDD EEEEE SSSS
+// I NN N C L U U D D E S
+// I N N N C L U U D D EEE SSS
+// I N NN C L U U D D E S
+// III N N CCCC LLLLL UUU DDDD EEEEE SSSS
+// ----------------------------------------------------------------------------------
+
+// These various headers need to be included so that this controller service can be
+// connected with the D-FaLL system.
+
+//some useful libraries
+#include <math.h>
+#include <stdlib.h>
+#include "ros/ros.h"
+#include <ros/package.h>
+
+//the generated structs from the msg-files have to be included
+#include "d_fall_pps/ViconData.h"
+#include "d_fall_pps/Setpoint.h"
+#include "d_fall_pps/ControlCommand.h"
+#include "d_fall_pps/Controller.h"
+#include "d_fall_pps/DebugMsg.h"
+#include "d_fall_pps/CustomButton.h"
+
+// Include the Parameter Service shared definitions
+#include "nodes/ParameterServiceDefinitions.h"
+
+#include <std_msgs/Int32.h>
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// DDDD EEEEE FFFFF III N N EEEEE SSSS
+// D D E F I NN N E S
+// D D EEE FFF I N N N EEE SSS
+// D D E F I N NN E S
+// DDDD EEEEE F III N N EEEEE SSSS
+// ----------------------------------------------------------------------------------
+
+// These constants are defined to make the code more readable and adaptable.
+
+// Universal constants
+#define PI 3.1415926535
+
+// These constants define the modes that can be used for controller the Crazyflie 2.0,
+// the constants defined here need to be in agreement with those defined in the
+// firmware running on the Crazyflie 2.0.
+// The following is a short description about each mode:
+// MOTOR_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
+// command directly to each of the motors
+// RATE_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
+// command directly to each of the motors, and additionally request the
+// body frame roll, pitch, and yaw angular rates from the PID rate
+// controllers implemented in the Crazyflie 2.0 firmware.
+// ANGE_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
+// command directly to each of the motors, and additionally request the
+// body frame roll, pitch, and yaw angles from the PID attitude
+// controllers implemented in the Crazyflie 2.0 firmware.
+#define CF_COMMAND_TYPE_MOTOR 6
+#define CF_COMMAND_TYPE_RATE 7
+#define CF_COMMAND_TYPE_ANGLE 8
+
+
+// These constants define the controller used for computing the response in the
+// "calculateControlOutput" function
+// The following is a short description about each mode:
+//
+// LQR_MODE_MOTOR LQR controller based on the state vector:
+// [position,velocity,angles,angular velocity]
+// commands per motor thrusts
+//
+// LQR_MODE_ACTUATOR LQR controller based on the state vector:
+// [position,velocity,angles,angular velocity]
+// commands actuators of total force and bodz torques
+//
+// LQR_MODE_RATE LQR controller based on the state vector:
+// [position,velocity,angles]
+//
+// LQR_MODE_ANGLE LQR controller based on the state vector:
+// [position,velocity]
+//
+#define CONTROLLER_MODE_LQR_MOTOR 1
+#define CONTROLLER_MODE_LQR_ACTUATOR 2
+#define CONTROLLER_MODE_LQR_RATE 3 // (DEFAULT)
+#define CONTROLLER_MODE_LQR_ANGLE 4
+#define CONTROLLER_MODE_LQR_ANGLE_RATE_NESTED 5
+#define CONTROLLER_MODE_ANGLE_RESPONSE_TEST 6
+
+
+// These constants define the method used for estimating the Inertial
+// frame state.
+// All methods are run at all times, this flag indicates which estimate
+// is passed onto the controller.
+// The following is a short description about each mode:
+//
+// ESTIMATOR_METHOD_FINITE_DIFFERENCE
+// Takes the poisition and angles directly as measured,
+// and estimates the velocities as a finite different to the
+// previous measurement
+//
+// ESTIMATOR_METHOD_POINT_MASS_PER_DIMENSION
+// Uses a 2nd order random walk estimator independently for
+// each of (x,y,z,roll,pitch,yaw)
+//
+// ESTIMATOR_METHOD_QUADROTOR_MODEL_BASED
+// Uses the model of the quad-rotor and the previous inputs
+//
+#define ESTIMATOR_METHOD_FINITE_DIFFERENCE 1
+#define ESTIMATOR_METHOD_POINT_MASS_PER_DIMENSION 2 // (DEFAULT)
+#define ESTIMATOR_METHOD_QUADROTOR_MODEL_BASED 3
+
+
+// Namespacing the package
+using namespace d_fall_pps;
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// V V A RRRR III A BBBB L EEEEE SSSS
+// V V A A R R I A A B B L E S
+// V V A A RRRR I A A BBBB L EEE SSS
+// V V AAAAA R R I AAAAA B B L E S
+// V A A R R III A A BBBB LLLLL EEEEE SSSS
+// ----------------------------------------------------------------------------------
+
+// VARIABLES FOR SOME "ALMOST CONSTANTS"
+// > Mass of the Crazyflie quad-rotor, in [grams]
+float cf_mass;
+// > Coefficients of the 16-bit command to thrust conversion
+std::vector<float> motorPoly(3);
+// The weight of the Crazyflie in [Newtons], i.e., mg
+float gravity_force;
+// One quarter of the "gravity_force"
+float gravity_force_quarter;
+
+
+
+
+// VARIABLES FOR THE CONTROLLER
+
+// Frequency at which the controller is running
+float vicon_frequency;
+
+// Frequency at which the controller is running
+float control_frequency;
+
+
+// > The setpoints for (x,y,z) position and yaw angle, in that order
+float setpoint[4] = {0.0,0.0,0.4,0.0};
+
+
+
+
+// The controller type to run in the "calculateControlOutput" function
+int controller_mode = CONTROLLER_MODE_LQR_RATE;
+
+
+// The LQR Controller parameters for "CONTROLLER_MODE_LQR_MOTOR"
+std::vector<float> gainMatrixMotor1 (12,0.0);
+std::vector<float> gainMatrixMotor2 (12,0.0);
+std::vector<float> gainMatrixMotor3 (12,0.0);
+std::vector<float> gainMatrixMotor4 (12,0.0);
+
+// The LQR Controller parameters for "CONTROLLER_MODE_LQR_ACTUATOR"
+std::vector<float> gainMatrixThrust_TwelveStateVector (12,0.0);
+std::vector<float> gainMatrixRollTorque (12,0.0);
+std::vector<float> gainMatrixPitchTorque (12,0.0);
+std::vector<float> gainMatrixYawTorque (12,0.0);
+
+// The LQR Controller parameters for "CONTROLLER_MODE_LQR_RATE"
+std::vector<float> gainMatrixThrust_NineStateVector (9,0.0);
+std::vector<float> gainMatrixRollRate (9,0.0);
+std::vector<float> gainMatrixPitchRate (9,0.0);
+std::vector<float> gainMatrixYawRate (9,0.0);
+
+// The LQR Controller parameters for "CONTROLLER_MODE_LQR_ANGLE"
+std::vector<float> gainMatrixThrust_SixStateVector (6,0.0);
+std::vector<float> gainMatrixRollAngle (6,0.0);
+std::vector<float> gainMatrixPitchAngle (6,0.0);
+
+// The LQR Controller parameters for "CONTROLLER_MODE_LQR_ANGLE_RATE_NESTED"
+std::vector<float> gainMatrixThrust_SixStateVector_50Hz (6,0.0);
+std::vector<float> gainMatrixRollAngle_50Hz (6,0.0);
+std::vector<float> gainMatrixPitchAngle_50Hz (6,0.0);
+
+std::vector<float> gainMatrixRollRate_Nested (3,0.0);
+std::vector<float> gainMatrixPitchRate_Nested (3,0.0);
+std::vector<float> gainMatrixYawRate_Nested (3,0.0);
+
+int lqr_angleRateNested_counter = 4;
+float lqr_angleRateNested_prev_thrustAdjustment = 0.0;
+float lqr_angleRateNested_prev_rollAngle = 0.0;
+float lqr_angleRateNested_prev_pitchAngle = 0.0;
+float lqr_angleRateNested_prev_yawAngle = 0.0;
+
+// The LQR Controller parameters for "CONTROLLER_MODE_ANGLE_RESPONSE_TEST"
+int angleResponseTest_counter = 4;
+float angleResponseTest_prev_thrustAdjustment = 0.0;
+float angleResponseTest_prev_rollAngle = 0.0;
+float angleResponseTest_prev_pitchAngle = 0.0;
+float angleResponseTest_prev_yawAngle = 0.0;
+float angleResponseTest_pitchAngle_degrees;
+float angleResponseTest_pitchAngle_radians;
+float angleResponseTest_distance_meters;
+
+
+
+
+// The 16-bit command limits
+float cmd_sixteenbit_min;
+float cmd_sixteenbit_max;
+
+
+// VARIABLES FOR THE ESTIMATOR
+
+// Frequency at which the controller is running
+float estimator_frequency;
+
+// > A flag for which estimator to use:
+int estimator_method = ESTIMATOR_METHOD_FINITE_DIFFERENCE;
+// > The current state interial estimate,
+// for use by the controller
+float current_stateInertialEstimate[12];
+
+// > The measurement of the Crazyflie at the "current" time step,
+// to avoid confusion
+float current_xzy_rpy_measurement[6];
+
+// > The measurement of the Crazyflie at the "previous" time step,
+// used for computing finite difference velocities
+float previous_xzy_rpy_measurement[6];
+
+// > The full 12 state estimate maintained by the finite
+// difference state estimator
+float stateInterialEstimate_viaFiniteDifference[12];
+
+// > The full 12 state estimate maintained by the point mass
+// kalman filter state estimator
+float stateInterialEstimate_viaPointMassKalmanFilter[12];
+
+// THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
+// > For the (x,y,z) position
+std::vector<float> PMKF_Ahat_row1_for_positions (2,0.0);
+std::vector<float> PMKF_Ahat_row2_for_positions (2,0.0);
+std::vector<float> PMKF_Kinf_for_positions (2,0.0);
+// > For the (roll,pitch,yaw) angles
+std::vector<float> PMKF_Ahat_row1_for_angles (2,0.0);
+std::vector<float> PMKF_Ahat_row2_for_angles (2,0.0);
+std::vector<float> PMKF_Kinf_for_angles (2,0.0);
+
+
+
+// VARIABLES FOR THE NAMESPACES FOR THE PARAMETER SERVICES
+// > For the paramter service of this agent
+std::string namespace_to_own_agent_parameter_service;
+// > For the parameter service of the coordinator
+std::string namespace_to_coordinator_parameter_service;
+
+
+// ROS PUBLISHER FOR SENDING OUT THE DEBUG MESSAGES
+ros::Publisher debugPublisher;
+
+
+// VARIABLES RELATING TO PERFORMING THE CONVERSION INTO BODY FRAME
+
+// Boolean whether to execute the convert into body frame function
+bool shouldPerformConvertIntoBodyFrame = false;
+
+
+// VARIABLES RELATING TO THE PUBLISHING OF A DEBUG MESSAGE
+
+// Boolean indiciating whether the "Debug Message" of this agent should be published or not
+bool shouldPublishDebugMessage = false;
+
+// Boolean indiciating whether the debugging ROS_INFO_STREAM should be displayed or not
+bool shouldDisplayDebugInfo = false;
+
+
+// VARIABLES RELATING TO PUBLISHING CURRENT POSITION AND FOLLOWING ANOTHER AGENT'S
+// POSITION
+
+// The ID of this agent, i.e., the ID of this compute
+int my_agentID = 0;
+
+// Boolean indicating whether the (x,y,z,yaw) of this agent should be published or not
+// > The default behaviour is: do not publish,
+// > This varaible is changed based on parameters loaded from the YAML file
+bool shouldPublishCurrent_xyz_yaw = false;
+
+// Boolean indicating whether the (x,y,z,yaw) setpoint of this agent should adapted in
+// an attempt to follow the "my_current_xyz_yaw_topic" from another agent
+// > The default behaviour is: do not follow,
+// > This varaible is changed based on parameters loaded from the YAML file
+bool shouldFollowAnotherAgent = false;
+
+// The order in which agents should follow eachother
+// > This parameter is a vector of integers that specifies agent ID's
+// > The order of the agent ID's is the ordering of the line formation
+// > i.e., the first ID is the leader, the second ID should follow the first ID, and so on
+std::vector<int> follow_in_a_line_agentIDs(100);
+
+// Integer specifying the ID of the agent that will be followed by this agent
+// > The default behaviour not to follow any agent, indicated by ID zero
+// > This varaible is changed based on parameters loaded from the YAML file
+int agentID_to_follow = 0;
+
+// ROS Publisher for my current (x,y,z,yaw) position
+ros::Publisher my_current_xyz_yaw_publisher;
+
+// ROS Subscriber for my position
+ros::Subscriber xyz_yaw_to_follow_subscriber;
+
+
+// RELEVANT NOTES ABOUT THE VARIABLES DECLARE HERE:
+// The "CrazyflieData" type used for the "request" variable is a
+// structure as defined in the file "CrazyflieData.msg" which has the following
+// properties:
+// string crazyflieName The name given to the Crazyflie in the Vicon software
+// float64 x The x position of the Crazyflie [metres]
+// float64 y The y position of the Crazyflie [metres]
+// float64 z The z position of the Crazyflie [metres]
+// float64 roll The roll component of the intrinsic Euler angles [radians]
+// float64 pitch The pitch component of the intrinsic Euler angles [radians]
+// float64 yaw The yaw component of the intrinsic Euler angles [radians]
+// float64 acquiringTime #delta t The time elapsed since the previous "CrazyflieData" was received [seconds]
+// bool occluded A boolean indicted whether the Crazyflie for visible at the time of this measurement
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// FFFFF U U N N CCCC TTTTT III OOO N N
+// F U U NN N C T I O O NN N
+// FFF U U N N N C T I O O N N N
+// F U U N NN C T I O O N NN
+// F UUU N N CCCC T III OOO N N
+//
+// PPPP RRRR OOO TTTTT OOO TTTTT Y Y PPPP EEEEE SSSS
+// P P R R O O T O O T Y Y P P E S
+// PPPP RRRR O O T O O T Y PPPP EEE SSS
+// P R R O O T O O T Y P E S
+// P R R OOO T OOO T Y P EEEEE SSSS
+// ----------------------------------------------------------------------------------
+
+// These function prototypes are not strictly required for this code to complile, but
+// adding the function prototypes here means the the functions can be written below in
+// any order. If the function prototypes are not included then the function need to
+// written below in an order that ensure each function is implemented before it is
+// called from another function, hence why the "main" function is at the bottom.
+
+// CONTROLLER COMPUTATIONS
+// > The function that is called to "start" all estimation and control computations
+bool calculateControlOutput(Controller::Request &request, Controller::Response &response);
+
+// > The various functions that implement an LQR controller
+void calculateControlOutput_viaLQR( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
+void calculateControlOutput_viaLQRforMotors( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
+void calculateControlOutput_viaLQRforActuators( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
+void calculateControlOutput_viaLQRforRates( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
+void calculateControlOutput_viaLQRforAngles( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
+void calculateControlOutput_viaLQRforAnglesRatesNested( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
+void calculateControlOutput_viaAngleResponseTest( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
+
+// ESTIMATOR COMPUTATIONS
+void performEstimatorUpdate_forStateInterial(Controller::Request &request);
+void performEstimatorUpdate_forStateInterial_viaFiniteDifference();
+void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter();
+
+
+// PUBLISHING OF THE DEBUG MESSAGE
+void construct_and_publish_debug_message(Controller::Request &request, Controller::Response &response);
+
+
+// TRANSFORMATION FROM INTERIAL ESTIMATE TO BODY FRAME ERROR
+void convert_stateInertial_to_bodyFrameError(float stateInertial[12], float setpoint[4], float (&bodyFrameError)[12]);
+
+// TRANSFORMATION OF THE (x,y) INERTIAL FRAME ERROR INTO AN (x,y) BODY FRAME ERROR
+void convertIntoBodyFrame(float stateInertial[12], float (&stateBody)[12], float yaw_measured);
+
+// CONVERSION FROM THRUST IN NEWTONS TO 16-BIT COMMAND
+float computeMotorPolyBackward(float thrust);
+
+// SETPOINT CHANGE CALLBACK
+void setpointCallback(const Setpoint& newSetpoint);
+void xyz_yaw_to_follow_callback(const Setpoint& newSetpoint);
+
+// CUSTOM COMMAND RECEIVED CALLBACK
+void customCommandReceivedCallback(const CustomButton& commandReceived);
+
+// PUBLISH THE CURRENT X,Y,Z, AND YAW
+void publish_current_xyz_yaw(float x, float y, float z, float yaw);
+
+// LOAD PARAMETERS
+float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name);
+void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length);
+int getParameterInt(ros::NodeHandle& nodeHandle, std::string name);
+void getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length);
+int getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val);
+bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name);
+
+void yamlReadyForFetchCallback(const std_msgs::Int32& msg);
+void fetchYamlParameters(ros::NodeHandle& nodeHandle);
+void processFetchedParameters();
+
diff --git a/pps_ws/src/d_fall_pps/include/nodes/PPSClient.h b/pps_ws/src/d_fall_pps/include/nodes/PPSClient.h
new file mode 100644
index 0000000000000000000000000000000000000000..82d1be478e0ec7c4f5ee39f13646dfac63520507
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/include/nodes/PPSClient.h
@@ -0,0 +1,316 @@
+// Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat, Angel Romero, Cyrill Burgener, Marco Mueller, Philipp Friedli
+//
+// 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:
+// ROS node that manages the student's setup.
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// III N N CCCC L U U DDDD EEEEE SSSS
+// I NN N C L U U D D E S
+// I N N N C L U U D D EEE SSS
+// I N NN C L U U D D E S
+// III N N CCCC LLLLL UUU DDDD EEEEE SSSS
+// ----------------------------------------------------------------------------------
+
+#include "ros/ros.h"
+#include <stdlib.h>
+#include <std_msgs/String.h>
+#include <ros/package.h>
+
+#include "d_fall_pps/Controller.h"
+#include "d_fall_pps/CMQuery.h"
+
+#include "d_fall_pps/ViconData.h"
+#include "d_fall_pps/CrazyflieData.h"
+#include "d_fall_pps/ControlCommand.h"
+#include "d_fall_pps/CrazyflieContext.h"
+#include "d_fall_pps/Setpoint.h"
+#include "std_msgs/Int32.h"
+#include "std_msgs/Float32.h"
+
+// Include the Parameter Service shared definitions
+#include "nodes/ParameterServiceDefinitions.h"
+
+// Need for having a ROS "bag" to store data for post-analysis
+//#include <rosbag/bag.h>
+
+#include "d_fall_pps/ControlCommand.h"
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// DDDD EEEEE FFFFF III N N EEEEE SSSS
+// D D E F I NN N E S
+// D D EEE FFF I N N N EEE SSS
+// D D E F I N NN E S
+// DDDD EEEEE F III N N EEEEE SSSS
+// ----------------------------------------------------------------------------------
+
+// Types PPS firmware
+#define CF_COMMAND_TYPE_MOTORS 6
+#define CF_COMMAND_TYPE_RATE 7
+#define CF_COMMAND_TYPE_ANGLE 8
+
+// Types of controllers being used:
+#define SAFE_CONTROLLER 1
+#define DEMO_CONTROLLER 2
+#define STUDENT_CONTROLLER 3
+#define MPC_CONTROLLER 4
+#define REMOTE_CONTROLLER 5
+#define TUNING_CONTROLLER 6
+
+// The constants that "command" changes in the
+// operation state of this agent
+#define CMD_USE_SAFE_CONTROLLER 1
+#define CMD_USE_DEMO_CONTROLLER 2
+#define CMD_USE_STUDENT_CONTROLLER 3
+#define CMD_USE_MPC_CONTROLLER 4
+#define CMD_USE_REMOTE_CONTROLLER 5
+#define CMD_USE_TUNING_CONTROLLER 6
+
+
+#define CMD_CRAZYFLY_TAKE_OFF 11
+#define CMD_CRAZYFLY_LAND 12
+#define CMD_CRAZYFLY_MOTORS_OFF 13
+
+// 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
+
+// Commands for CrazyRadio
+#define CMD_RECONNECT 0
+#define CMD_DISCONNECT 1
+
+
+// CrazyRadio states:
+#define CONNECTED 0
+#define CONNECTING 1
+#define DISCONNECTED 2
+
+
+// Universal constants
+#define PI 3.141592653589
+
+// Namespacing the package
+using namespace d_fall_pps;
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// V V A RRRR III A BBBB L EEEEE SSSS
+// V V A A R R I A A B B L E S
+// V V A A RRRR I A A BBBB L EEE SSS
+// V V AAAAA R R I AAAAA B B L E S
+// V A A R R III A A BBBB LLLLL EEEEE SSSS
+// ----------------------------------------------------------------------------------
+
+// "agentID", gives namespace and identifier in CentralManagerService
+int agentID;
+
+// The safe controller specified in the ClientConfig.yaml
+ros::ServiceClient safeController;
+// The Demo controller specified in the ClientConfig.yaml
+ros::ServiceClient demoController;
+// The Student controller specified in the ClientConfig.yaml
+ros::ServiceClient studentController;
+// The MPC controller specified in the ClientConfig.yaml
+ros::ServiceClient mpcController;
+// The Remote controller specified in the ClientConfig.yaml
+ros::ServiceClient remoteController;
+// The Tuning controller specified in the ClientConfig.yaml
+ros::ServiceClient tuningController;
+
+
+//values for safteyCheck
+bool strictSafety;
+float angleMargin;
+
+// battery threshold
+float m_battery_threshold_while_flying;
+float m_battery_threshold_while_motors_off;
+
+
+// battery values
+
+int m_battery_state;
+float m_battery_voltage;
+
+Setpoint controller_setpoint;
+
+// variables for linear trayectory
+Setpoint current_safe_setpoint;
+double distance;
+double unit_vector[3];
+bool was_in_threshold = false;
+double distance_threshold; //to be loaded from yaml
+
+
+ros::ServiceClient centralManager;
+ros::Publisher controlCommandPublisher;
+
+// communicate with safeControllerService, setpoint, etc...
+ros::Publisher safeControllerServiceSetpointPublisher;
+
+// publisher for flying state
+ros::Publisher flyingStatePublisher;
+
+// publisher for battery state
+ros::Publisher batteryStatePublisher;
+
+// publisher to send commands to itself.
+ros::Publisher commandPublisher;
+
+// communication with crazyRadio node. Connect and disconnect
+ros::Publisher crazyRadioCommandPublisher;
+
+
+// Variable for the namespaces for the paramter services
+// > For the paramter service of this agent
+std::string namespace_to_own_agent_parameter_service;
+// > For the parameter service of the coordinator
+std::string namespace_to_coordinator_parameter_service;
+
+
+// variables for the states:
+int flying_state;
+bool changed_state_flag;
+
+// variable for crazyradio status
+int crazyradio_status;
+
+//describes the area of the crazyflie and other parameters
+CrazyflieContext context;
+
+//wheter to use safe of demo controller
+int instant_controller; //variable for the instant controller, e.g., we use safe controller for taking off and landing even if demo controller is enabled. This variable WILL change automatically
+
+// controller used:
+int controller_used;
+
+ros::Publisher controllerUsedPublisher;
+
+std::string ros_namespace;
+
+float take_off_distance;
+float landing_distance;
+float duration_take_off;
+float duration_landing;
+
+bool finished_take_off = false;
+bool finished_land = false;
+
+ros::Timer timer_takeoff;
+ros::Timer timer_land;
+
+// A ROS "bag" to store data for post-analysis
+//rosbag::Bag bag;
+
+
+
+
+// ----------------------------------------------------------------------------------
+// FFFFF U U N N CCCC TTTTT III OOO N N
+// F U U NN N C T I O O NN N
+// FFF U U N N N C T I O O N N N
+// F U U N NN C T I O O N NN
+// F UUU N N CCCC T III OOO N N
+//
+// PPPP RRRR OOO TTTTT OOO TTTTT Y Y PPPP EEEEE SSSS
+// P P R R O O T O O T Y Y P P E S
+// PPPP RRRR O O T O O T Y PPPP EEE SSS
+// P R R O O T O O T Y P E S
+// P R R OOO T OOO T Y P EEEEE SSSS
+// ----------------------------------------------------------------------------------
+
+
+// > For the LOAD PARAMETERS
+void yamlReadyForFetchCallback(const std_msgs::Int32& msg);
+void fetchYamlParametersForSafeController(ros::NodeHandle& nodeHandle);
+void fetchClientConfigParameters(ros::NodeHandle& nodeHandle);
+
+void crazyRadioCommandAllAgentsCallback(const std_msgs::Int32& msg);
+
+
+
+
+
+void viconCallback(const ViconData& viconData);
+
+// > For the LOADING of YAML PARAMETERS
+void yamlReadyForFetchCallback(const std_msgs::Int32& msg);
+void fetchYamlParametersForSafeController(ros::NodeHandle& nodeHandle);
+void fetchClientConfigParameters(ros::NodeHandle& nodeHandle);
+
+
+
+// > For the {dis/re}-connect command received from the coordinator
+void crazyRadioCommandAllAgentsCallback(const std_msgs::Int32& msg);
+
+
+// > For the BATTERY
+int getBatteryState();
+void setBatteryStateTo(int new_battery_state);
+float movingAverageBatteryFilter(float new_input);
+void CFBatteryCallback(const std_msgs::Float32& msg);
+
+
+
+
+
+
+void loadSafeController();
+void loadDemoController();
+void loadStudentController();
+void loadMpcController();
+void loadRemoteController();
+void loadTuningController();
+
+void sendMessageUsingController(int controller);
+void setInstantController(int controller);
+int getInstantController();
+void setControllerUsed(int controller);
+int getControllerUsed();
+
+
+
+
+
diff --git a/pps_ws/src/d_fall_pps/include/nodes/ParameterService.h b/pps_ws/src/d_fall_pps/include/nodes/ParameterService.h
new file mode 100644
index 0000000000000000000000000000000000000000..36153c11a6fe4639fd150f7c588c4db3769d16fd
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/include/nodes/ParameterService.h
@@ -0,0 +1,124 @@
+// Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat
+//
+// 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:
+// The service that manages the loading of YAML parameters
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// III N N CCCC L U U DDDD EEEEE SSSS
+// I NN N C L U U D D E S
+// I N N N C L U U D D EEE SSS
+// I N NN C L U U D D E S
+// III N N CCCC LLLLL UUU DDDD EEEEE SSSS
+// ----------------------------------------------------------------------------------
+
+#include <stdlib.h>
+#include <string>
+
+#include <ros/ros.h>
+#include <ros/package.h>
+#include <ros/network.h>
+#include "std_msgs/Int32.h"
+//#include "std_msgs/Float32.h"
+//#include <std_msgs/String.h>
+
+#include "d_fall_pps/Controller.h"
+#include "d_fall_pps/LoadYamlFiles.h"
+
+// Include the shared definitions
+#include "nodes/ParameterServiceDefinitions.h"
+
+
+// ----------------------------------------------------------------------------------
+// DDDD EEEEE FFFFF III N N EEEEE SSSS
+// D D E F I NN N E S
+// D D EEE FFF I N N N EEE SSS
+// D D E F I N NN E S
+// DDDD EEEEE F III N N EEEEE SSSS
+// ----------------------------------------------------------------------------------
+
+// The types, i.e., agent or coordinator
+#define TYPE_INVALID -1
+#define TYPE_COORDINATOR 1
+#define TYPE_AGENT 2
+
+
+// Namespacing the package
+using namespace d_fall_pps;
+//using namespace std;
+
+
+
+
+// ----------------------------------------------------------------------------------
+// V V A RRRR III A BBBB L EEEEE SSSS
+// V V A A R R I A A B B L E S
+// V V A A RRRR I A A BBBB L EEE SSS
+// V V AAAAA R R I AAAAA B B L E S
+// V A A R R III A A BBBB LLLLL EEEEE SSSS
+// ----------------------------------------------------------------------------------
+
+// The type of this node, i.e., agent or a coordinator, specified as a parameter in the
+// "*.launch" file
+int my_type = 0;
+
+// The ID of this agent, i.e., the ID of this computer in the case that this computer is
+// and agent
+std::string my_agentID = "000";
+
+// Publisher that notifies the relevant nodes when the YAML paramters have been loaded
+// from file into ram/cache, and hence are ready to be fetched
+ros::Publisher controllerYamlReadyForFetchPublisher;
+
+
+std::string m_base_namespace;
+
+ros::Subscriber requestLoadControllerYamlSubscriber_agent_to_self;
+
+
+// ----------------------------------------------------------------------------------
+// FFFFF U U N N CCCC TTTTT III OOO N N
+// F U U NN N C T I O O NN N
+// FFF U U N N N C T I O O N N N
+// F U U N NN C T I O O N NN
+// F UUU N N CCCC T III OOO N N
+//
+// PPPP RRRR OOO TTTTT OOO TTTTT Y Y PPPP EEEEE SSSS
+// P P R R O O T O O T Y Y P P E S
+// PPPP RRRR O O T O O T Y PPPP EEE SSS
+// P R R O O T O O T Y P E S
+// P R R OOO T OOO T Y P EEEEE SSSS
+// ----------------------------------------------------------------------------------
+
+void requestLoadControllerYamlCallback(const std_msgs::Int32& msg);
+
+bool loadYamlFiles(LoadYamlFiles::Request &request, LoadYamlFiles::Response &response);
diff --git a/pps_ws/src/d_fall_pps/include/nodes/ParameterServiceDefinitions.h b/pps_ws/src/d_fall_pps/include/nodes/ParameterServiceDefinitions.h
new file mode 100644
index 0000000000000000000000000000000000000000..12ef8b42b6c02b580a75bc1864b8002b3eaf7bc1
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/include/nodes/ParameterServiceDefinitions.h
@@ -0,0 +1,72 @@
+// Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat
+//
+// 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:
+// The service that manages the loading of YAML parameters
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+// DEFINES THAT ARE SHARED WITH OTHER FILES
+
+// For which controller parameters to load from file
+#define LOAD_YAML_SAFE_CONTROLLER_AGENT 1
+#define LOAD_YAML_DEMO_CONTROLLER_AGENT 2
+#define LOAD_YAML_STUDENT_CONTROLLER_AGENT 3
+#define LOAD_YAML_MPC_CONTROLLER_AGENT 4
+#define LOAD_YAML_REMOTE_CONTROLLER_AGENT 5
+#define LOAD_YAML_TUNING_CONTROLLER_AGENT 6
+
+#define LOAD_YAML_SAFE_CONTROLLER_COORDINATOR 11
+#define LOAD_YAML_DEMO_CONTROLLER_COORDINATOR 12
+#define LOAD_YAML_STUDENT_CONTROLLER_COORDINATOR 13
+#define LOAD_YAML_MPC_CONTROLLER_COORDINATOR 14
+#define LOAD_YAML_REMOTE_CONTROLLER_COORDINATOR 15
+#define LOAD_YAML_TUNING_CONTROLLER_COORDINATOR 16
+
+
+// For sending commands to the controller node informing
+// which parameters to fetch
+// > NOTE: these are identical to the #defines above, but
+// used because they have the name distinguishes
+// between:
+// - "loading" a yaml file into ram
+// - "fetching" the values that were loaded into ram
+#define FETCH_YAML_SAFE_CONTROLLER_FROM_OWN_AGENT 1
+#define FETCH_YAML_DEMO_CONTROLLER_FROM_OWN_AGENT 2
+#define FETCH_YAML_STUDENT_CONTROLLER_FROM_OWN_AGENT 3
+#define FETCH_YAML_MPC_CONTROLLER_FROM_OWN_AGENT 4
+#define FETCH_YAML_REMOTE_CONTROLLER_FROM_OWN_AGENT 5
+#define FETCH_YAML_TUNING_CONTROLLER_FROM_OWN_AGENT 6
+
+#define FETCH_YAML_SAFE_CONTROLLER_FROM_COORDINATOR 11
+#define FETCH_YAML_DEMO_CONTROLLER_FROM_COORDINATOR 12
+#define FETCH_YAML_STUDENT_CONTROLLER_FROM_COORDINATOR 13
+#define FETCH_YAML_MPC_CONTROLLER_FROM_COORDINATOR 14
+#define FETCH_YAML_REMOTE_CONTROLLER_FROM_COORDINATOR 15
+#define FETCH_YAML_TUNING_CONTROLLER_FROM_COORDINATOR 16
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/include/nodes/RemoteControllerService.h b/pps_ws/src/d_fall_pps/include/nodes/RemoteControllerService.h
new file mode 100644
index 0000000000000000000000000000000000000000..21eaae66e4b8d490e04203f0352cf53461ca6792
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/include/nodes/RemoteControllerService.h
@@ -0,0 +1,479 @@
+// Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat
+//
+// 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:
+// Place for students to implement their controller
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// III N N CCCC L U U DDDD EEEEE SSSS
+// I NN N C L U U D D E S
+// I N N N C L U U D D EEE SSS
+// I N NN C L U U D D E S
+// III N N CCCC LLLLL UUU DDDD EEEEE SSSS
+// ----------------------------------------------------------------------------------
+
+// These various headers need to be included so that this controller service can be
+// connected with the D-FaLL system.
+
+//some useful libraries
+#include <math.h>
+#include <stdlib.h>
+#include "ros/ros.h"
+#include <ros/package.h>
+
+//the generated structs from the msg-files have to be included
+#include "d_fall_pps/ViconData.h"
+#include "d_fall_pps/Setpoint.h"
+#include "d_fall_pps/ControlCommand.h"
+#include "d_fall_pps/Controller.h"
+#include "d_fall_pps/DebugMsg.h"
+#include "d_fall_pps/CustomButton.h"
+#include "d_fall_pps/ViconSubscribeObjectName.h"
+#include "d_fall_pps/CustomControllerYAML.h"
+
+// Include the Parameter Service shared definitions
+#include "nodes/ParameterServiceDefinitions.h"
+
+#include <std_msgs/Int32.h>
+
+
+
+
+// ----------------------------------------------------------------------------------
+// DDDD EEEEE FFFFF III N N EEEEE SSSS
+// D D E F I NN N E S
+// D D EEE FFF I N N N EEE SSS
+// D D E F I N NN E S
+// DDDD EEEEE F III N N EEEEE SSSS
+// ----------------------------------------------------------------------------------
+
+// These constants are defined to make the code more readable and adaptable.
+
+// Universal constants
+#define PI 3.1415926535
+
+// These constants define the modes that can be used for controller the Crazyflie 2.0,
+// the constants defined here need to be in agreement with those defined in the
+// firmware running on the Crazyflie 2.0.
+// The following is a short description about each mode:
+// MOTOR_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
+// command directly to each of the motors
+// RATE_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
+// command directly to each of the motors, and additionally request the
+// body frame roll, pitch, and yaw angular rates from the PID rate
+// controllers implemented in the Crazyflie 2.0 firmware.
+// ANGE_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
+// command directly to each of the motors, and additionally request the
+// body frame roll, pitch, and yaw angles from the PID attitude
+// controllers implemented in the Crazyflie 2.0 firmware.
+
+#define CF_COMMAND_TYPE_MOTOR 6
+#define CF_COMMAND_TYPE_RATE 7
+#define CF_COMMAND_TYPE_ANGLE 8
+
+
+// These constants define the controller used for computing the response in the
+// "calculateControlOutput" function
+// The following is a short description about each mode:
+//
+// LQR_MODE_MOTOR LQR controller based on the state vector:
+// [position,velocity,angles,angular velocity]
+// commands per motor thrusts
+//
+// LQR_MODE_ACTUATOR LQR controller based on the state vector:
+// [position,velocity,angles,angular velocity]
+// commands actuators of total force and bodz torques
+//
+// LQR_MODE_RATE LQR controller based on the state vector:
+// [position,velocity,angles]
+//
+// LQR_MODE_ANGLE LQR controller based on the state vector:
+// [position,velocity]
+//
+//#define CONTROLLER_MODE_LQR_MOTOR 1
+//#define CONTROLLER_MODE_LQR_ACTUATOR 2
+#define CONTROLLER_MODE_LQR_RATE 3 // (DEFAULT)
+#define CONTROLLER_MODE_LQR_ANGLE 4
+#define CONTROLLER_MODE_LQR_ANGLE_RATE_NESTED 5
+//#define CONTROLLER_MODE_ANGLE_RESPONSE_TEST 6
+
+
+// These constants define the method used for estimating the Inertial
+// frame state.
+// All methods are run at all times, this flag indicates which estimate
+// is passed onto the controller.
+// The following is a short description about each mode:
+//
+// ESTIMATOR_METHOD_FINITE_DIFFERENCE
+// Takes the poisition and angles directly as measured,
+// and estimates the velocities as a finite different to the
+// previous measurement
+//
+// ESTIMATOR_METHOD_POINT_MASS_PER_DIMENSION
+// Uses a 2nd order random walk estimator independently for
+// each of (x,y,z,roll,pitch,yaw)
+//
+// ESTIMATOR_METHOD_QUADROTOR_MODEL_BASED
+// Uses the model of the quad-rotor and the previous inputs
+//
+#define ESTIMATOR_METHOD_FINITE_DIFFERENCE 1
+#define ESTIMATOR_METHOD_POINT_MASS_PER_DIMENSION 2 // (DEFAULT)
+#define ESTIMATOR_METHOD_QUADROTOR_MODEL_BASED 3
+
+// These constants define the controller used for computing the response in the
+// "calculateControlOutput" function
+// The following is a short description about each mode:
+// LQR_RATE_MODE LQR controller based on the state vector:
+// [position,velocity,angles]
+//
+// LQR_ANGLE_MODE LQR controller based on the state vector:
+// [position,velocity]
+//
+#define LQR_RATE_MODE 1 // (DEFAULT)
+#define LQR_ANGLE_MODE 2
+
+// Namespacing the package
+using namespace d_fall_pps;
+
+
+// ----------------------------------------------------------------------------------
+// V V A RRRR III A BBBB L EEEEE SSSS
+// V V A A R R I A A B B L E S
+// V V A A RRRR I A A BBBB L EEE SSS
+// V V AAAAA R R I AAAAA B B L E S
+// V A A R R III A A BBBB LLLLL EEEEE SSSS
+// ----------------------------------------------------------------------------------
+
+
+// ******************************************************************************* //
+// VARIABLES SPECIFIC TO THE REMOTE CONTROL FEATURE
+
+// ROS Subscriber for the Vicon data of the remote
+ros::Subscriber viconSubscriber;
+
+// ROS Publisher for the Vicon data of the remote
+ros::Publisher remote_xyz_rpy_publisher;
+
+// ROS Publisher for the Setpoint for the remote controller
+ros::Publisher remote_control_setpoint_publisher;
+
+// Vicon object name of the Remote to follow
+std::string viconObjectName_forRemote ("empty");
+std::string default_viconObjectName_forRemote ("DFALL_REMOTE01");
+
+// Boolean for whether the Remote's state should be published as a message
+bool shouldPublishRemote_xyz_rpy = false;
+
+// Boolean for whether the Remote's state should be display in the terminal window
+bool shouldDisplayRemote_xyz_rpy = false;
+
+// Boolean for whether the Remote control mode is active
+bool isActive_remoteControlMode = false;
+
+// The setpoint from the remote
+float setpointFromRemote_roll = 0.0;
+float setpointFromRemote_pitch = 0.0;
+float setpointFromRemote_yaw = 0.0;
+float setpointFromRemote_z = 0.0;
+
+// The z-height of the remote when "Activate" is pressed
+float z_of_remote_previous_measurement = 0.0;
+float z_when_remote_activated = 0.0;
+
+// Roll and pitch limit (in degrees for angles)
+float remoteControlLimit_roll_degrees = 15.0;
+float remoteControlLimit_pitch_degrees = 15.0;
+float remoteControlLimit_roll = 0.262;
+float remoteControlLimit_pitch = 0.262;
+
+// Factor by which to reduce the remote control input
+float remoteConrtolSetpointFactor_roll = 1.0;
+float remoteConrtolSetpointFactor_pitch = 1.0;
+float remoteConrtolSetpointFactor_yaw = 1.0;
+float remoteConrtolSetpointFactor_z = 1.0;
+
+// LQR Gain matrix for tracking the angle commands from the Crazyflie
+std::vector<float> gainMatrixRollRate_forRemoteControl (3,0.0);
+std::vector<float> gainMatrixPitchRate_forRemoteControl (3,0.0);
+std::vector<float> gainMatrixYawRate_forRemoteControl (3,0.0);
+
+// ******************************************************************************* //
+
+
+
+// VARIABLES FOR SOME "ALMOST CONSTANTS"
+// > Mass of the Crazyflie quad-rotor, in [grams]
+float cf_mass;
+// > Coefficients of the 16-bit command to thrust conversion
+std::vector<float> motorPoly(3);
+// The weight of the Crazyflie in [Newtons], i.e., mg
+float gravity_force;
+// One quarter of the "gravity_force"
+float gravity_force_quarter;
+
+
+
+
+// VARIABLES FOR THE CONTROLLER
+
+// Frequency at which the controller is running
+float vicon_frequency;
+
+// Frequency at which the controller is running
+float control_frequency;
+
+
+// > The setpoints for (x,y,z) position and yaw angle, in that order
+float setpoint[4] = {0.0,0.0,0.4,0.0};
+
+
+
+
+// The controller type to run in the "calculateControlOutput" function
+int controller_mode = CONTROLLER_MODE_LQR_RATE;
+
+// The LQR Controller parameters for "CONTROLLER_MODE_LQR_RATE"
+std::vector<float> gainMatrixThrust_NineStateVector (9,0.0);
+std::vector<float> gainMatrixRollRate (9,0.0);
+std::vector<float> gainMatrixPitchRate (9,0.0);
+std::vector<float> gainMatrixYawRate (9,0.0);
+
+// The LQR Controller parameters for "CONTROLLER_MODE_LQR_ANGLE"
+std::vector<float> gainMatrixThrust_SixStateVector (6,0.0);
+std::vector<float> gainMatrixRollAngle (6,0.0);
+std::vector<float> gainMatrixPitchAngle (6,0.0);
+
+// The LQR Controller parameters for "CONTROLLER_MODE_LQR_ANGLE_RATE_NESTED"
+std::vector<float> gainMatrixThrust_SixStateVector_50Hz (6,0.0);
+std::vector<float> gainMatrixRollAngle_50Hz (6,0.0);
+std::vector<float> gainMatrixPitchAngle_50Hz (6,0.0);
+
+std::vector<float> gainMatrixRollRate_Nested (3,0.0);
+std::vector<float> gainMatrixPitchRate_Nested (3,0.0);
+std::vector<float> gainMatrixYawRate_Nested (3,0.0);
+
+int lqr_angleRateNested_counter = 4;
+float lqr_angleRateNested_prev_thrustAdjustment = 0.0;
+float lqr_angleRateNested_prev_rollAngle = 0.0;
+float lqr_angleRateNested_prev_pitchAngle = 0.0;
+float lqr_angleRateNested_prev_yawAngle = 0.0;
+
+
+// The 16-bit command limits
+float cmd_sixteenbit_min;
+float cmd_sixteenbit_max;
+
+
+// VARIABLES FOR THE ESTIMATOR
+
+// Frequency at which the controller is running
+float estimator_frequency;
+
+// > A flag for which estimator to use:
+int estimator_method = ESTIMATOR_METHOD_FINITE_DIFFERENCE;
+// > The current state interial estimate,
+// for use by the controller
+float current_stateInertialEstimate[12];
+
+// > The measurement of the Crazyflie at the "current" time step,
+// to avoid confusion
+float current_xzy_rpy_measurement[6];
+
+// > The measurement of the Crazyflie at the "previous" time step,
+// used for computing finite difference velocities
+float previous_xzy_rpy_measurement[6];
+
+// > The full 12 state estimate maintained by the finite
+// difference state estimator
+float stateInterialEstimate_viaFiniteDifference[12];
+
+// > The full 12 state estimate maintained by the point mass
+// kalman filter state estimator
+float stateInterialEstimate_viaPointMassKalmanFilter[12];
+
+// THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
+// > For the (x,y,z) position
+std::vector<float> PMKF_Ahat_row1_for_positions (2,0.0);
+std::vector<float> PMKF_Ahat_row2_for_positions (2,0.0);
+std::vector<float> PMKF_Kinf_for_positions (2,0.0);
+// > For the (roll,pitch,yaw) angles
+std::vector<float> PMKF_Ahat_row1_for_angles (2,0.0);
+std::vector<float> PMKF_Ahat_row2_for_angles (2,0.0);
+std::vector<float> PMKF_Kinf_for_angles (2,0.0);
+
+
+
+// VARIABLES FOR THE NAMESPACES FOR THE PARAMETER SERVICES
+// > For the paramter service of this agent
+std::string namespace_to_own_agent_parameter_service;
+// > For the parameter service of the coordinator
+std::string namespace_to_coordinator_parameter_service;
+
+
+// ROS PUBLISHER FOR SENDING OUT THE DEBUG MESSAGES
+ros::Publisher debugPublisher;
+
+
+// VARIABLES RELATING TO PERFORMING THE CONVERSION INTO BODY FRAME
+
+// Boolean whether to execute the convert into body frame function
+bool shouldPerformConvertIntoBodyFrame = false;
+
+
+// VARIABLES RELATING TO THE PUBLISHING OF A DEBUG MESSAGE
+
+// Boolean indiciating whether the "Debug Message" of this agent should be published or not
+bool shouldPublishDebugMessage = false;
+
+// Boolean indiciating whether the debugging ROS_INFO_STREAM should be displayed or not
+bool shouldDisplayDebugInfo = false;
+
+
+// VARIABLES RELATING TO PUBLISHING CURRENT POSITION AND FOLLOWING ANOTHER AGENT'S
+// POSITION
+
+// The ID of this agent, i.e., the ID of this compute
+int my_agentID = 0;
+
+
+
+
+// RELEVANT NOTES ABOUT THE VARIABLES DECLARE HERE:
+// The "CrazyflieData" type used for the "request" variable is a
+// structure as defined in the file "CrazyflieData.msg" which has the following
+// properties:
+// string crazyflieName The name given to the Crazyflie in the Vicon software
+// float64 x The x position of the Crazyflie [metres]
+// float64 y The y position of the Crazyflie [metres]
+// float64 z The z position of the Crazyflie [metres]
+// float64 roll The roll component of the intrinsic Euler angles [radians]
+// float64 pitch The pitch component of the intrinsic Euler angles [radians]
+// float64 yaw The yaw component of the intrinsic Euler angles [radians]
+// float64 acquiringTime #delta t The time elapsed since the previous "CrazyflieData" was received [seconds]
+// bool occluded A boolean indicted whether the Crazyflie for visible at the time of this measurement
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// FFFFF U U N N CCCC TTTTT III OOO N N
+// F U U NN N C T I O O NN N
+// FFF U U N N N C T I O O N N N
+// F U U N NN C T I O O N NN
+// F UUU N N CCCC T III OOO N N
+//
+// PPPP RRRR OOO TTTTT OOO TTTTT Y Y PPPP EEEEE SSSS
+// P P R R O O T O O T Y Y P P E S
+// PPPP RRRR O O T O O T Y PPPP EEE SSS
+// P R R O O T O O T Y P E S
+// P R R OOO T OOO T Y P EEEEE SSSS
+// ----------------------------------------------------------------------------------
+
+// These function prototypes are not strictly required for this code to complile, but
+// adding the function prototypes here means the the functions can be written below in
+// any order. If the function prototypes are not included then the function need to
+// written below in an order that ensure each function is implemented before it is
+// called from another function, hence why the "main" function is at the bottom.
+
+// CONTROLLER COMPUTATIONS
+// > The function that is called to "start" all estimation and control computations
+bool calculateControlOutput(Controller::Request &request, Controller::Response &response);
+
+// > The various functions that implement an LQR controller
+void calculateControlOutput_viaLQR( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
+void calculateControlOutput_viaLQRforMotors( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
+void calculateControlOutput_viaLQRforActuators( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
+void calculateControlOutput_viaLQRforRates( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
+void calculateControlOutput_viaLQRforAngles( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
+void calculateControlOutput_viaLQRforAnglesRatesNested( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
+void calculateControlOutput_viaAngleResponseTest( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
+
+// ESTIMATOR COMPUTATIONS
+void performEstimatorUpdate_forStateInterial(Controller::Request &request);
+void performEstimatorUpdate_forStateInterial_viaFiniteDifference();
+void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter();
+
+
+// PUBLISHING OF THE DEBUG MESSAGE
+void construct_and_publish_debug_message(Controller::Request &request, Controller::Response &response);
+
+
+// TRANSFORMATION FROM INTERIAL ESTIMATE TO BODY FRAME ERROR
+void convert_stateInertial_to_bodyFrameError(float stateInertial[12], float setpoint[4], float (&bodyFrameError)[12]);
+
+// TRANSFORMATION OF THE (x,y) INERTIAL FRAME ERROR INTO AN (x,y) BODY FRAME ERROR
+void convertIntoBodyFrame(float stateInertial[12], float (&stateBody)[12], float yaw_measured);
+
+// CONVERSION FROM THRUST IN NEWTONS TO 16-BIT COMMAND
+float computeMotorPolyBackward(float thrust);
+
+// SETPOINT CHANGE CALLBACK
+void setpointCallback(const Setpoint& newSetpoint);
+
+
+
+// LOAD PARAMETERS
+float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name);
+void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length);
+int getParameterInt(ros::NodeHandle& nodeHandle, std::string name);
+void getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length);
+int getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val);
+bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name);
+std::string getParameterString(ros::NodeHandle& nodeHandle, std::string name);
+
+void yamlReadyForFetchCallback(const std_msgs::Int32& msg);
+void fetchYamlParameters(ros::NodeHandle& nodeHandle);
+void processFetchedParameters();
+
+
+
+// ******************************************************************************* //
+// FUNCTIONS SPECIFIC TO THE REMOTE CONTROL FEATURE
+
+void calculateControlOutput_forRemoteControl(float stateInertial[12], Controller::Request &request, Controller::Response &response);
+
+// CALLBACK WITH INFOMATION ABOUT WHICH REMOTE TO SUBSCRIBE TO
+void viconSubscribeObjectNameCallback(const ViconSubscribeObjectName& msg);
+
+// VICON DATA CALLBACK
+void viconCallback(const ViconData& viconData);
+
+// ACTIVATE REMOTE CONTROL MODE MESSAGE CALLBACK
+void shouldActivateCallback(const std_msgs::Int32& msg);
+
+// ******************************************************************************* //
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/include/nodes/SafeControllerService.h b/pps_ws/src/d_fall_pps/include/nodes/SafeControllerService.h
new file mode 100644
index 0000000000000000000000000000000000000000..19a1a90d349978f5b4c2392b9a5350400dae4305
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/include/nodes/SafeControllerService.h
@@ -0,0 +1,183 @@
+// Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat, Cyrill Burgener, Marco Mueller, Philipp Friedli
+//
+// 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:
+// The fall-back controller that keeps the Crazyflie safe
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+// ----------------------------------------------------------------------------------
+// III N N CCCC L U U DDDD EEEEE SSSS
+// I NN N C L U U D D E S
+// I N N N C L U U D D EEE SSS
+// I N NN C L U U D D E S
+// III N N CCCC LLLLL UUU DDDD EEEEE SSSS
+// ----------------------------------------------------------------------------------
+
+// These various headers need to be included so that this controller service can be
+// connected with the D-FaLL system.
+
+#include <math.h>
+#include <stdlib.h>
+#include "ros/ros.h"
+#include <std_msgs/String.h>
+#include <ros/package.h>
+#include "std_msgs/Float32.h"
+
+#include "d_fall_pps/CrazyflieData.h"
+#include "d_fall_pps/Setpoint.h"
+#include "d_fall_pps/ControlCommand.h"
+#include "d_fall_pps/Controller.h"
+#include "d_fall_pps/LoadYamlFiles.h"
+
+#include <std_msgs/Int32.h>
+
+// Include the shared definitions
+#include "nodes/ParameterServiceDefinitions.h"
+
+
+
+// ----------------------------------------------------------------------------------
+// DDDD EEEEE FFFFF III N N EEEEE SSSS
+// D D E F I NN N E S
+// D D EEE FFF I N N N EEE SSS
+// D D E F I N NN E S
+// DDDD EEEEE F III N N EEEEE SSSS
+// ----------------------------------------------------------------------------------
+
+// These constants are defined to make the code more readable and adaptable.
+
+// Universal constants
+#define PI 3.1415926535
+
+// The constants define the modes that can be used for controller the Crazyflie 2.0,
+// the constants defined here need to be in agreement with those defined in the
+// firmware running on the Crazyflie 2.0.
+// The following is a short description about each mode:
+// MOTOR_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
+// command directly to each of the motors
+// RATE_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
+// command directly to each of the motors, and additionally request the
+// body frame roll, pitch, and yaw angular rates from the PID rate
+// controllers implemented in the Crazyflie 2.0 firmware.
+// ANGE_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
+// command directly to each of the motors, and additionally request the
+// body frame roll, pitch, and yaw angles from the PID attitude
+// controllers implemented in the Crazyflie 2.0 firmware.
+#define MOTOR_MODE 6
+#define RATE_MODE 7
+#define ANGLE_MODE 8
+
+// Namespacing the package
+using namespace d_fall_pps;
+
+
+
+
+// ----------------------------------------------------------------------------------
+// V V A RRRR III A BBBB L EEEEE SSSS
+// V V A A R R I A A B B L E S
+// V V A A RRRR I A A BBBB L EEE SSS
+// V V AAAAA R R I AAAAA B B L E S
+// V A A R R III A A BBBB LLLLL EEEEE SSSS
+// ----------------------------------------------------------------------------------
+
+// The Load Yaml Files Service
+ros::ServiceClient loadYamlFilesService_own_agent;
+
+std::vector<float> ffThrust(4);
+std::vector<float> feedforwardMotor(4);
+float cf_mass;
+float gravity_force;
+std::vector<float> motorPoly(3);
+
+std::vector<float> gainMatrixRoll(9);
+std::vector<float> gainMatrixPitch(9);
+std::vector<float> gainMatrixYaw(9);
+std::vector<float> gainMatrixThrust(9);
+
+//K_infinite of feedback
+std::vector<float> filterGain(6);
+//only for velocity calculation
+std::vector<float> estimatorMatrix(2);
+float prevEstimate[9];
+
+std::vector<float> setpoint(4);
+std::vector<float> defaultSetpoint(4);
+float saturationThrust;
+
+CrazyflieData previousLocation;
+
+
+// Variable for the namespaces for the paramter services
+// > For the paramter service of this agent
+std::string namespace_to_own_agent_parameter_service;
+// > For the parameter service of the coordinator
+std::string namespace_to_coordinator_parameter_service;
+
+
+
+// ----------------------------------------------------------------------------------
+// FFFFF U U N N CCCC TTTTT III OOO N N
+// F U U NN N C T I O O NN N
+// FFF U U N N N C T I O O N N N
+// F U U N NN C T I O O N NN
+// F UUU N N CCCC T III OOO N N
+//
+// PPPP RRRR OOO TTTTT OOO TTTTT Y Y PPPP EEEEE SSSS
+// P P R R O O T O O T Y Y P P E S
+// PPPP RRRR O O T O O T Y PPPP EEE SSS
+// P R R O O T O O T Y P E S
+// P R R OOO T OOO T Y P EEEEE SSSS
+// ----------------------------------------------------------------------------------
+
+// These function prototypes are not strictly required for this code to complile, but
+// adding the function prototypes here means the the functions can be written below in
+// any order. If the function prototypes are not included then the function need to
+// written below in an order that ensure each function is implemented before it is
+// called from another function, hence why the "main" function is at the bottom.
+
+// > For the CONTROL LOOP
+bool calculateControlOutput(Controller::Request &request, Controller::Response &response);
+void convertIntoBodyFrame(float est[9], float (&state)[9], float yaw_measured);
+float computeMotorPolyBackward(float thrust);
+void estimateState(Controller::Request &request, float (&est)[9]);
+
+// > For the LOAD PARAMETERS
+void yamlReadyForFetchCallback(const std_msgs::Int32& msg);
+void fetchYamlParameters(ros::NodeHandle& nodeHandle);
+void processFetchedParameters();
+
+// > For the GETPARAM()
+float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name);
+void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length);
+int getParameterInt(ros::NodeHandle& nodeHandle, std::string name);
+void getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length);
+int getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val);
+bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name);
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/include/nodes/StudentControllerService.h b/pps_ws/src/d_fall_pps/include/nodes/StudentControllerService.h
new file mode 100644
index 0000000000000000000000000000000000000000..9c15a5341b35fe8dba9d6e9550d78d8f8b9b12e1
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/include/nodes/StudentControllerService.h
@@ -0,0 +1,225 @@
+// Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat, Angel Romero, Cyrill Burgener, Marco Mueller, Philipp Friedli
+//
+// 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:
+// Place for students to implement their controller
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// III N N CCCC L U U DDDD EEEEE SSSS
+// I NN N C L U U D D E S
+// I N N N C L U U D D EEE SSS
+// I N NN C L U U D D E S
+// III N N CCCC LLLLL UUU DDDD EEEEE SSSS
+// ----------------------------------------------------------------------------------
+
+// These various headers need to be included so that this controller service can be
+// connected with the D-FaLL system.
+
+//some useful libraries
+#include <math.h>
+#include <stdlib.h>
+#include "ros/ros.h"
+#include <ros/package.h>
+
+//the generated structs from the msg-files have to be included
+#include "d_fall_pps/ViconData.h"
+#include "d_fall_pps/Setpoint.h"
+#include "d_fall_pps/ControlCommand.h"
+#include "d_fall_pps/Controller.h"
+#include "d_fall_pps/DebugMsg.h"
+#include "d_fall_pps/CustomControllerYAML.h"
+#include "d_fall_pps/CustomButton.h"
+
+// Include the Parameter Service shared definitions
+#include "nodes/ParameterServiceDefinitions.h"
+
+#include <std_msgs/Int32.h>
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// DDDD EEEEE FFFFF III N N EEEEE SSSS
+// D D E F I NN N E S
+// D D EEE FFF I N N N EEE SSS
+// D D E F I N NN E S
+// DDDD EEEEE F III N N EEEEE SSSS
+// ----------------------------------------------------------------------------------
+
+// These constants are defined to make the code more readable and adaptable.
+
+// Universal constants
+#define PI 3.1415926535
+
+// These constants define the modes that can be used for controller the Crazyflie 2.0,
+// the constants defined here need to be in agreement with those defined in the
+// firmware running on the Crazyflie 2.0.
+// The following is a short description about each mode:
+// MOTOR_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
+// command directly to each of the motors
+// RATE_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
+// command directly to each of the motors, and additionally request the
+// body frame roll, pitch, and yaw angular rates from the PID rate
+// controllers implemented in the Crazyflie 2.0 firmware.
+// ANGE_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
+// command directly to each of the motors, and additionally request the
+// body frame roll, pitch, and yaw angles from the PID attitude
+// controllers implemented in the Crazyflie 2.0 firmware.
+#define MOTOR_MODE 6
+#define RATE_MODE 7
+#define ANGLE_MODE 8
+
+// These constants define the controller used for computing the response in the
+// "calculateControlOutput" function
+// The following is a short description about each mode:
+// LQR_RATE_MODE LQR controller based on the state vector:
+// [position,velocity,angles]
+//
+// LQR_ANGLE_MODE LQR controller based on the state vector:
+// [position,velocity]
+//
+#define LQR_RATE_MODE 1 // (DEFAULT)
+#define LQR_ANGLE_MODE 2
+
+// Namespacing the package
+using namespace d_fall_pps;
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// V V A RRRR III A BBBB L EEEEE SSSS
+// V V A A R R I A A B B L E S
+// V V A A RRRR I A A BBBB L EEE SSS
+// V V AAAAA R R I AAAAA B B L E S
+// V A A R R III A A BBBB LLLLL EEEEE SSSS
+// ----------------------------------------------------------------------------------
+
+// Variables for controller
+float cf_mass; // Crazyflie mass in grams
+std::vector<float> motorPoly(3); // Coefficients of the 16-bit command to thrust conversion
+float control_frequency = 200.0; // Frequency at which the controller is running
+float gravity_force = 0.0; // The weight of the Crazyflie in Newtons, i.e., mg
+
+float previous_stateErrorInertial[9]; // The location error of the Crazyflie at the "previous" time step
+
+std::vector<float> setpoint{0.0,0.0,0.4,0.0}; // The setpoints for (x,y,z) position and yaw angle, in that order
+
+
+// The LQR Controller parameters for "LQR_RATE_MODE"
+std::vector<float> gainMatrixRollRate = { 0.00,-1.71, 0.00, 0.00,-1.33, 0.00, 5.12, 0.00, 0.00};
+std::vector<float> gainMatrixPitchRate = { 1.71, 0.00, 0.00, 1.33, 0.00, 0.00, 0.00, 5.12, 0.00};
+std::vector<float> gainMatrixYawRate = { 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.84};
+std::vector<float> gainMatrixThrust = { 0.00, 0.00, 0.19, 0.00, 0.00, 0.08, 0.00, 0.00, 0.00};
+
+
+// ROS Publisher for debugging variables
+ros::Publisher debugPublisher;
+
+
+// Variable for the namespaces for the paramter services
+// > For the paramter service of this agent
+std::string namespace_to_own_agent_parameter_service;
+// > For the parameter service of the coordinator
+std::string namespace_to_coordinator_parameter_service;
+
+// The ID of this agent, i.e., the ID of this compute
+int my_agentID = 0;
+
+
+// RELEVANT NOTES ABOUT THE VARIABLES DECLARE HERE:
+// The "CrazyflieData" type used for the "request" variable is a
+// structure as defined in the file "CrazyflieData.msg" which has the following
+// properties:
+// string crazyflieName The name given to the Crazyflie in the Vicon software
+// float64 x The x position of the Crazyflie [metres]
+// float64 y The y position of the Crazyflie [metres]
+// float64 z The z position of the Crazyflie [metres]
+// float64 roll The roll component of the intrinsic Euler angles [radians]
+// float64 pitch The pitch component of the intrinsic Euler angles [radians]
+// float64 yaw The yaw component of the intrinsic Euler angles [radians]
+// float64 acquiringTime #delta t The time elapsed since the previous "CrazyflieData" was received [seconds]
+// bool occluded A boolean indicted whether the Crazyflie for visible at the time of this measurement
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// FFFFF U U N N CCCC TTTTT III OOO N N
+// F U U NN N C T I O O NN N
+// FFF U U N N N C T I O O N N N
+// F U U N NN C T I O O N NN
+// F UUU N N CCCC T III OOO N N
+//
+// PPPP RRRR OOO TTTTT OOO TTTTT Y Y PPPP EEEEE SSSS
+// P P R R O O T O O T Y Y P P E S
+// PPPP RRRR O O T O O T Y PPPP EEE SSS
+// P R R O O T O O T Y P E S
+// P R R OOO T OOO T Y P EEEEE SSSS
+// ----------------------------------------------------------------------------------
+
+// These function prototypes are not strictly required for this code to complile, but
+// adding the function prototypes here means the the functions can be written below in
+// any order. If the function prototypes are not included then the function need to
+// written below in an order that ensure each function is implemented before it is
+// called from another function, hence why the "main" function is at the bottom.
+
+// CONTROLLER COMPUTATIONS
+bool calculateControlOutput(Controller::Request &request, Controller::Response &response);
+
+// TRANSFORMATION OF THE (x,y) INERTIAL FRAME ERROR INTO AN (x,y) BODY FRAME ERROR
+void convertIntoBodyFrame(float stateInertial[9], float (&stateBody)[9], float yaw_measured);
+
+// CONVERSION FROM THRUST IN NEWTONS TO 16-BIT COMMAND
+float computeMotorPolyBackward(float thrust);
+
+// SETPOINT CHANGE CALLBACK
+void setpointCallback(const Setpoint& newSetpoint);
+
+// CUSTOM COMMAND RECEIVED CALLBACK
+void customCommandReceivedCallback(const CustomButton& commandReceived);
+
+// LOAD PARAMETERS
+float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name);
+void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length);
+int getParameterInt(ros::NodeHandle& nodeHandle, std::string name);
+void getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length);
+int getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val);
+bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name);
+
+void yamlReadyForFetchCallback(const std_msgs::Int32& msg);
+void fetchYamlParameters(ros::NodeHandle& nodeHandle);
+void processFetchedParameters();
+//void customYAMLasMessageCallback(const CustomControllerYAML& newCustomControllerParameters);
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/include/nodes/TuningControllerService.h b/pps_ws/src/d_fall_pps/include/nodes/TuningControllerService.h
new file mode 100644
index 0000000000000000000000000000000000000000..1bc722d03614311c8d55921492096e5750485cd3
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/include/nodes/TuningControllerService.h
@@ -0,0 +1,500 @@
+// Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat
+//
+// 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:
+// Place for students to implement their controller
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// III N N CCCC L U U DDDD EEEEE SSSS
+// I NN N C L U U D D E S
+// I N N N C L U U D D EEE SSS
+// I N NN C L U U D D E S
+// III N N CCCC LLLLL UUU DDDD EEEEE SSSS
+// ----------------------------------------------------------------------------------
+
+// These various headers need to be included so that this controller service can be
+// connected with the D-FaLL system.
+
+//some useful libraries
+#include <math.h>
+#include <stdlib.h>
+#include "ros/ros.h"
+#include <ros/package.h>
+
+//the generated structs from the msg-files have to be included
+#include "d_fall_pps/ViconData.h"
+#include "d_fall_pps/Setpoint.h"
+#include "d_fall_pps/ControlCommand.h"
+#include "d_fall_pps/Controller.h"
+#include "d_fall_pps/DebugMsg.h"
+#include "d_fall_pps/CustomButton.h"
+#include "d_fall_pps/ViconSubscribeObjectName.h"
+#include "d_fall_pps/CustomControllerYAML.h"
+
+// Include the Parameter Service shared definitions
+#include "nodes/ParameterServiceDefinitions.h"
+
+#include <std_msgs/Int32.h>
+
+
+
+
+// ----------------------------------------------------------------------------------
+// DDDD EEEEE FFFFF III N N EEEEE SSSS
+// D D E F I NN N E S
+// D D EEE FFF I N N N EEE SSS
+// D D E F I N NN E S
+// DDDD EEEEE F III N N EEEEE SSSS
+// ----------------------------------------------------------------------------------
+
+// These constants are defined to make the code more readable and adaptable.
+
+// Universal constants
+#define PI 3.1415926535
+
+// These constants define the modes that can be used for controller the Crazyflie 2.0,
+// the constants defined here need to be in agreement with those defined in the
+// firmware running on the Crazyflie 2.0.
+// The following is a short description about each mode:
+// MOTOR_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
+// command directly to each of the motors
+// RATE_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
+// command directly to each of the motors, and additionally request the
+// body frame roll, pitch, and yaw angular rates from the PID rate
+// controllers implemented in the Crazyflie 2.0 firmware.
+// ANGE_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
+// command directly to each of the motors, and additionally request the
+// body frame roll, pitch, and yaw angles from the PID attitude
+// controllers implemented in the Crazyflie 2.0 firmware.
+
+#define CF_COMMAND_TYPE_MOTOR 6
+#define CF_COMMAND_TYPE_RATE 7
+#define CF_COMMAND_TYPE_ANGLE 8
+
+
+// These constants define the controller used for computing the response in the
+// "calculateControlOutput" function
+// The following is a short description about each mode:
+//
+// LQR_MODE_MOTOR LQR controller based on the state vector:
+// [position,velocity,angles,angular velocity]
+// commands per motor thrusts
+//
+// LQR_MODE_ACTUATOR LQR controller based on the state vector:
+// [position,velocity,angles,angular velocity]
+// commands actuators of total force and bodz torques
+//
+// LQR_MODE_RATE LQR controller based on the state vector:
+// [position,velocity,angles]
+//
+// LQR_MODE_ANGLE LQR controller based on the state vector:
+// [position,velocity]
+//
+//#define CONTROLLER_MODE_LQR_MOTOR 1
+//#define CONTROLLER_MODE_LQR_ACTUATOR 2
+#define CONTROLLER_MODE_LQR_RATE 3 // (DEFAULT)
+#define CONTROLLER_MODE_LQR_ANGLE 4
+#define CONTROLLER_MODE_LQR_ANGLE_RATE_NESTED 5
+//#define CONTROLLER_MODE_ANGLE_RESPONSE_TEST 6
+
+
+// These constants define the method used for estimating the Inertial
+// frame state.
+// All methods are run at all times, this flag indicates which estimate
+// is passed onto the controller.
+// The following is a short description about each mode:
+//
+// ESTIMATOR_METHOD_FINITE_DIFFERENCE
+// Takes the poisition and angles directly as measured,
+// and estimates the velocities as a finite different to the
+// previous measurement
+//
+// ESTIMATOR_METHOD_POINT_MASS_PER_DIMENSION
+// Uses a 2nd order random walk estimator independently for
+// each of (x,y,z,roll,pitch,yaw)
+//
+// ESTIMATOR_METHOD_QUADROTOR_MODEL_BASED
+// Uses the model of the quad-rotor and the previous inputs
+//
+#define ESTIMATOR_METHOD_FINITE_DIFFERENCE 1
+#define ESTIMATOR_METHOD_POINT_MASS_PER_DIMENSION 2 // (DEFAULT)
+#define ESTIMATOR_METHOD_QUADROTOR_MODEL_BASED 3
+
+// These constants define the controller used for computing the response in the
+// "calculateControlOutput" function
+// The following is a short description about each mode:
+// LQR_RATE_MODE LQR controller based on the state vector:
+// [position,velocity,angles]
+//
+// LQR_ANGLE_MODE LQR controller based on the state vector:
+// [position,velocity]
+//
+#define LQR_RATE_MODE 1 // (DEFAULT)
+#define LQR_ANGLE_MODE 2
+
+// Namespacing the package
+using namespace d_fall_pps;
+
+
+// ----------------------------------------------------------------------------------
+// V V A RRRR III A BBBB L EEEEE SSSS
+// V V A A R R I A A B B L E S
+// V V A A RRRR I A A BBBB L EEE SSS
+// V V AAAAA R R I AAAAA B B L E S
+// V A A R R III A A BBBB LLLLL EEEEE SSSS
+// ----------------------------------------------------------------------------------
+
+
+// ******************************************************************************* //
+// VARIABLES SPECIFIC TO THE TUNING CONTROL FEATURE
+
+// Setpoint for the HORIZONTAL test
+int test_horizontal_currentpoint = 1;
+std::vector<float> test_horizontal_setpoint1 (4,0.0);
+std::vector<float> test_horizontal_setpoint2 (4,0.0);
+
+// Setpoint for the VERTICAL test
+int test_vertical_currentpoint = 1;
+std::vector<float> test_vertical_setpoint1 (4,0.0);
+std::vector<float> test_vertical_setpoint2 (4,0.0);
+
+// Setpoint for the HEADING test
+int test_heading_currentpoint = 1;
+std::vector<float> test_heading_setpoint1 (4,0.0);
+std::vector<float> test_heading_setpoint2 (4,0.0);
+
+// Setpoint for the ALL test
+int test_all_currentpoint = 1;
+std::vector<float> test_all_setpoint1 (4,0.0);
+std::vector<float> test_all_setpoint2 (4,0.0);
+
+// Parameters for flying in a circle
+float test_circle_radius = 0.2;
+float test_circle_seconds_per_rev = 5.0;
+float test_circle_height = 0.4;
+float test_circle_pirouette_per_rev = 0.0;
+float test_circle_time_to_reach_start = 0.7;
+bool isActive_fly_test_circle = false;
+int test_circle_ticks_since_start = 0;
+
+// Multipliers for the HORIZONTAL contorller
+float multiplier_horizontal = 0.1;
+float multiplier_horizontal_min = 0.9;
+float multiplier_horizontal_max = 1.1;
+// Multipliers for the VERTICAL contorller
+float multiplier_vertical = 0.1;
+float multiplier_vertical_min = 0.9;
+float multiplier_vertical_max = 1.1;
+// Multipliers for the HEADING contorller
+float multiplier_heading = 0.1;
+float multiplier_heading_min = 0.9;
+float multiplier_heading_max = 1.1;
+
+
+// LQR Gain matrix for tracking the angle commands from the Crazyflie
+std::vector<float> gainMatrixRollRate_forRemoteControl (3,0.0);
+std::vector<float> gainMatrixPitchRate_forRemoteControl (3,0.0);
+std::vector<float> gainMatrixYawRate_forRemoteControl (3,0.0);
+
+// ******************************************************************************* //
+
+
+
+// VARIABLES FOR SOME "ALMOST CONSTANTS"
+// > Mass of the Crazyflie quad-rotor, in [grams]
+float cf_mass;
+// > Coefficients of the 16-bit command to thrust conversion
+std::vector<float> motorPoly(3);
+// The weight of the Crazyflie in [Newtons], i.e., mg
+float gravity_force;
+// One quarter of the "gravity_force"
+float gravity_force_quarter;
+
+
+
+
+// VARIABLES FOR THE CONTROLLER
+
+// Frequency at which the controller is running
+float vicon_frequency;
+
+// Frequency at which the controller is running
+float control_frequency;
+
+
+// > The setpoints for (x,y,z) position and yaw angle, in that order
+float setpoint[4] = {0.0,0.0,0.4,0.0};
+
+
+
+
+// The controller type to run in the "calculateControlOutput" function
+int controller_mode = CONTROLLER_MODE_LQR_RATE;
+
+// The LQR Controller parameters for "CONTROLLER_MODE_LQR_RATE"
+std::vector<float> gainMatrixThrust_NineStateVector (9,0.0);
+std::vector<float> gainMatrixRollRate (9,0.0);
+std::vector<float> gainMatrixPitchRate (9,0.0);
+std::vector<float> gainMatrixYawRate (9,0.0);
+
+// The LQR Controller parameters for "CONTROLLER_MODE_LQR_ANGLE"
+std::vector<float> gainMatrixThrust_SixStateVector (6,0.0);
+std::vector<float> gainMatrixRollAngle (6,0.0);
+std::vector<float> gainMatrixPitchAngle (6,0.0);
+
+// The LQR Controller parameters for "CONTROLLER_MODE_LQR_ANGLE_RATE_NESTED"
+std::vector<float> gainMatrixThrust_SixStateVector_50Hz (6,0.0);
+std::vector<float> gainMatrixRollAngle_50Hz (6,0.0);
+std::vector<float> gainMatrixPitchAngle_50Hz (6,0.0);
+
+std::vector<float> gainMatrixRollRate_Nested (3,0.0);
+std::vector<float> gainMatrixPitchRate_Nested (3,0.0);
+std::vector<float> gainMatrixYawRate_Nested (3,0.0);
+
+int lqr_angleRateNested_counter = 4;
+float lqr_angleRateNested_prev_thrustAdjustment = 0.0;
+float lqr_angleRateNested_prev_rollAngle = 0.0;
+float lqr_angleRateNested_prev_pitchAngle = 0.0;
+float lqr_angleRateNested_prev_yawAngle = 0.0;
+
+
+// // SAME GAINS AGAIN BUT INSTEAD AS DEFAULTS
+
+
+// // The LQR Controller parameters for "CONTROLLER_MODE_LQR_RATE"
+// std::vector<float> gainMatrixThrust_NineStateVector_default (9,0.0);
+// std::vector<float> gainMatrixRollRate_default (9,0.0);
+// std::vector<float> gainMatrixPitchRate_default (9,0.0);
+// std::vector<float> gainMatrixYawRate_default (9,0.0);
+
+// // The LQR Controller parameters for "CONTROLLER_MODE_LQR_ANGLE"
+// std::vector<float> gainMatrixThrust_SixStateVector_default (6,0.0);
+// std::vector<float> gainMatrixRollAngle_default (6,0.0);
+// std::vector<float> gainMatrixPitchAngle_default (6,0.0);
+
+// // The LQR Controller parameters for "CONTROLLER_MODE_LQR_ANGLE_RATE_NESTED"
+// std::vector<float> gainMatrixThrust_SixStateVector_50Hz_default (6,0.0);
+// std::vector<float> gainMatrixRollAngle_50Hz_default (6,0.0);
+// std::vector<float> gainMatrixPitchAngle_50Hz_default (6,0.0);
+
+// std::vector<float> gainMatrixRollRate_Nested_default (3,0.0);
+// std::vector<float> gainMatrixPitchRate_Nested_default (3,0.0);
+// std::vector<float> gainMatrixYawRate_Nested_default (3,0.0);
+
+
+// The 16-bit command limits
+float cmd_sixteenbit_min;
+float cmd_sixteenbit_max;
+
+
+// VARIABLES FOR THE ESTIMATOR
+
+// Frequency at which the controller is running
+float estimator_frequency;
+
+// > A flag for which estimator to use:
+int estimator_method = ESTIMATOR_METHOD_FINITE_DIFFERENCE;
+// > The current state interial estimate,
+// for use by the controller
+float current_stateInertialEstimate[12];
+
+// > The measurement of the Crazyflie at the "current" time step,
+// to avoid confusion
+float current_xzy_rpy_measurement[6];
+
+// > The measurement of the Crazyflie at the "previous" time step,
+// used for computing finite difference velocities
+float previous_xzy_rpy_measurement[6];
+
+// > The full 12 state estimate maintained by the finite
+// difference state estimator
+float stateInterialEstimate_viaFiniteDifference[12];
+
+// > The full 12 state estimate maintained by the point mass
+// kalman filter state estimator
+float stateInterialEstimate_viaPointMassKalmanFilter[12];
+
+// THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
+// > For the (x,y,z) position
+std::vector<float> PMKF_Ahat_row1_for_positions (2,0.0);
+std::vector<float> PMKF_Ahat_row2_for_positions (2,0.0);
+std::vector<float> PMKF_Kinf_for_positions (2,0.0);
+// > For the (roll,pitch,yaw) angles
+std::vector<float> PMKF_Ahat_row1_for_angles (2,0.0);
+std::vector<float> PMKF_Ahat_row2_for_angles (2,0.0);
+std::vector<float> PMKF_Kinf_for_angles (2,0.0);
+
+
+
+// VARIABLES FOR THE NAMESPACES FOR THE PARAMETER SERVICES
+// > For the paramter service of this agent
+std::string namespace_to_own_agent_parameter_service;
+// > For the parameter service of the coordinator
+std::string namespace_to_coordinator_parameter_service;
+
+
+// ROS PUBLISHER FOR SENDING OUT THE DEBUG MESSAGES
+ros::Publisher debugPublisher;
+
+
+// VARIABLES RELATING TO PERFORMING THE CONVERSION INTO BODY FRAME
+
+// Boolean whether to execute the convert into body frame function
+bool shouldPerformConvertIntoBodyFrame = false;
+
+
+// VARIABLES RELATING TO THE PUBLISHING OF A DEBUG MESSAGE
+
+// Boolean indiciating whether the "Debug Message" of this agent should be published or not
+bool shouldPublishDebugMessage = false;
+
+// Boolean indiciating whether the debugging ROS_INFO_STREAM should be displayed or not
+bool shouldDisplayDebugInfo = false;
+
+
+// VARIABLES RELATING TO PUBLISHING CURRENT POSITION AND FOLLOWING ANOTHER AGENT'S
+// POSITION
+
+// The ID of this agent, i.e., the ID of this compute
+int my_agentID = 0;
+
+
+
+
+// RELEVANT NOTES ABOUT THE VARIABLES DECLARE HERE:
+// The "CrazyflieData" type used for the "request" variable is a
+// structure as defined in the file "CrazyflieData.msg" which has the following
+// properties:
+// string crazyflieName The name given to the Crazyflie in the Vicon software
+// float64 x The x position of the Crazyflie [metres]
+// float64 y The y position of the Crazyflie [metres]
+// float64 z The z position of the Crazyflie [metres]
+// float64 roll The roll component of the intrinsic Euler angles [radians]
+// float64 pitch The pitch component of the intrinsic Euler angles [radians]
+// float64 yaw The yaw component of the intrinsic Euler angles [radians]
+// float64 acquiringTime #delta t The time elapsed since the previous "CrazyflieData" was received [seconds]
+// bool occluded A boolean indicted whether the Crazyflie for visible at the time of this measurement
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// FFFFF U U N N CCCC TTTTT III OOO N N
+// F U U NN N C T I O O NN N
+// FFF U U N N N C T I O O N N N
+// F U U N NN C T I O O N NN
+// F UUU N N CCCC T III OOO N N
+//
+// PPPP RRRR OOO TTTTT OOO TTTTT Y Y PPPP EEEEE SSSS
+// P P R R O O T O O T Y Y P P E S
+// PPPP RRRR O O T O O T Y PPPP EEE SSS
+// P R R O O T O O T Y P E S
+// P R R OOO T OOO T Y P EEEEE SSSS
+// ----------------------------------------------------------------------------------
+
+// These function prototypes are not strictly required for this code to complile, but
+// adding the function prototypes here means the the functions can be written below in
+// any order. If the function prototypes are not included then the function need to
+// written below in an order that ensure each function is implemented before it is
+// called from another function, hence why the "main" function is at the bottom.
+
+// CONTROLLER COMPUTATIONS
+// > The function that is called to "start" all estimation and control computations
+bool calculateControlOutput(Controller::Request &request, Controller::Response &response);
+
+// > The various functions that implement an LQR controller
+void calculateControlOutput_viaLQR( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
+void calculateControlOutput_viaLQRforMotors( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
+void calculateControlOutput_viaLQRforActuators( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
+void calculateControlOutput_viaLQRforRates( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
+void calculateControlOutput_viaLQRforAngles( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
+void calculateControlOutput_viaLQRforAnglesRatesNested( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
+void calculateControlOutput_viaAngleResponseTest( float stateErrorBody[12], Controller::Request &request, Controller::Response &response);
+
+// ESTIMATOR COMPUTATIONS
+void performEstimatorUpdate_forStateInterial(Controller::Request &request);
+void performEstimatorUpdate_forStateInterial_viaFiniteDifference();
+void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter();
+
+
+// PUBLISHING OF THE DEBUG MESSAGE
+void construct_and_publish_debug_message(Controller::Request &request, Controller::Response &response);
+
+
+// TRANSFORMATION FROM INTERIAL ESTIMATE TO BODY FRAME ERROR
+void convert_stateInertial_to_bodyFrameError(float stateInertial[12], float setpoint[4], float (&bodyFrameError)[12]);
+
+// TRANSFORMATION OF THE (x,y) INERTIAL FRAME ERROR INTO AN (x,y) BODY FRAME ERROR
+void convertIntoBodyFrame(float stateInertial[12], float (&stateBody)[12], float yaw_measured);
+
+// CONVERSION FROM THRUST IN NEWTONS TO 16-BIT COMMAND
+float computeMotorPolyBackward(float thrust);
+
+// SETPOINT CHANGE CALLBACK
+void setpointCallback(const Setpoint& newSetpoint);
+
+
+
+// LOAD PARAMETERS
+float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name);
+void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length);
+int getParameterInt(ros::NodeHandle& nodeHandle, std::string name);
+void getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length);
+int getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val);
+bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name);
+std::string getParameterString(ros::NodeHandle& nodeHandle, std::string name);
+
+void yamlReadyForFetchCallback(const std_msgs::Int32& msg);
+void fetchYamlParameters(ros::NodeHandle& nodeHandle);
+void processFetchedParameters();
+
+
+
+// ******************************************************************************* //
+// FUNCTIONS SPECIFIC TO THE TUNING CONTROL FEATURE
+
+// ACTIVATE THE TESTS
+void activateTestCallback(const std_msgs::Int32& msg);
+// UPDATE SETPOINT FOR CIRCLE TEST
+void update_setpoint_for_test_circle();
+// CHANGE THE GAINS
+void horizontalGainCallback(const std_msgs::Int32& msg);
+void verticalGainCallback(const std_msgs::Int32& msg);
+void headingGainCallback(const std_msgs::Int32& msg);
+
+// ******************************************************************************* //
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/launch/Agent.launch b/pps_ws/src/d_fall_pps/launch/Agent.launch
new file mode 100755
index 0000000000000000000000000000000000000000..b9987cb9e60e804d3e52823dde0ce86eefff97c2
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/launch/Agent.launch
@@ -0,0 +1,145 @@
+<launch>
+
+ <!-- INPUT ARGUMENT OF THE AGENT's ID -->
+ <arg name="agentID" default="$(optenv DFALL_DEFAULT_AGENT_ID)" />
+
+ <!-- Example of how to use the value in agentID -->
+ <!-- <param name="param" value="$(arg agentID)"/> -->
+
+ <!-- Example of how to specify the agentID from command line -->
+ <!-- roslaunch d_fall_pps agentID:=1 -->
+
+ <group ns="agent$(arg agentID)">
+
+ <!-- CRAZY RADIO -->
+ <node
+ pkg = "d_fall_pps"
+ name = "CrazyRadio"
+ output = "screen"
+ type = "CrazyRadio.py"
+ >
+ <rosparam command="load" file="$(find d_fall_pps)/param/ClientConfig.yaml" />
+ </node>
+
+ <!-- PPS CLIENT -->
+ <node
+ pkg = "d_fall_pps"
+ name = "PPSClient"
+ output = "screen"
+ type = "PPSClient"
+ >
+ <rosparam command="load" file="$(find d_fall_pps)/param/ClientConfig.yaml" />
+ <param name="agentID" value="$(arg agentID)" />
+ </node>
+
+ <!-- SAFE CONTROLLER -->
+ <node
+ pkg = "d_fall_pps"
+ name = "SafeControllerService"
+ output = "screen"
+ type = "SafeControllerService"
+ >
+ </node>
+
+ <!-- DEMO CONTROLLER -->
+ <node
+ pkg = "d_fall_pps"
+ name = "DemoControllerService"
+ output = "screen"
+ type = "DemoControllerService"
+ >
+ </node>
+
+ <!-- STUDENT CONTROLLER -->
+ <node
+ pkg = "d_fall_pps"
+ name = "StudentControllerService"
+ output = "screen"
+ type = "StudentControllerService"
+ >
+ </node>
+
+ <!-- MPC CONTROLLER -->
+ <node
+ pkg = "d_fall_pps"
+ name = "MpcControllerService"
+ output = "screen"
+ type = "MpcControllerService"
+ >
+ </node>
+
+ <!-- REMOTE CONTROLLER -->
+ <node
+ pkg = "d_fall_pps"
+ name = "RemoteControllerService"
+ output = "screen"
+ type = "RemoteControllerService"
+ >
+ </node>
+
+ <!-- TUNING CONTROLLER -->
+ <node
+ pkg = "d_fall_pps"
+ name = "TuningControllerService"
+ output = "screen"
+ type = "TuningControllerService"
+ >
+ </node>
+
+ <!-- PARAMETER SERVICE -->
+ <node
+ pkg = "d_fall_pps"
+ name = "ParameterService"
+ output = "screen"
+ type = "ParameterService"
+ >
+ <param name="type" type="str" value="agent" />
+ <param name="agentID" type="str" value="$(arg agentID)" />
+ <rosparam
+ command = "load"
+ file = "$(find d_fall_pps)/param/YamlFileNames.yaml"
+ ns = "YamlFileNames"
+ />
+ <rosparam
+ command = "load"
+ file = "$(find d_fall_pps)/param/SafeController.yaml"
+ ns = "SafeController"
+ />
+ <rosparam
+ command = "load"
+ file = "$(find d_fall_pps)/param/DemoController.yaml"
+ ns = "DemoController"
+ />
+ <rosparam
+ command = "load"
+ file = "$(find d_fall_pps)/param/StudentController.yaml"
+ ns = "StudentController"
+ />
+ <rosparam
+ command = "load"
+ file = "$(find d_fall_pps)/param/MpcController.yaml"
+ ns = "MpcController"
+ />
+ <rosparam
+ command = "load"
+ file = "$(find d_fall_pps)/param/RemoteController.yaml"
+ ns = "RemoteController"
+ />
+ <rosparam
+ command = "load"
+ file = "$(find d_fall_pps)/param/TuningController.yaml"
+ ns = "TuningController"
+ />
+ </node>
+
+ <!-- AGENT GUI (aka. the "student GUI") -->
+ <node
+ pkg = "d_fall_pps"
+ name = "student_GUI"
+ output = "screen"
+ type = "student_GUI">
+ </node>
+
+ </group>
+
+</launch>
diff --git a/pps_ws/src/d_fall_pps/launch/Config.sh b/pps_ws/src/d_fall_pps/launch/Config.sh
index ef8a0e84bf79a691210e7852cb221242639e7f72..48d929ff5876d90a5548b4e1d0a49fcf657b3ac1 100755
--- a/pps_ws/src/d_fall_pps/launch/Config.sh
+++ b/pps_ws/src/d_fall_pps/launch/Config.sh
@@ -1,4 +1,6 @@
export ROS_MASTER_URI=http://teacher:11311
export ROS_IP=$(hostname -I | awk '{print $1;}')
-export ROS_NAMESPACE=$(cat /etc/StudentID)
+export DFALL_DEFAULT_AGENT_ID=$(cat /etc/dfall_default_agent_id)
+export DFALL_DEFAULT_COORD_ID=$(cat /etc/dfall_default_coord_id)
+export ROS_NAMESPACE='dfall'
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>
diff --git a/pps_ws/src/d_fall_pps/launch/Crazyflie.launch b/pps_ws/src/d_fall_pps/launch/Crazyflie.launch
deleted file mode 100755
index 003fc84e62208be400ab6a671b2cb5caf21f7131..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/launch/Crazyflie.launch
+++ /dev/null
@@ -1,33 +0,0 @@
-<launch>
- <node pkg="d_fall_pps" name="CentralManagerService" output="screen" type="CentralManagerService">
- </node>
-
- <node pkg="d_fall_pps" name="CrazyRadio" output="screen" type="CrazyRadio.py">
- <rosparam command="load" file="$(find d_fall_pps)/param/ClientConfig.yaml" />
- </node>
-
- <node pkg="d_fall_pps" name="ViconDataPublisher" output="screen" type="ViconDataPublisher">
- <rosparam command="load" file="$(find d_fall_pps)/param/ViconConfig.yaml" />
- </node>
-
- <node pkg="d_fall_pps" name="PPSClient" output="screen" type="PPSClient">
- <rosparam command="load" file="$(find d_fall_pps)/param/ClientConfig.yaml" />
- </node>
-
-
- <node pkg="d_fall_pps" name="SafeControllerService" output="screen" type="SafeControllerService">
- <rosparam command="load" file="$(find d_fall_pps)/param/SafeController.yaml" />
- </node>
-
- <node pkg="d_fall_pps" name="CustomControllerService" output="screen" type="CustomControllerService">
- </node>
-
-
- <node pkg="rqt_plot" name="commandplotter" type="rqt_plot" args="/PPSClient/ControlCommand/roll /PPSClient/ControlCommand/pitch /PPSClient/ControlCommand/yaw" />
-
- <!-- When we have a GUI, this has to be adapted and included -->
- <node pkg="d_fall_pps" name="my_GUI" output="screen" type="my_GUI">
- <!-- <rosparam command="load" file="$(find d_fall_pps)/launch/ControlParams.yaml" /> -->
- </node>
-
-</launch>
diff --git a/pps_ws/src/d_fall_pps/launch/FollowN_1.launch b/pps_ws/src/d_fall_pps/launch/FollowN_1.launch
deleted file mode 100644
index 750e4db856d11fe383b10fa7bcbae7ce739563f3..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/launch/FollowN_1.launch
+++ /dev/null
@@ -1,28 +0,0 @@
-<launch>
-
- <node pkg="d_fall_pps" name="CrazyRadio" output="screen" type="CrazyRadio.py">
- <rosparam command="load" file="$(find d_fall_pps)/param/ClientConfig.yaml" />
- </node>
-
- <node pkg="d_fall_pps" name="PPSClient" output="screen" type="PPSClient">
- <rosparam command="load" file="$(find d_fall_pps)/param/ClientConfig.yaml" />
- <param name="studentID" value="$(optenv ROS_NAMESPACE)" />
- </node>
-
- <node pkg="d_fall_pps" name="SafeControllerService" output="screen" type="SafeControllerService">
- <rosparam command="load" file="$(find d_fall_pps)/param/SafeController.yaml" />
- </node>
-
- <node pkg="d_fall_pps" name="FollowN_1Service" output="screen" type="FollowN_1Service">
- <rosparam command="load" file="$(find d_fall_pps)/param/SafeController.yaml" />
- </node>
-
- <!-- When we have a GUI, this has to be adapted and included -->
- <node pkg="d_fall_pps" name="student_GUI" output="screen" type="student_GUI">
- <!-- <rosparam command="load" file="$(find d_fall_pps)/launch/ControlParams.yaml" /> -->
- </node>
-
-
- <!-- <node pkg="rqt_plot" name="commandplotter" type="rqt_plot" args="/PPSClient/ControlCommand/roll /PPSClient/ControlCommand/pitch /PPSClient/ControlCommand/yaw" /> -->
-
-</launch>
diff --git a/pps_ws/src/d_fall_pps/launch/Student.launch b/pps_ws/src/d_fall_pps/launch/Student.launch
deleted file mode 100755
index 918c01067471dccce718677c983b50457b531112..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/launch/Student.launch
+++ /dev/null
@@ -1,34 +0,0 @@
-<launch>
-
- <!-- CRAZY RADIO -->
- <node pkg="d_fall_pps" name="CrazyRadio" output="screen" type="CrazyRadio.py">
- <rosparam command="load" file="$(find d_fall_pps)/param/ClientConfig.yaml" />
- </node>
-
- <!-- PPS CLIENT -->
- <node pkg="d_fall_pps" name="PPSClient" output="screen" type="PPSClient">
- <rosparam command="load" file="$(find d_fall_pps)/param/ClientConfig.yaml" />
- <param name="studentID" value="$(optenv ROS_NAMESPACE)" />
- </node>
-
- <!-- SAFE CONTROLLER -->
- <node pkg="d_fall_pps" name="SafeControllerService" output="screen" type="SafeControllerService">
- </node>
-
- <!-- CUSTOM CONTROLLER -->
- <node pkg="d_fall_pps" name="CustomControllerService" output="screen" type="CustomControllerService">
- </node>
-
- <!-- PARAMETER SERVICE -->
- <node pkg="d_fall_pps" name="ParameterService" output="screen" type="ParameterService">
- <param name="type" type="str" value="agent" />
- <param name="agentID" value="$(optenv ROS_NAMESPACE)" />
- <rosparam command="load" file="$(find d_fall_pps)/param/SafeController.yaml" ns="SafeController" />
- <rosparam command="load" file="$(find d_fall_pps)/param/CustomController.yaml" ns="CustomController" />
- </node>
-
- <!-- AGENT GUI (aka. the "student GUI") -->
- <node pkg="d_fall_pps" name="student_GUI" output="screen" type="student_GUI">
- </node>
-
-</launch>
diff --git a/pps_ws/src/d_fall_pps/launch/StudentCircle.launch b/pps_ws/src/d_fall_pps/launch/StudentCircle.launch
deleted file mode 100755
index 70a9972edcef1a8cc3705c89a4496c42efa60eea..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/launch/StudentCircle.launch
+++ /dev/null
@@ -1,23 +0,0 @@
-<launch>
-
- <node pkg="d_fall_pps" name="CrazyRadio" output="screen" type="CrazyRadio.py">
- <rosparam command="load" file="$(find d_fall_pps)/param/ClientConfig.yaml" />
- </node>
-
- <node pkg="d_fall_pps" name="PPSClient" output="screen" type="PPSClient">
- <rosparam command="load" file="$(find d_fall_pps)/param/ClientConfig.yaml" />
- <param name="studentID" value="$(optenv ROS_NAMESPACE)" />
- </node>
-
- <node pkg="d_fall_pps" name="SafeControllerService" output="screen" type="SafeControllerService">
- <rosparam command="load" file="$(find d_fall_pps)/param/SafeController.yaml" />
- </node>
-
- <node pkg="d_fall_pps" name="CircleControllerService" output="screen" type="CircleControllerService">
- <rosparam command="load" file="$(find d_fall_pps)/param/SafeController.yaml" />
- </node>
-
-
- <!-- <node pkg="rqt_plot" name="commandplotter" type="rqt_plot" args="/PPSClient/ControlCommand/roll /PPSClient/ControlCommand/pitch /PPSClient/ControlCommand/yaw" /> -->
-
-</launch>
diff --git a/pps_ws/src/d_fall_pps/launch/StudentFollow.launch b/pps_ws/src/d_fall_pps/launch/StudentFollow.launch
deleted file mode 100755
index 0dbd33c73925fb2619fb445cda3c1f7ab000d130..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/launch/StudentFollow.launch
+++ /dev/null
@@ -1,23 +0,0 @@
-<launch>
-
- <node pkg="d_fall_pps" name="CrazyRadio" output="screen" type="CrazyRadio.py">
- <rosparam command="load" file="$(find d_fall_pps)/param/ClientConfig.yaml" />
- </node>
-
- <node pkg="d_fall_pps" name="PPSClient" output="screen" type="PPSClient">
- <rosparam command="load" file="$(find d_fall_pps)/param/ClientConfig.yaml" />
- <param name="studentID" value="$(optenv ROS_NAMESPACE)" />
- </node>
-
- <node pkg="d_fall_pps" name="SafeControllerService" output="screen" type="SafeControllerService">
- <rosparam command="load" file="$(find d_fall_pps)/param/SafeController.yaml" />
- </node>
-
- <node pkg="d_fall_pps" name="FollowCrazyflieService" output="screen" type="FollowCrazyflieService">
- <rosparam command="load" file="$(find d_fall_pps)/param/SafeController.yaml" />
- </node>
-
-
- <!-- <node pkg="rqt_plot" name="commandplotter" type="rqt_plot" args="/PPSClient/ControlCommand/roll /PPSClient/ControlCommand/pitch /PPSClient/ControlCommand/yaw" /> -->
-
-</launch>
diff --git a/pps_ws/src/d_fall_pps/launch/Teacher.launch b/pps_ws/src/d_fall_pps/launch/Teacher.launch
index 1c2e6a9d63045b3969339f7f17d3ac0faf18e2b8..ef7b20a73aaf513096282fc723b2e33c8fe9d3d7 100755
--- a/pps_ws/src/d_fall_pps/launch/Teacher.launch
+++ b/pps_ws/src/d_fall_pps/launch/Teacher.launch
@@ -15,7 +15,7 @@
<param name="type" type="str" value="coordinator" />
<param name="agentID" value="0" />
<rosparam command="load" file="$(find d_fall_pps)/param/SafeController.yaml" ns="SafeController" />
- <rosparam command="load" file="$(find d_fall_pps)/param/CustomController.yaml" ns="CustomController" />
+ <rosparam command="load" file="$(find d_fall_pps)/param/DemoController.yaml" ns="CustomController" />
</node>
</launch>
diff --git a/pps_ws/src/d_fall_pps/msg/CustomControllerYAML.msg b/pps_ws/src/d_fall_pps/msg/CustomControllerYAML.msg
deleted file mode 100644
index ebe1dee883ffcd744fe81b94023e395d099775fc..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/msg/CustomControllerYAML.msg
+++ /dev/null
@@ -1,7 +0,0 @@
-float64 mass
-float64 control_frequency
-float64[] motorPoly
-bool shouldPerformConvertIntoBodyFrame
-bool shouldPublishCurrent_xyz_yaw
-bool shouldFollowAnotherAgent
-uint32[] follow_in_a_line_agentIDs
diff --git a/pps_ws/src/d_fall_pps/msg/ViconSubscribeObjectName.msg b/pps_ws/src/d_fall_pps/msg/ViconSubscribeObjectName.msg
new file mode 100644
index 0000000000000000000000000000000000000000..fbe1b0746c9daf3af001e7870357e6a87f4198bb
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/msg/ViconSubscribeObjectName.msg
@@ -0,0 +1,2 @@
+string objectName
+bool shouldSubscribe
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/param/ClientConfig.yaml b/pps_ws/src/d_fall_pps/param/ClientConfig.yaml
index 07c6cbedcd7a8ba61cdf13927c69c44003c25ce9..86e9ec7c01a2ad8335d7bc0a2281fefa49f9a9dc 100755
--- a/pps_ws/src/d_fall_pps/param/ClientConfig.yaml
+++ b/pps_ws/src/d_fall_pps/param/ClientConfig.yaml
@@ -1,8 +1,14 @@
-safeController: "SafeControllerService/RateController"
-customController: "CustomControllerService/CustomController"
+safeController: "SafeControllerService/RateController"
+demoController: "DemoControllerService/DemoController"
+studentController: "StudentControllerService/StudentController"
+mpcController: "MpcControllerService/MpcController"
+remoteController: "RemoteControllerService/RemoteController"
+tuningController: "TuningControllerService/TuningController"
+
strictSafety: true
-angleMargin: 0.6
-battery_threshold_while_flying: 2.9 # in V
-battery_threshold_while_motors_off: 3.34 # in V
-battery_polling_period: 100 # in ms
+angleMargin: 0.8
+
+battery_threshold_while_flying: 2.8 # in V
+battery_threshold_while_motors_off: 3.30 # in V
+battery_polling_period: 100 # in ms
diff --git a/pps_ws/src/d_fall_pps/param/DemoController.yaml b/pps_ws/src/d_fall_pps/param/DemoController.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..4601c05ebf929131b7ceface3dec8970ea80fee3
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/param/DemoController.yaml
@@ -0,0 +1,127 @@
+# Mass of the crazyflie
+mass : 29
+
+# Frequency of the controller, in hertz
+vicon_frequency : 200
+control_frequency : 200
+
+# Quadratic motor regression equation (a0, a1, a2)
+motorPoly : [5.484560e-4, 1.032633e-6, 2.130295e-11]
+
+# Boolean for whether to execute the convert into body frame function
+shouldPerformConvertIntoBodyFrame : true
+
+# Boolean indiciating whether the (x,y,z,yaw) of this agent should be published or not
+shouldPublishCurrent_xyz_yaw : true
+
+# Boolean indicating whether the (x,y,z,yaw) setpoint of this agent should adapted in
+# an attempt to follow the "my_current_xyz_yaw_topic" from another agent
+shouldFollowAnotherAgent : false
+
+# The order in which agents should follow eachother
+# > This parameter is a vector of integers that specifies agent ID's
+# > The order of the agent ID's is the ordering of the line formation
+# > i.e., the first ID is the leader, the second ID should follow the first ID, and so on
+follow_in_a_line_agentIDs : [5, 6, 7]
+
+# Boolean indiciating whether the "Debug Message" of this agent should be published or not
+shouldPublishDebugMessage : true
+
+# Boolean indiciating whether the debugging ROS_INFO_STREAM should be displayed or not
+shouldDisplayDebugInfo : false
+
+
+# A flag for which controller mode to use, defined as:
+# 1 CONTROLLER_MODE_LQR_MOTOR
+# - LQR controller based on the state vector: [position,velocity,angles,angular velocity]
+# commands per motor thrusts
+# 2 CONTROLLER_MODE_LQR_ACTUATOR
+# - LQR controller based on the state vector: [position,velocity,angles,angular velocity]
+# commands actuators of total force and bodz torques
+# 3 CONTROLLER_MODE_LQR_RATE
+# - LQR controller based on the state vector: [position,velocity,angles]
+# 4 CONTROLLER_MODE_LQR_ANGLE
+# - LQR controller based on the state vector: [position,velocity]
+# 5 CONTROLLER_MODE_LQR_ANGLE_RATE_NESTED
+# - LQR Nested angle and rate controller
+# 6 CONTROLLER_MODE_ANGLE_RESPONSE_TEST
+# - Swaps between pitch set-points to test angle set-point response time
+# i.e., this controller test the assumption that "the inner loop is infinitely fast"
+#
+controller_mode : 3
+
+
+# A flag for which estimator to use, defined as:
+# 1 - Finite Different Method,
+# Takes the poisition and angles directly as measured,
+# and estimates the velocities as a finite different to the
+# previous measurement
+# 2 - Point Mass Per Dimension Method
+# Uses a 2nd order random walk estimator independently for
+# each of (x,y,z,roll,pitch,yaw)
+# 3 - Quad-rotor Model Based Method
+# Uses the model of the quad-rotor and the previous inputs
+estimator_method : 1
+
+
+# The LQR Controller parameters for "mode = 1"
+gainMatrixMotor1 : [ -0.0180, 0.0180, 0.0350,-0.0110, 0.0110, 0.0260,-0.0220,-0.0220,-0.0110,-0.00024,-0.00024,-0.00045]
+gainMatrixMotor2 : [ 0.0180, 0.0180, 0.0350, 0.0110, 0.0110, 0.0260,-0.0220, 0.0220, 0.0110,-0.00024, 0.00024, 0.00045]
+gainMatrixMotor3 : [ 0.0180,-0.0180, 0.0350, 0.0110,-0.0110, 0.0260, 0.0220, 0.0220,-0.0110, 0.00024, 0.00024,-0.00045]
+gainMatrixMotor4 : [ -0.0180,-0.0180, 0.0350,-0.0110,-0.0110, 0.0260, 0.0220,-0.0220, 0.0110, 0.00024,-0.00024, 0.00045]
+
+# The LQR Controller parameters for "mode = 2"
+gainMatrixThrust_TwelveStateVector : [ 0.0000, 0.0000, 0.9800, 0.0000, 0.0000, 0.25, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000]
+gainMatrixRollTorque : [ 0.0000,-0.0017, 0.0000, 0.0000,-0.0009, 0.00, 0.0019, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000]
+gainMatrixPitchTorque : [ 0.0017, 0.0000, 0.0000, 0.0009, 0.0000, 0.00, 0.0000, 0.0019, 0.0000, 0.0000, 0.0000, 0.0000]
+gainMatrixYawTorque : [ 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.00, 0.0000, 0.0000, 0.0002, 0.0000, 0.0000, 0.0000]
+
+# The LQR Controller parameters for "mode = 3"
+gainMatrixThrust_NineStateVector : [ 0.00, 0.00, 0.98, 0.00, 0.00, 0.25, 0.00, 0.00, 0.00]
+gainMatrixRollRate : [ 0.00,-6.20, 0.00, 0.00,-3.00, 0.00, 5.20, 0.00, 0.00]
+gainMatrixPitchRate : [ 6.20, 0.00, 0.00, 3.00, 0.00, 0.00, 0.00, 5.20, 0.00]
+gainMatrixYawRate : [ 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.30]
+
+# The LQR Controller parameters for "mode = 4"
+gainMatrixThrust_SixStateVector : [ 0.00, 0.00, 0.98, 0.00, 0.00, 0.25]
+gainMatrixRollAngle : [ 0.00,-0.20, 0.00, 0.00,-0.20, 0.00]
+gainMatrixPitchAngle : [ 0.20, 0.00, 0.00, 0.20, 0.00, 0.00]
+
+
+# The LQR Controller parameters for "mode = 5"
+gainMatrixThrust_SixStateVector_50Hz: [ 0.00, 0.00, 0.82, 0.00, 0.00, 0.22]
+gainMatrixRollAngle_50Hz : [ 0.00,-0.31, 0.00, 0.00,-0.25, 0.00]
+gainMatrixPitchAngle_50Hz : [ 0.31, 0.00, 0.00, 0.25, 0.00, 0.00]
+
+gainMatrixRollRate_Nested : [ 4.00, 0.00, 0.00]
+gainMatrixPitchRate_Nested : [ 0.00, 4.00, 0.00]
+gainMatrixYawRate_Nested : [ 0.00, 0.00, 2.30]
+
+
+# The paramters for the "Angle Reponse Test" controller mode
+angleResponseTest_pitchAngle_degrees : 7
+angleResponseTest_distance_meters : 0.02
+
+
+
+
+# The max and minimum thrust for a 16-bit command
+command_sixteenbit_min : 1000
+command_sixteenbit_max : 60000
+
+
+# THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
+# > For the (x,y,z) position
+PMKF_Ahat_row1_for_positions : [ 0.6723, 0.0034]
+PMKF_Ahat_row2_for_positions : [-12.9648, 0.9352]
+PMKF_Kinf_for_positions : [ 0.3277,12.9648]
+
+
+# > For the (roll,pitch,yaw) angles
+PMKF_Ahat_row1_for_angles : [ 0.6954, 0.0035]
+PMKF_Ahat_row2_for_angles : [-11.0342, 0.9448]
+PMKF_Kinf_for_angles : [ 0.3046,11.0342]
+
+#PMKF_Ahat_row1_for_angles : [ 0.6723, 0.0034]
+#PMKF_Ahat_row2_for_angles : [-12.9648, 0.9352]
+#PMKF_Kinf_for_angles : [ 0.3277,12.9648]
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/param/MpcController.yaml b/pps_ws/src/d_fall_pps/param/MpcController.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..8288be1dafa339d67b42ebc30ec65174d028b37b
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/param/MpcController.yaml
@@ -0,0 +1,24 @@
+# Mass of the crazyflie
+mass : 27
+
+# Frequency of the controller, in hertz
+control_frequency : 200
+
+# Quadratic motor regression equation (a0, a1, a2)
+motorPoly : [5.484560e-4, 1.032633e-6, 2.130295e-11]
+
+gainMatrixRollRate_Nested : [ 4.00, 0.00, 0.00]
+gainMatrixPitchRate_Nested : [ 0.00, 4.00, 0.00]
+gainMatrixYawRate_Nested : [ 0.00, 0.00, 2.30]
+
+# THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
+# > For the (x,y,z) position
+PMKF_Ahat_row1_for_positions : [ 0.6723, 0.0034]
+PMKF_Ahat_row2_for_positions : [-12.9648, 0.9352]
+PMKF_Kinf_for_positions : [ 0.3277,12.9648]
+
+
+# > For the (roll,pitch,yaw) angles
+PMKF_Ahat_row1_for_angles : [ 0.6954, 0.0035]
+PMKF_Ahat_row2_for_angles : [-11.0342, 0.9448]
+PMKF_Kinf_for_angles : [ 0.3046,11.0342]
diff --git a/pps_ws/src/d_fall_pps/param/RemoteController.yaml b/pps_ws/src/d_fall_pps/param/RemoteController.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..7653cf734f2a018f877353bdb9acb2a0d5aa1dd4
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/param/RemoteController.yaml
@@ -0,0 +1,121 @@
+# ***************************************************************************** #
+# PARAMETERS SPECIFIC TO THE REMOTE CONTROL FEATURE
+
+# Vicon object name of the Remote to follow
+default_viconObjectName_forRemote : 'DFALL_REMOTE01'
+
+# Boolean for whether the Remote's state should be published as a message
+shouldPublishRemote_xyz_rpy : True
+
+# Boolean for whether the Remote's state should be display in the terminal window
+shouldDisplayRemote_xyz_rpy : False
+
+# Roll and pitch limit (in degrees for angles)
+remoteControlLimit_roll_degrees : 15
+remoteControlLimit_pitch_degrees : 15
+
+# Factor by which to reduce the remote control input
+remoteConrtolSetpointFactor_roll : 0.5
+remoteConrtolSetpointFactor_pitch : 0.5
+remoteConrtolSetpointFactor_yaw : 1.0
+remoteConrtolSetpointFactor_z : 1.0
+
+# LQR Gain matrix for tracking the angle commands from the Crazyflie
+gainMatrixRollRate_forRemoteControl : [ 5.00, 0.00, 0.00]
+gainMatrixPitchRate_forRemoteControl : [ 0.00, 5.00, 0.00]
+gainMatrixYawRate_forRemoteControl : [ 0.00, 0.00, 2.80]
+
+# ***************************************************************************** #
+
+
+
+# Mass of the crazyflie
+mass : 29
+
+# Frequency of the controller, in hertz
+vicon_frequency : 200
+control_frequency : 200
+
+# Quadratic motor regression equation (a0, a1, a2)
+motorPoly : [5.484560e-4, 1.032633e-6, 2.130295e-11]
+
+# Boolean for whether to execute the convert into body frame function
+shouldPerformConvertIntoBodyFrame : true
+
+# Boolean indiciating whether the "Debug Message" of this agent should be published or not
+shouldPublishDebugMessage : false
+
+# Boolean indiciating whether the debugging ROS_INFO_STREAM should be displayed or not
+shouldDisplayDebugInfo : false
+
+
+# A flag for which controller mode to use, defined as:
+# 1 CONTROLLER_MODE_LQR_MOTOR
+# - LQR controller based on the state vector: [position,velocity,angles,angular velocity]
+# commands per motor thrusts
+# 2 CONTROLLER_MODE_LQR_ACTUATOR
+# - LQR controller based on the state vector: [position,velocity,angles,angular velocity]
+# commands actuators of total force and bodz torques
+# 3 CONTROLLER_MODE_LQR_RATE
+# - LQR controller based on the state vector: [position,velocity,angles]
+# 4 CONTROLLER_MODE_LQR_ANGLE
+# - LQR controller based on the state vector: [position,velocity]
+# 5 CONTROLLER_MODE_LQR_ANGLE_RATE_NESTED
+# - LQR Nested angle and rate controller
+# 6 CONTROLLER_MODE_ANGLE_RESPONSE_TEST
+# - Swaps between pitch set-points to test angle set-point response time
+# i.e., this controller test the assumption that "the inner loop is infinitely fast"
+#
+controller_mode : 3
+
+
+# A flag for which estimator to use, defined as:
+# 1 - Finite Different Method,
+# Takes the poisition and angles directly as measured,
+# and estimates the velocities as a finite different to the
+# previous measurement
+# 2 - Point Mass Per Dimension Method
+# Uses a 2nd order random walk estimator independently for
+# each of (x,y,z,roll,pitch,yaw)
+# 3 - Quad-rotor Model Based Method
+# Uses the model of the quad-rotor and the previous inputs
+estimator_method : 1
+
+
+# The LQR Controller parameters for "mode = 3"
+gainMatrixThrust_NineStateVector : [ 0.00, 0.00, 0.98, 0.00, 0.00, 0.25, 0.00, 0.00, 0.00]
+gainMatrixRollRate : [ 0.00,-1.80, 0.00, 0.00,-1.38, 0.00, 5.20, 0.00, 0.00]
+gainMatrixPitchRate : [ 1.80, 0.00, 0.00, 1.38, 0.00, 0.00, 0.00, 5.20, 0.00]
+gainMatrixYawRate : [ 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.30]
+
+# The LQR Controller parameters for "mode = 4"
+gainMatrixThrust_SixStateVector : [ 0.00, 0.00, 0.98, 0.00, 0.00, 0.25]
+gainMatrixRollAngle : [ 0.00,-0.20, 0.00, 0.00,-0.20, 0.00]
+gainMatrixPitchAngle : [ 0.20, 0.00, 0.00, 0.20, 0.00, 0.00]
+
+# The LQR Controller parameters for "mode = 5"
+gainMatrixThrust_SixStateVector_50Hz: [ 0.00, 0.00, 0.82, 0.00, 0.00, 0.22]
+gainMatrixRollAngle_50Hz : [ 0.00,-0.31, 0.00, 0.00,-0.25, 0.00]
+gainMatrixPitchAngle_50Hz : [ 0.31, 0.00, 0.00, 0.25, 0.00, 0.00]
+
+gainMatrixRollRate_Nested : [ 4.00, 0.00, 0.00]
+gainMatrixPitchRate_Nested : [ 0.00, 4.00, 0.00]
+gainMatrixYawRate_Nested : [ 0.00, 0.00, 2.30]
+
+
+# The max and minimum thrust for a 16-bit command
+command_sixteenbit_min : 1000
+command_sixteenbit_max : 60000
+
+
+# THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
+# > For the (x,y,z) position
+PMKF_Ahat_row1_for_positions : [ 0.6723, 0.0034]
+PMKF_Ahat_row2_for_positions : [-12.9648, 0.9352]
+PMKF_Kinf_for_positions : [ 0.3277,12.9648]
+
+
+# > For the (roll,pitch,yaw) angles
+PMKF_Ahat_row1_for_angles : [ 0.6954, 0.0035]
+PMKF_Ahat_row2_for_angles : [-11.0342, 0.9448]
+PMKF_Kinf_for_angles : [ 0.3046,11.0342]
diff --git a/pps_ws/src/d_fall_pps/param/SafeController.yaml b/pps_ws/src/d_fall_pps/param/SafeController.yaml
index 60efb4480984c85a640d235799b61a0eaa7da4a5..a64ccb8acea5b6c20b5b218a1798231b4c4b5ca0 100644
--- a/pps_ws/src/d_fall_pps/param/SafeController.yaml
+++ b/pps_ws/src/d_fall_pps/param/SafeController.yaml
@@ -7,7 +7,7 @@ motorPoly: [5.484560e-4, 1.032633e-6, 2.130295e-11]
gainMatrixRoll: [0, -1.714330725, 0, 0, -1.337107465, 0, 5.115369735, 0, 0]
gainMatrixPitch: [1.714330725, 0, 0, 1.337107465, 0, 0, 0, 5.115369735, 0]
gainMatrixYaw: [0, 0, 0, 0, 0, 0, 0, 0, 2.843099534]
-gainMatrixThrust: [0, 0, 0.22195826, 0, 0, 0.12362477, 0, 0, 0]
+gainMatrixThrust: [0, 0, 0.19195826, 0, 0, 0.08362477, 0, 0, 0]
#kalman filter
filterGain: [1, 1, 1, 22.3384, 22.3384, 22.3384] #K_infinite of feedback
@@ -18,9 +18,9 @@ defaultSetpoint: [0.0, 0.0, 0.4, 0.0]
#take off and landing parameters (in meters and seconds)
takeOffDistance : 0.4
-landingDistance : 0.05
-durationTakeOff : 1.5
-durationLanding : 2
+landingDistance : -0.05
+durationTakeOff : 1.0
+durationLanding : 2.0
# Liner Trayectory following parameters
distanceThreshold : 0.5
diff --git a/pps_ws/src/d_fall_pps/param/CustomController.yaml b/pps_ws/src/d_fall_pps/param/StudentController.yaml
similarity index 95%
rename from pps_ws/src/d_fall_pps/param/CustomController.yaml
rename to pps_ws/src/d_fall_pps/param/StudentController.yaml
index 69c089c90b825bc1ad54dd030a398ea65adc64db..7dbb3ec7780a586be493fa688907a364ad214061 100644
--- a/pps_ws/src/d_fall_pps/param/CustomController.yaml
+++ b/pps_ws/src/d_fall_pps/param/StudentController.yaml
@@ -1,5 +1,5 @@
# Mass of the crazyflie
-mass : 31
+mass : 28
# Frequency of the controller, in hertz
control_frequency : 200
diff --git a/pps_ws/src/d_fall_pps/param/TuningController.yaml b/pps_ws/src/d_fall_pps/param/TuningController.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..0bf1cb89869c7d2462bfd4db9f5d5162f1905a2d
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/param/TuningController.yaml
@@ -0,0 +1,131 @@
+# ***************************************************************************** #
+# PARAMETERS SPECIFIC TO THE REMOTE CONTROL FEATURE
+
+# Setpoint for the HORIZONTAL test
+test_horizontal_setpoint1 : [0.3, 0.0, 0.4, 0.0]
+test_horizontal_setpoint2 : [-0.3, 0.0, 0.4, 0.0]
+
+# Setpoint for the VERTICAL test
+test_vertical_setpoint1 : [0.0, 0.0, 0.4, 0.0]
+test_vertical_setpoint2 : [0.0, 0.0, 0.7, 0.0]
+
+# Setpoint for the HEADING test
+test_heading_setpoint1 : [0.0, 0.0, 0.4, 0.0]
+test_heading_setpoint2 : [0.0, 0.0, 0.4, 3.14]
+
+# Setpoint for the ALL test
+test_all_setpoint1 : [0.3, 0.0, 0.4, 0.0]
+test_all_setpoint2 : [-0.3, 0.0, 0.7, 1.57]
+
+# Parameters for flying in a circle
+test_circle_radius : 0.40
+test_circle_seconds_per_rev : 3.0
+test_circle_height : 0.4
+test_circle_pirouette_per_rev : 0.0
+test_circle_time_to_reach_start : 0.7
+
+# Multipliers for the HORIZONTAL contorller
+multiplier_horizontal_min : 0.1
+multiplier_horizontal_max : 2.2
+# Multipliers for the VERTICAL contorller
+multiplier_vertical_min : 0.1
+multiplier_vertical_max : 1.7
+# Multipliers for the HEADING contorller
+multiplier_heading_min : 0.05
+multiplier_heading_max : 3.0
+
+
+# ***************************************************************************** #
+
+
+
+# Mass of the crazyflie
+mass : 29
+
+# Frequency of the controller, in hertz
+vicon_frequency : 200
+control_frequency : 200
+
+# Quadratic motor regression equation (a0, a1, a2)
+motorPoly : [5.484560e-4, 1.032633e-6, 2.130295e-11]
+
+# Boolean for whether to execute the convert into body frame function
+shouldPerformConvertIntoBodyFrame : true
+
+# Boolean indiciating whether the "Debug Message" of this agent should be published or not
+shouldPublishDebugMessage : false
+
+# Boolean indiciating whether the debugging ROS_INFO_STREAM should be displayed or not
+shouldDisplayDebugInfo : false
+
+
+# A flag for which controller mode to use, defined as:
+# 1 CONTROLLER_MODE_LQR_MOTOR
+# - LQR controller based on the state vector: [position,velocity,angles,angular velocity]
+# commands per motor thrusts
+# 2 CONTROLLER_MODE_LQR_ACTUATOR
+# - LQR controller based on the state vector: [position,velocity,angles,angular velocity]
+# commands actuators of total force and bodz torques
+# 3 CONTROLLER_MODE_LQR_RATE
+# - LQR controller based on the state vector: [position,velocity,angles]
+# 4 CONTROLLER_MODE_LQR_ANGLE
+# - LQR controller based on the state vector: [position,velocity]
+# 5 CONTROLLER_MODE_LQR_ANGLE_RATE_NESTED
+# - LQR Nested angle and rate controller
+# 6 CONTROLLER_MODE_ANGLE_RESPONSE_TEST
+# - Swaps between pitch set-points to test angle set-point response time
+# i.e., this controller test the assumption that "the inner loop is infinitely fast"
+#
+controller_mode : 3
+
+
+# A flag for which estimator to use, defined as:
+# 1 - Finite Different Method,
+# Takes the poisition and angles directly as measured,
+# and estimates the velocities as a finite different to the
+# previous measurement
+# 2 - Point Mass Per Dimension Method
+# Uses a 2nd order random walk estimator independently for
+# each of (x,y,z,roll,pitch,yaw)
+# 3 - Quad-rotor Model Based Method
+# Uses the model of the quad-rotor and the previous inputs
+estimator_method : 1
+
+
+# The LQR Controller parameters for "mode = 3"
+gainMatrixThrust_NineStateVector : [ 0.00, 0.00, 0.98, 0.00, 0.00, 0.25, 0.00, 0.00, 0.00]
+gainMatrixRollRate : [ 0.00,-1.80, 0.00, 0.00,-1.38, 0.00, 5.20, 0.00, 0.00]
+gainMatrixPitchRate : [ 1.80, 0.00, 0.00, 1.38, 0.00, 0.00, 0.00, 5.20, 0.00]
+gainMatrixYawRate : [ 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.30]
+
+# The LQR Controller parameters for "mode = 4"
+gainMatrixThrust_SixStateVector : [ 0.00, 0.00, 0.98, 0.00, 0.00, 0.25]
+gainMatrixRollAngle : [ 0.00,-0.20, 0.00, 0.00,-0.20, 0.00]
+gainMatrixPitchAngle : [ 0.20, 0.00, 0.00, 0.20, 0.00, 0.00]
+
+# The LQR Controller parameters for "mode = 5"
+gainMatrixThrust_SixStateVector_50Hz: [ 0.00, 0.00, 0.82, 0.00, 0.00, 0.22]
+gainMatrixRollAngle_50Hz : [ 0.00,-0.31, 0.00, 0.00,-0.25, 0.00]
+gainMatrixPitchAngle_50Hz : [ 0.31, 0.00, 0.00, 0.25, 0.00, 0.00]
+
+gainMatrixRollRate_Nested : [ 4.00, 0.00, 0.00]
+gainMatrixPitchRate_Nested : [ 0.00, 4.00, 0.00]
+gainMatrixYawRate_Nested : [ 0.00, 0.00, 2.30]
+
+
+# The max and minimum thrust for a 16-bit command
+command_sixteenbit_min : 1000
+command_sixteenbit_max : 60000
+
+
+# THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
+# > For the (x,y,z) position
+PMKF_Ahat_row1_for_positions : [ 0.6723, 0.0034]
+PMKF_Ahat_row2_for_positions : [-12.9648, 0.9352]
+PMKF_Kinf_for_positions : [ 0.3277,12.9648]
+
+
+# > For the (roll,pitch,yaw) angles
+PMKF_Ahat_row1_for_angles : [ 0.6954, 0.0035]
+PMKF_Ahat_row2_for_angles : [-11.0342, 0.9448]
+PMKF_Kinf_for_angles : [ 0.3046,11.0342]
diff --git a/pps_ws/src/d_fall_pps/param/YamlFileNames.yaml b/pps_ws/src/d_fall_pps/param/YamlFileNames.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..53fd75c2ba6f14e86dfcd8e8809314c35fd9a8af
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/param/YamlFileNames.yaml
@@ -0,0 +1,4 @@
+dictionary : {
+ 'ClientConfig' : 'ClientConfig' ,
+ 'SafeController' : 'SafeController'
+}
diff --git a/pps_ws/src/d_fall_pps/src/CircleControllerService.cpp b/pps_ws/src/d_fall_pps/src/CircleControllerService.cpp
deleted file mode 100755
index 0f6c387a8232d16b588ead699b0e90c5b94238be..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/src/CircleControllerService.cpp
+++ /dev/null
@@ -1,258 +0,0 @@
-// Copyright (C) 2017, ETH Zurich, D-ITET, Cyrill Burgener, Marco Mueller, Philipp Friedli
-//
-// 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:
-// Alternate controller that flies the quadcopter in a circle.
-//
-// ----------------------------------------------------------------------------------
-
-
-#include <math.h>
-#include <stdlib.h>
-#include "ros/ros.h"
-#include <std_msgs/String.h>
-#include <ros/package.h>
-#include "std_msgs/Float32.h"
-
-#include "d_fall_pps/CrazyflieData.h"
-#include "d_fall_pps/Setpoint.h"
-#include "d_fall_pps/ControlCommand.h"
-#include "d_fall_pps/Controller.h"
-
-#define PI 3.1415926535
-#define RATE_CONTROLLER 7
-
-using namespace d_fall_pps;
-
-std::vector<float> ffThrust(4);
-std::vector<float> feedforwardMotor(4);
-std::vector<float> motorPoly(3);
-
-std::vector<float> gainMatrixRoll(9);
-std::vector<float> gainMatrixPitch(9);
-std::vector<float> gainMatrixYaw(9);
-std::vector<float> gainMatrixThrust(9);
-
-//K_infinite of feedback
-std::vector<float> filterGain(6);
-//only for velocity calculation
-std::vector<float> estimatorMatrix(2);
-float prevEstimate[9];
-
-float saturationThrust;
-
-CrazyflieData previousLocation;
-
-//circle stuff
-float currentTime;
-const float OMEGA = 0.25*2*PI;
-const float RADIUS = 0.25;
-
-//point publisher for FollowCrazyflieService
-ros::Publisher followPublisher;
-
-
-void loadParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length) {
- if(!nodeHandle.getParam(name, val)){
- ROS_ERROR_STREAM("missing parameter '" << name << "'");
- }
- if(val.size() != length) {
- ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
- }
-}
-
-void loadParameters(ros::NodeHandle& nodeHandle) {
- loadParameterFloatVector(nodeHandle, "feedforwardMotor", feedforwardMotor, 4);
- loadParameterFloatVector(nodeHandle, "motorPoly", motorPoly, 3);
-
- for(int i = 0; i < 4; ++i) {
- ffThrust[i] = motorPoly[2] * feedforwardMotor[i] * feedforwardMotor[i] + motorPoly[1] * feedforwardMotor[i] + motorPoly[0];
- }
- saturationThrust = motorPoly[2] * 12000 * 12000 + motorPoly[1] * 12000 + motorPoly[0];
-
- loadParameterFloatVector(nodeHandle, "gainMatrixRoll", gainMatrixRoll, 9);
- loadParameterFloatVector(nodeHandle, "gainMatrixPitch", gainMatrixPitch, 9);
- loadParameterFloatVector(nodeHandle, "gainMatrixYaw", gainMatrixYaw, 9);
- loadParameterFloatVector(nodeHandle, "gainMatrixThrust", gainMatrixThrust, 9);
-
- loadParameterFloatVector(nodeHandle, "filterGain", filterGain, 6);
- loadParameterFloatVector(nodeHandle, "estimatorMatrix", estimatorMatrix, 2);
-
-}
-
-float computeMotorPolyBackward(float thrust) {
- return (-motorPoly[1] + sqrt(motorPoly[1] * motorPoly[1] - 4 * motorPoly[2] * (motorPoly[0] - thrust))) / (2 * motorPoly[2]);
-}
-
-
-//Kalman
-void estimateState(Controller::Request &request, float (&est)[9]) {
- // attitude
- est[6] = request.ownCrazyflie.roll;
- est[7] = request.ownCrazyflie.pitch;
- est[8] = request.ownCrazyflie.yaw;
-
- //velocity & filtering
- float ahat_x[6]; //estimator matrix times state (x, y, z, vx, vy, vz)
- ahat_x[0] = 0; ahat_x[1]=0; ahat_x[2]=0;
- ahat_x[3] = estimatorMatrix[0] * prevEstimate[0] + estimatorMatrix[1] * prevEstimate[3];
- ahat_x[4] = estimatorMatrix[0] * prevEstimate[1] + estimatorMatrix[1] * prevEstimate[4];
- ahat_x[5] = estimatorMatrix[0] * prevEstimate[2] + estimatorMatrix[1] * prevEstimate[5];
-
-
- float k_x[6]; //filterGain times state
- k_x[0] = request.ownCrazyflie.x * filterGain[0];
- k_x[1] = request.ownCrazyflie.y * filterGain[1];
- k_x[2] = request.ownCrazyflie.z * filterGain[2];
- k_x[3] = request.ownCrazyflie.x * filterGain[3];
- k_x[4] = request.ownCrazyflie.y * filterGain[4];
- k_x[5] = request.ownCrazyflie.z * filterGain[5];
-
- est[0] = ahat_x[0] + k_x[0];
- est[1] = ahat_x[1] + k_x[1];
- est[2] = ahat_x[2] + k_x[2];
- est[3] = ahat_x[3] + k_x[3];
- est[4] = ahat_x[4] + k_x[4];
- est[5] = ahat_x[5] + k_x[5];
-
- memcpy(prevEstimate, est, 9 * sizeof(float));
-
-}
-
-void convertIntoBodyFrame(float est[9], float (&state)[9], float yaw_measured) {
- float sinYaw = sin(yaw_measured);
- float cosYaw = cos(yaw_measured);
-
- state[0] = est[0] * cosYaw + est[1] * sinYaw;
- state[1] = -est[0] * sinYaw + est[1] * cosYaw;
- state[2] = est[2];
-
- state[3] = est[3] * cosYaw + est[4] * sinYaw;
- state[4] = -est[3] * sinYaw + est[4] * cosYaw;
- state[5] = est[5];
-
- state[6] = est[6];
- state[7] = est[7];
- state[8] = est[8];
-}
-
-void calculateCircle(Setpoint &circlePoint){
- circlePoint.x = 0;
- circlePoint.y = RADIUS*sin(OMEGA*currentTime);
- circlePoint.z = RADIUS*cos(OMEGA*currentTime);
- circlePoint.yaw = 0;//OMEGA*currentTime;
-
-}
-
-bool calculateControlOutput(Controller::Request &request, Controller::Response &response) {
- CrazyflieData vicon = request.ownCrazyflie;
-
- currentTime += request.ownCrazyflie.acquiringTime;
-
- //publish setpoint for FollowController of another student group
- Setpoint pubSetpoint;
- pubSetpoint.x = request.ownCrazyflie.x;
- pubSetpoint.y = request.ownCrazyflie.y;
- pubSetpoint.z = request.ownCrazyflie.z;
- pubSetpoint.yaw = request.ownCrazyflie.yaw;
- followPublisher.publish(pubSetpoint);
-
- //actual Circle Controller stuff follows
- Setpoint circlePoint;
- calculateCircle(circlePoint);
-
- float yaw_measured = request.ownCrazyflie.yaw;
-
- //move coordinate system to make setpoint origin
- request.ownCrazyflie.x -= circlePoint.x;
- request.ownCrazyflie.y -= circlePoint.y;
- request.ownCrazyflie.z -= circlePoint.z;
- float yaw = request.ownCrazyflie.yaw - circlePoint.yaw;
-
- while(yaw > PI) {yaw -= 2 * PI;}
- while(yaw < -PI) {yaw += 2 * PI;}
- request.ownCrazyflie.yaw = yaw;
-
- float est[9]; //px, py, pz, vx, vy, vz, roll, pitch, yaw
- estimateState(request, est);
-
- float state[9]; //px, py, pz, vx, vy, vz, roll, pitch, yaw
- convertIntoBodyFrame(est, state, yaw_measured);
-
- //calculate feedback
- float outRoll = 0;
- float outPitch = 0;
- float outYaw = 0;
- float thrustIntermediate = 0;
- for(int i = 0; i < 9; ++i) {
- outRoll -= gainMatrixRoll[i] * state[i];
- outPitch -= gainMatrixPitch[i] * state[i];
- outYaw -= gainMatrixYaw[i] * state[i];
- thrustIntermediate -= gainMatrixThrust[i] * state[i];
- }
-
- response.controlOutput.roll = outRoll;
- response.controlOutput.pitch = outPitch;
- response.controlOutput.yaw = outYaw;
-
- if(thrustIntermediate > saturationThrust)
- thrustIntermediate = saturationThrust;
- else if(thrustIntermediate < -saturationThrust)
- thrustIntermediate = -saturationThrust;
-
- response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustIntermediate + ffThrust[0]);
- response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustIntermediate + ffThrust[1]);
- response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustIntermediate + ffThrust[2]);
- response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustIntermediate + ffThrust[3]);
-
- response.controlOutput.onboardControllerType = RATE_CONTROLLER;
-
- previousLocation = request.ownCrazyflie;
-
-
-
- return true;
-}
-
-
-int main(int argc, char* argv[]) {
- ros::init(argc, argv, "CircleControllerService");
-
- currentTime = 0;
-
- ros::NodeHandle nodeHandle("~");
- loadParameters(nodeHandle);
-
- //publisher for Follow Controller of another student group
- followPublisher = nodeHandle.advertise<Setpoint>("FollowTopic", 1);
-
- ros::ServiceServer service = nodeHandle.advertiseService("CircleController", calculateControlOutput);
- ROS_INFO("CircleControllerService ready");
-
- ros::spin();
-
- return 0;
-}
diff --git a/pps_ws/src/d_fall_pps/src/FollowCrazyflieService.cpp b/pps_ws/src/d_fall_pps/src/FollowCrazyflieService.cpp
deleted file mode 100755
index 1b36613f44bb275636ceff2fc4adc2ee772efab9..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/src/FollowCrazyflieService.cpp
+++ /dev/null
@@ -1,236 +0,0 @@
-// Copyright (C) 2017, ETH Zurich, D-ITET, Cyrill Burgener, Marco Mueller, Philipp Friedli
-//
-// 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:
-// Controller that follows the position of another Crazyflie
-//
-// ----------------------------------------------------------------------------------
-
-
-#include <math.h>
-#include <stdlib.h>
-#include "ros/ros.h"
-#include <std_msgs/String.h>
-#include <ros/package.h>
-#include "std_msgs/Float32.h"
-
-#include "d_fall_pps/CrazyflieData.h"
-#include "d_fall_pps/Setpoint.h"
-#include "d_fall_pps/ControlCommand.h"
-#include "d_fall_pps/Controller.h"
-
-#define PI 3.1415926535
-#define RATE_CONTROLLER 7
-
-using namespace d_fall_pps;
-
-std::vector<float> ffThrust(4);
-std::vector<float> feedforwardMotor(4);
-std::vector<float> motorPoly(3);
-
-std::vector<float> gainMatrixRoll(9);
-std::vector<float> gainMatrixPitch(9);
-std::vector<float> gainMatrixYaw(9);
-std::vector<float> gainMatrixThrust(9);
-
-//K_infinite of feedback
-std::vector<float> filterGain(6);
-//only for velocity calculation
-std::vector<float> estimatorMatrix(2);
-float prevEstimate[9];
-
-std::vector<float> setpoint(4);
-float saturationThrust;
-
-CrazyflieData previousLocation;
-
-void loadParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length) {
- if(!nodeHandle.getParam(name, val)){
- ROS_ERROR_STREAM("missing parameter '" << name << "'");
- }
- if(val.size() != length) {
- ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
- }
-}
-
-void loadParameters(ros::NodeHandle& nodeHandle) {
- loadParameterFloatVector(nodeHandle, "feedforwardMotor", feedforwardMotor, 4);
- loadParameterFloatVector(nodeHandle, "motorPoly", motorPoly, 3);
-
- for(int i = 0; i < 4; ++i) {
- ffThrust[i] = motorPoly[2] * feedforwardMotor[i] * feedforwardMotor[i] + motorPoly[1] * feedforwardMotor[i] + motorPoly[0];
- }
- saturationThrust = motorPoly[2] * 12000 * 12000 + motorPoly[1] * 12000 + motorPoly[0];
-
- loadParameterFloatVector(nodeHandle, "gainMatrixRoll", gainMatrixRoll, 9);
- loadParameterFloatVector(nodeHandle, "gainMatrixPitch", gainMatrixPitch, 9);
- loadParameterFloatVector(nodeHandle, "gainMatrixYaw", gainMatrixYaw, 9);
- loadParameterFloatVector(nodeHandle, "gainMatrixThrust", gainMatrixThrust, 9);
-
- loadParameterFloatVector(nodeHandle, "filterGain", filterGain, 6);
- loadParameterFloatVector(nodeHandle, "estimatorMatrix", estimatorMatrix, 2);
-
- loadParameterFloatVector(nodeHandle, "defaultSetpoint", setpoint, 4);
-}
-
-float computeMotorPolyBackward(float thrust) {
- return (-motorPoly[1] + sqrt(motorPoly[1] * motorPoly[1] - 4 * motorPoly[2] * (motorPoly[0] - thrust))) / (2 * motorPoly[2]);
-}
-
-
-//Kalman
-void estimateState(Controller::Request &request, float (&est)[9]) {
- // attitude
- est[6] = request.ownCrazyflie.roll;
- est[7] = request.ownCrazyflie.pitch;
- est[8] = request.ownCrazyflie.yaw;
-
- //velocity & filtering
- float ahat_x[6]; //estimator matrix times state (x, y, z, vx, vy, vz)
- ahat_x[0] = 0; ahat_x[1]=0; ahat_x[2]=0;
- ahat_x[3] = estimatorMatrix[0] * prevEstimate[0] + estimatorMatrix[1] * prevEstimate[3];
- ahat_x[4] = estimatorMatrix[0] * prevEstimate[1] + estimatorMatrix[1] * prevEstimate[4];
- ahat_x[5] = estimatorMatrix[0] * prevEstimate[2] + estimatorMatrix[1] * prevEstimate[5];
-
-
- float k_x[6]; //filterGain times state
- k_x[0] = request.ownCrazyflie.x * filterGain[0];
- k_x[1] = request.ownCrazyflie.y * filterGain[1];
- k_x[2] = request.ownCrazyflie.z * filterGain[2];
- k_x[3] = request.ownCrazyflie.x * filterGain[3];
- k_x[4] = request.ownCrazyflie.y * filterGain[4];
- k_x[5] = request.ownCrazyflie.z * filterGain[5];
-
- est[0] = ahat_x[0] + k_x[0];
- est[1] = ahat_x[1] + k_x[1];
- est[2] = ahat_x[2] + k_x[2];
- est[3] = ahat_x[3] + k_x[3];
- est[4] = ahat_x[4] + k_x[4];
- est[5] = ahat_x[5] + k_x[5];
-
- memcpy(prevEstimate, est, 9 * sizeof(float));
-
-}
-
-
-void convertIntoBodyFrame(float est[9], float (&state)[9], float yaw_measured) {
- float sinYaw = sin(yaw_measured);
- float cosYaw = cos(yaw_measured);
-
- state[0] = est[0] * cosYaw + est[1] * sinYaw;
- state[1] = -est[0] * sinYaw + est[1] * cosYaw;
- state[2] = est[2];
-
- state[3] = est[3] * cosYaw + est[4] * sinYaw;
- state[4] = -est[3] * sinYaw + est[4] * cosYaw;
- state[5] = est[5];
-
- state[6] = est[6];
- state[7] = est[7];
- state[8] = est[8];
-}
-
-bool calculateControlOutput(Controller::Request &request, Controller::Response &response) {
- CrazyflieData vicon = request.ownCrazyflie;
-
- float yaw_measured = request.ownCrazyflie.yaw;
-
- //move coordinate system to make setpoint origin
- request.ownCrazyflie.x -= setpoint[0];
- request.ownCrazyflie.y -= setpoint[1];
- request.ownCrazyflie.z -= setpoint[2];
- float yaw = request.ownCrazyflie.yaw - setpoint[3];
-
- while(yaw > PI) {yaw -= 2 * PI;}
- while(yaw < -PI) {yaw += 2 * PI;}
- request.ownCrazyflie.yaw = yaw;
-
- float est[9]; //px, py, pz, vx, vy, vz, roll, pitch, yaw
- estimateState(request, est);
-
- float state[9]; //px, py, pz, vx, vy, vz, roll, pitch, yaw
- convertIntoBodyFrame(est, state, yaw_measured);
-
- //calculate feedback
- float outRoll = 0;
- float outPitch = 0;
- float outYaw = 0;
- float thrustIntermediate = 0;
- for(int i = 0; i < 9; ++i) {
- outRoll -= gainMatrixRoll[i] * state[i];
- outPitch -= gainMatrixPitch[i] * state[i];
- outYaw -= gainMatrixYaw[i] * state[i];
- thrustIntermediate -= gainMatrixThrust[i] * state[i];
- }
-
- //INFORMATION: this ugly fix was needed for the older firmware
- //outYaw *= 0.5;
-
- response.controlOutput.roll = outRoll;
- response.controlOutput.pitch = outPitch;
- response.controlOutput.yaw = outYaw;
-
- if(thrustIntermediate > saturationThrust)
- thrustIntermediate = saturationThrust;
- else if(thrustIntermediate < -saturationThrust)
- thrustIntermediate = -saturationThrust;
-
- response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustIntermediate + ffThrust[0]);
- response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustIntermediate + ffThrust[1]);
- response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustIntermediate + ffThrust[2]);
- response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustIntermediate + ffThrust[3]);
-
- response.controlOutput.onboardControllerType = RATE_CONTROLLER;
-
- previousLocation = request.ownCrazyflie;
-
- return true;
-}
-
-//the setpoint is copied from the other crazyflie. This crazyflie should then make the same figure in its own area
-void followCallback(const Setpoint& newSetpoint) {
- setpoint[0] = newSetpoint.x;
- setpoint[1] = newSetpoint.y;
- setpoint[2] = newSetpoint.z;
- setpoint[3] = newSetpoint.yaw;
-}
-
-
-int main(int argc, char* argv[]) {
- ros::init(argc, argv, "FollowControllerService");
-
- ros::NodeHandle nodeHandle("~");
- loadParameters(nodeHandle);
-
- ros::Subscriber followSubscriber = nodeHandle.subscribe("/3/CircleControllerService/FollowTopic", 5, followCallback);
-
- ros::ServiceServer service = nodeHandle.advertiseService("FollowController", calculateControlOutput);
- ROS_INFO("FollowCrazyflieService ready");
-
- ros::spin();
-
- return 0;
-}
diff --git a/pps_ws/src/d_fall_pps/src/FollowN_1Service.cpp b/pps_ws/src/d_fall_pps/src/FollowN_1Service.cpp
deleted file mode 100644
index 84dec28c5a6ab657e49679213c809a84348e6ccf..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/src/FollowN_1Service.cpp
+++ /dev/null
@@ -1,271 +0,0 @@
-// Copyright (C) 2017, ETH Zurich, D-ITET, Angel Romero, Cyrill Burgener, Marco Mueller, Philipp Friedli
-//
-// 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:
-// Controller that follows the Crazyflie with ID N-1
-//
-// ----------------------------------------------------------------------------------
-
-
-#include <math.h>
-#include <stdlib.h>
-#include "ros/ros.h"
-#include <std_msgs/String.h>
-#include <ros/package.h>
-#include "std_msgs/Float32.h"
-
-#include "d_fall_pps/CrazyflieData.h"
-#include "d_fall_pps/Setpoint.h"
-#include "d_fall_pps/ControlCommand.h"
-#include "d_fall_pps/Controller.h"
-
-#define PI 3.1415926535
-#define RATE_CONTROLLER 7
-
-using namespace d_fall_pps;
-
-std::vector<float> ffThrust(4);
-std::vector<float> feedforwardMotor(4);
-std::vector<float> motorPoly(3);
-
-std::vector<float> gainMatrixRoll(9);
-std::vector<float> gainMatrixPitch(9);
-std::vector<float> gainMatrixYaw(9);
-std::vector<float> gainMatrixThrust(9);
-
-//K_infinite of feedback
-std::vector<float> filterGain(6);
-//only for velocity calculation
-std::vector<float> estimatorMatrix(2);
-float prevEstimate[9];
-
-std::vector<float> setpoint(4);
-float saturationThrust;
-ros::Publisher followPublisher;
-ros::Subscriber followSubscriber;
-
-CrazyflieData previousLocation;
-
-void loadParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length) {
- if(!nodeHandle.getParam(name, val)){
- ROS_ERROR_STREAM("missing parameter '" << name << "'");
- }
- if(val.size() != length) {
- ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
- }
-}
-
-void loadParameters(ros::NodeHandle& nodeHandle) {
- loadParameterFloatVector(nodeHandle, "feedforwardMotor", feedforwardMotor, 4);
- loadParameterFloatVector(nodeHandle, "motorPoly", motorPoly, 3);
-
- for(int i = 0; i < 4; ++i) {
- ffThrust[i] = motorPoly[2] * feedforwardMotor[i] * feedforwardMotor[i] + motorPoly[1] * feedforwardMotor[i] + motorPoly[0];
- }
- saturationThrust = motorPoly[2] * 12000 * 12000 + motorPoly[1] * 12000 + motorPoly[0];
-
- loadParameterFloatVector(nodeHandle, "gainMatrixRoll", gainMatrixRoll, 9);
- loadParameterFloatVector(nodeHandle, "gainMatrixPitch", gainMatrixPitch, 9);
- loadParameterFloatVector(nodeHandle, "gainMatrixYaw", gainMatrixYaw, 9);
- loadParameterFloatVector(nodeHandle, "gainMatrixThrust", gainMatrixThrust, 9);
-
- loadParameterFloatVector(nodeHandle, "filterGain", filterGain, 6);
- loadParameterFloatVector(nodeHandle, "estimatorMatrix", estimatorMatrix, 2);
-
- loadParameterFloatVector(nodeHandle, "defaultSetpoint", setpoint, 4);
-}
-
-float computeMotorPolyBackward(float thrust) {
- return (-motorPoly[1] + sqrt(motorPoly[1] * motorPoly[1] - 4 * motorPoly[2] * (motorPoly[0] - thrust))) / (2 * motorPoly[2]);
-}
-
-
-//Kalman
-void estimateState(Controller::Request &request, float (&est)[9]) {
- // attitude
- est[6] = request.ownCrazyflie.roll;
- est[7] = request.ownCrazyflie.pitch;
- est[8] = request.ownCrazyflie.yaw;
-
- //velocity & filtering
- float ahat_x[6]; //estimator matrix times state (x, y, z, vx, vy, vz)
- ahat_x[0] = 0; ahat_x[1]=0; ahat_x[2]=0;
- ahat_x[3] = estimatorMatrix[0] * prevEstimate[0] + estimatorMatrix[1] * prevEstimate[3];
- ahat_x[4] = estimatorMatrix[0] * prevEstimate[1] + estimatorMatrix[1] * prevEstimate[4];
- ahat_x[5] = estimatorMatrix[0] * prevEstimate[2] + estimatorMatrix[1] * prevEstimate[5];
-
-
- float k_x[6]; //filterGain times state
- k_x[0] = request.ownCrazyflie.x * filterGain[0];
- k_x[1] = request.ownCrazyflie.y * filterGain[1];
- k_x[2] = request.ownCrazyflie.z * filterGain[2];
- k_x[3] = request.ownCrazyflie.x * filterGain[3];
- k_x[4] = request.ownCrazyflie.y * filterGain[4];
- k_x[5] = request.ownCrazyflie.z * filterGain[5];
-
- est[0] = ahat_x[0] + k_x[0];
- est[1] = ahat_x[1] + k_x[1];
- est[2] = ahat_x[2] + k_x[2];
- est[3] = ahat_x[3] + k_x[3];
- est[4] = ahat_x[4] + k_x[4];
- est[5] = ahat_x[5] + k_x[5];
-
- memcpy(prevEstimate, est, 9 * sizeof(float));
-
-}
-
-
-void convertIntoBodyFrame(float est[9], float (&state)[9], float yaw_measured) {
- float sinYaw = sin(yaw_measured);
- float cosYaw = cos(yaw_measured);
-
- state[0] = est[0] * cosYaw + est[1] * sinYaw;
- state[1] = -est[0] * sinYaw + est[1] * cosYaw;
- state[2] = est[2];
-
- state[3] = est[3] * cosYaw + est[4] * sinYaw;
- state[4] = -est[3] * sinYaw + est[4] * cosYaw;
- state[5] = est[5];
-
- state[6] = est[6];
- state[7] = est[7];
- state[8] = est[8];
-}
-
-bool calculateControlOutput(Controller::Request &request, Controller::Response &response) {
- CrazyflieData vicon = request.ownCrazyflie;
-
- float yaw_measured = request.ownCrazyflie.yaw;
-
- //publish setpoint for FollowController of another student group
- Setpoint pubSetpoint;
- pubSetpoint.x = request.ownCrazyflie.x;
- pubSetpoint.y = request.ownCrazyflie.y;
- pubSetpoint.z = request.ownCrazyflie.z;
- pubSetpoint.yaw = request.ownCrazyflie.yaw;
- followPublisher.publish(pubSetpoint);
-
- //move coordinate system to make setpoint origin
- request.ownCrazyflie.x -= setpoint[0];
- request.ownCrazyflie.y -= setpoint[1];
- request.ownCrazyflie.z -= setpoint[2];
- float yaw = request.ownCrazyflie.yaw - setpoint[3];
-
- while(yaw > PI) {yaw -= 2 * PI;}
- while(yaw < -PI) {yaw += 2 * PI;}
- request.ownCrazyflie.yaw = yaw;
-
- float est[9]; //px, py, pz, vx, vy, vz, roll, pitch, yaw
- estimateState(request, est);
-
- float state[9]; //px, py, pz, vx, vy, vz, roll, pitch, yaw
- convertIntoBodyFrame(est, state, yaw_measured);
-
- //calculate feedback
- float outRoll = 0;
- float outPitch = 0;
- float outYaw = 0;
- float thrustIntermediate = 0;
- for(int i = 0; i < 9; ++i) {
- outRoll -= gainMatrixRoll[i] * state[i];
- outPitch -= gainMatrixPitch[i] * state[i];
- outYaw -= gainMatrixYaw[i] * state[i];
- thrustIntermediate -= gainMatrixThrust[i] * state[i];
- }
-
- //INFORMATION: this ugly fix was needed for the older firmware
- //outYaw *= 0.5;
-
- response.controlOutput.roll = outRoll;
- response.controlOutput.pitch = outPitch;
- response.controlOutput.yaw = outYaw;
-
- if(thrustIntermediate > saturationThrust)
- thrustIntermediate = saturationThrust;
- else if(thrustIntermediate < -saturationThrust)
- thrustIntermediate = -saturationThrust;
-
- response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustIntermediate + ffThrust[0]);
- response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustIntermediate + ffThrust[1]);
- response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustIntermediate + ffThrust[2]);
- response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustIntermediate + ffThrust[3]);
-
- response.controlOutput.onboardControllerType = RATE_CONTROLLER;
-
- previousLocation = request.ownCrazyflie;
-
- return true;
-}
-
-//the setpoint is copied from the other crazyflie. This crazyflie should then make the same figure in its own area
-void followCallback(const Setpoint& newSetpoint) {
- setpoint[0] = newSetpoint.x;
- setpoint[1] = newSetpoint.y;
- setpoint[2] = newSetpoint.z;
- setpoint[3] = newSetpoint.yaw;
- ROS_INFO("received callback new setpoint from FollowN1Service/FollowTopic");
-}
-
-
-int main(int argc, char* argv[]) {
- ros::init(argc, argv, "FollowN_1Service");
-
- ros::NodeHandle nodeHandle("~");
- loadParameters(nodeHandle);
-
- int student_id;
- std::string m_namespace = ros::this_node::getNamespace();
- ros::NodeHandle PPSClient_nodeHandle(m_namespace + "/PPSClient");
-
- if(!PPSClient_nodeHandle.getParam("studentID", student_id))
- {
- ROS_ERROR("Failed to get studentID from FollowN_1Service");
- }
-
- followPublisher = nodeHandle.advertise<Setpoint>("/" + std::to_string(student_id) + "/FollowN_1Service/FollowTopic", 1);
-
- if(student_id != 1)
- {
- // subscribe to n-1 setpoint
- followSubscriber = nodeHandle.subscribe("/" + std::to_string(student_id - 1) + "/FollowN_1Service/FollowTopic", 1, followCallback);
- }
- else
- {
- // what is the setpoint for 1? 0 0 0.5 0
- setpoint[0] = 0;
- setpoint[1] = 0;
- setpoint[2] = 0.5;
- setpoint[3] = 0;
- }
-
-
- ros::ServiceServer service = nodeHandle.advertiseService("FollowController", calculateControlOutput);
- ROS_INFO("FollowN_1Service ready");
-
- ros::spin();
-
- return 0;
-}
diff --git a/pps_ws/src/d_fall_pps/src/CentralManagerService.cpp b/pps_ws/src/d_fall_pps/src/nodes/CentralManagerService.cpp
similarity index 61%
rename from pps_ws/src/d_fall_pps/src/CentralManagerService.cpp
rename to pps_ws/src/d_fall_pps/src/nodes/CentralManagerService.cpp
index 4a0e58a8ebbe3a75bbbc05ec619dbad05ac2211e..0034c54e0751c3d280e52dff4ec5aa433d328ab7 100755
--- a/pps_ws/src/d_fall_pps/src/CentralManagerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/CentralManagerService.cpp
@@ -33,94 +33,10 @@
-// ----------------------------------------------------------------------------------
-// III N N CCCC L U U DDDD EEEEE SSSS
-// I NN N C L U U D D E S
-// I N N N C L U U D D EEE SSS
-// I N NN C L U U D D E S
-// III N N CCCC LLLLL UUU DDDD EEEEE SSSS
-// ----------------------------------------------------------------------------------
-
-#include <stdlib.h>
-#include <ros/ros.h>
-#include "d_fall_pps/CrazyflieContext.h"
-#include "d_fall_pps/CrazyflieDB.h"
-
-#include "d_fall_pps/CMRead.h"
-#include "d_fall_pps/CMQuery.h"
-#include "d_fall_pps/CMUpdate.h"
-#include "d_fall_pps/CMCommand.h"
-#include "CentralManagerService.h"
-
-#include "CrazyflieIO.h"
-
-
-
-
-
-// ----------------------------------------------------------------------------------
-// DDDD EEEEE FFFFF III N N EEEEE SSSS
-// D D E F I NN N E S
-// D D EEE FFF I N N N EEE SSS
-// D D E F I N NN E S
-// DDDD EEEEE F III N N EEEEE SSSS
-// ----------------------------------------------------------------------------------
-
-
-// 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
-
-// For which controller parameters and from where to fetch
-#define FETCH_YAML_SAFE_CONTROLLER_FROM_OWN_AGENT 1
-#define FETCH_YAML_CUSTOM_CONTROLLER_FROM_OWN_AGENT 2
-#define FETCH_YAML_SAFE_CONTROLLER_FROM_COORDINATOR 3
-#define FETCH_YAML_CUSTOM_CONTROLLER_FROM_COORDINATOR 4
-
-
-using namespace d_fall_pps;
-using namespace std;
-
-
-
-
-
-// ----------------------------------------------------------------------------------
-// V V A RRRR III A BBBB L EEEEE SSSS
-// V V A A R R I A A B B L E S
-// V V A A RRRR I A A BBBB L EEE SSS
-// V V AAAAA R R I AAAAA B B L E S
-// V A A R R III A A BBBB LLLLL EEEEE SSSS
-// ----------------------------------------------------------------------------------
-
-CrazyflieDB crazyflieDB;
-
-
-
+// INCLUDE THE HEADER
+#include "nodes/CentralManagerService.h"
-// ----------------------------------------------------------------------------------
-// FFFFF U U N N CCCC TTTTT III OOO N N
-// F U U NN N C T I O O NN N
-// FFF U U N N N C T I O O N N N
-// F U U N NN C T I O O N NN
-// F UUU N N CCCC T III OOO N N
-//
-// PPPP RRRR OOO TTTTT OOO TTTTT Y Y PPPP EEEEE SSSS
-// P P R R O O T O O T Y Y P P E S
-// PPPP RRRR O O T O O T Y PPPP EEE SSS
-// P R R O O T O O T Y P E S
-// P R R OOO T OOO T Y P EEEEE SSSS
-// ----------------------------------------------------------------------------------
-
-bool cmRead(CMRead::Request &request, CMRead::Response &response);
-int findEntryByStudID(unsigned int studID);
-bool cmQuery(CMQuery::Request &request, CMQuery::Response &response);
-int findEntryByCF(string name);
-bool cmUpdate(CMUpdate::Request &request, CMUpdate::Response &response);
-bool cmCommand(CMCommand::Request &request, CMCommand::Response &response);
@@ -142,24 +58,6 @@ bool cmCommand(CMCommand::Request &request, CMCommand::Response &response);
-// ----------------------------------------------------------------------------------
-// L OOO A DDDD
-// L O O A A D D
-// L O O A A D D
-// L O O AAAAA D D
-// LLLLL OOO A A DDDD
-//
-// PPPP A RRRR A M M EEEEE TTTTT EEEEE RRRR SSSS
-// P P A A R R A A MM MM E T E R R S
-// PPPP A A RRRR A A M M M EEE T EEE RRRR SSS
-// P AAAAA R R AAAAA M M E T E R R S
-// P A A R R A A M M EEEEE T EEEEE R R SSSS
-// ----------------------------------------------------------------------------------
-
-// The requesting to load parameters is currently handled by the Paramter Service
-
-
-
// ----------------------------------------------------------------------------------
// DDDD A TTTTT A BBBB A SSSS EEEEE
// D D A A T A A B B A A S E
diff --git a/pps_ws/src/d_fall_pps/src/nodes/DemoControllerService.cpp b/pps_ws/src/d_fall_pps/src/nodes/DemoControllerService.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a4ffa5643672ff24d4c9256b4a8e584ef1b6db21
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/src/nodes/DemoControllerService.cpp
@@ -0,0 +1,1938 @@
+// Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat, Angel Romero, Cyrill Burgener, Marco Mueller, Philipp Friedli
+//
+// 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:
+// Place for students to implement their controller
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// INCLUDE THE HEADER
+#include "nodes/DemoControllerService.h"
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// FFFFF U U N N CCCC TTTTT III OOO N N
+// F U U NN N C T I O O NN N
+// FFF U U N N N C T I O O N N N
+// F U U N NN C T I O O N NN
+// F UUU N N CCCC T III OOO N N
+//
+// III M M PPPP L EEEEE M M EEEEE N N TTTTT A TTTTT III OOO N N
+// I MM MM P P L E MM MM E NN N T A A T I O O NN N
+// I M M M PPPP L EEE M M M EEE N N N T A A T I O O N N N
+// I M M P L E M M E N NN T AAAAA T I O O N NN
+// III M M P LLLLL EEEEE M M EEEEE N N T A A T III OOO N N
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// ------------------------------------------------------------------------------
+// OOO U U TTTTT EEEEE RRRR
+// O O U U T E R R
+// O O U U T EEE RRRR
+// O O U U T E R R
+// OOO UUU T EEEEE R R
+//
+// CCCC OOO N N TTTTT RRRR OOO L L OOO OOO PPPP
+// C O O NN N T R R O O L L O O O O P P
+// C O O N N N T RRRR O O L L O O O O PPPP
+// C O O N NN T R R O O L L O O O O P
+// CCCC OOO N N T R R OOO LLLLL LLLLL OOO OOO P
+// ----------------------------------------------------------------------------------
+
+// This function is the callback that is linked to the "DemoController" service that
+// is advertised in the main function. This must have arguments that match the
+// "input-output" behaviour defined in the "Controller.srv" file (located in the "srv"
+// folder)
+//
+// The arument "request" is a structure provided to this service with the following two
+// properties:
+// request.ownCrazyflie
+// This property is itself a structure of type "CrazyflieData", which is defined in the
+// file "CrazyflieData.msg", and has the following properties
+// string crazyflieName
+// float64 x The x position of the Crazyflie [metres]
+// float64 y The y position of the Crazyflie [metres]
+// float64 z The z position of the Crazyflie [metres]
+// float64 roll The roll component of the intrinsic Euler angles [radians]
+// float64 pitch The pitch component of the intrinsic Euler angles [radians]
+// float64 yaw The yaw component of the intrinsic Euler angles [radians]
+// float64 acquiringTime #delta t The time elapsed since the previous "CrazyflieData" was received [seconds]
+// bool occluded A boolean indicted whether the Crazyflie for visible at the time of this measurement
+// The values in these properties are directly the measurement taken by the Vicon
+// motion capture system of the Crazyflie that is to be controlled by this service
+//
+// request.otherCrazyflies
+// This property is an array of "CrazyflieData" structures, what allows access to the
+// Vicon measurements of other Crazyflies.
+//
+// The argument "response" is a structure that is expected to be filled in by this
+// service by this function, it has only the following property
+// response.ControlCommand
+// This property is iteself a structure of type "ControlCommand", which is defined in
+// the file "ControlCommand.msg", and has the following properties:
+// float32 roll The command sent to the Crazyflie for the body frame x-axis
+// float32 pitch The command sent to the Crazyflie for the body frame y-axis
+// float32 yaw The command sent to the Crazyflie for the body frame z-axis
+// uint16 motorCmd1 The command sent to the Crazyflie for motor 1
+// uint16 motorCmd2 The command sent to the Crazyflie for motor 1
+// uint16 motorCmd3 The command sent to the Crazyflie for motor 1
+// uint16 motorCmd4 The command sent to the Crazyflie for motor 1
+// uint8 onboardControllerType The flag sent to the Crazyflie for indicating how to implement the command
+//
+// IMPORTANT NOTES FOR "onboardControllerType" AND AXIS CONVENTIONS
+// > The allowed values for "onboardControllerType" are in the "Defines" section at the
+// top of this file, they are:
+// CF_COMMAND_TYPE_MOTOR
+// CF_COMMAND_TYPE_RATE
+// CF_COMMAND_TYPE_ANGLE.
+// > With CF_COMMAND_TYPE_RATE the ".roll", ".ptich", and ".yaw" properties of "response.ControlCommand"
+// specify the angular rate in [radians/second] that will be requested from the
+// PID controllers running in the Crazyflie 2.0 firmware.
+// > With CF_COMMAND_TYPE_RATE the ".motorCmd1" to ".motorCmd4" properties of "response.ControlCommand"
+// are the baseline motor commands requested from the Crazyflie, with the adjustment
+// for body rates being added on top of this in the firmware (i.e., as per the code
+// of the "distribute_power" function provided in exercise sheet 2).
+// > With CF_COMMAND_TYPE_RATE the axis convention for the roll, pitch, and yaw body rates returned
+// in "response.ControlCommand" should use right-hand coordinate axes with x-forward
+// and z-upwards (i.e., the positive z-axis is aligned with the direction of positive
+// thrust). To assist, teh following is an ASCII art of this convention:
+//
+// ASCII ART OF THE CRAZYFLIE 2.0 LAYOUT
+//
+// > This is a top view,
+// > M1 to M4 stand for Motor 1 to Motor 4,
+// > "CW" indicates that the motor rotates Clockwise,
+// > "CCW" indicates that the motor rotates Counter-Clockwise,
+// > By right-hand axis convention, the positive z-direction points our of the screen,
+// > This being a "top view" means tha the direction of positive thrust produced
+// by the propellers is also out of the screen.
+//
+// ____ ____
+// / \ / \
+// (CW) | M4 | x | M1 | (CCW)
+// \____/\ ^ /\____/
+// \ \ | / /
+// \ \ | / /
+// \ \______ | ______/ /
+// \ | /
+// | | |
+// y <-------------o |
+// | |
+// / _______________ \
+// / / \ \
+// / / \ \
+// ____/ / \ \____
+// / \/ \/ \
+// (CCW) | M3 | | M2 | (CW)
+// \____/ \____/
+//
+//
+//
+// This function WILL NEED TO BE edited for successful completion of the PPS exercise
+bool calculateControlOutput(Controller::Request &request, Controller::Response &response)
+{
+
+ // THIS IS THE START OF THE "OUTER" CONTROL LOOP
+ // > i.e., this is the control loop run on this laptop
+ // > this function is called at the frequency specified
+ // > this function performs all estimation and control
+
+
+ // PERFORM THE ESTIMATOR UPDATE FOR THE INTERIAL FRAME STATE
+ // > After this function is complete the class variable
+ // "current_stateInertialEstimate" is updated and ready
+ // to be used for subsequent controller copmutations
+ performEstimatorUpdate_forStateInterial(request);
+
+
+ // CONVERT THE CURRENT INERTIAL FRAME STATE ESTIMATE, INTO
+ // THE BODY FRAME ERROR REQUIRED BY THE CONTROLLER
+ // > Define a local array to fill in with the body frame error
+ float current_bodyFrameError[12];
+ // > Call the function to perform the conversion
+ convert_stateInertial_to_bodyFrameError(current_stateInertialEstimate,setpoint,current_bodyFrameError);
+
+
+
+ // CARRY OUT THE CONTROLLER COMPUTATIONS
+ calculateControlOutput_viaLQR(current_bodyFrameError,request,response);
+
+
+
+ // PUBLISH THE CURRENT X,Y,Z, AND YAW (if required)
+ if (shouldPublishCurrent_xyz_yaw)
+ {
+ publish_current_xyz_yaw(request.ownCrazyflie.x,request.ownCrazyflie.y,request.ownCrazyflie.z,request.ownCrazyflie.yaw);
+ }
+
+ // PUBLISH THE DEBUG MESSAGE (if required)
+ if (shouldPublishDebugMessage)
+ {
+ construct_and_publish_debug_message(request,response);
+ }
+
+ // RETURN "true" TO INDICATE THAT THE COMPUTATIONS WERE SUCCESSFUL
+ return true;
+}
+
+
+
+
+// ------------------------------------------------------------------------------
+// EEEEE SSSS TTTTT III M M A TTTTT III OOO N N
+// E S T I MM MM A A T I O O NN N
+// EEE SSS T I M M M A A T I O O N N N
+// E S T I M M AAAAA T I O O N NN
+// EEEEE SSSS T III M M A A T III OOO N N
+// ------------------------------------------------------------------------------
+void performEstimatorUpdate_forStateInterial(Controller::Request &request)
+{
+
+ // PUT THE CURRENT MEASURED DATA INTO THE CLASS VARIABLE
+ // > for (x,y,z) position
+ current_xzy_rpy_measurement[0] = request.ownCrazyflie.x;
+ current_xzy_rpy_measurement[1] = request.ownCrazyflie.y;
+ current_xzy_rpy_measurement[2] = request.ownCrazyflie.z;
+ // > for (roll,pitch,yaw) angles
+ current_xzy_rpy_measurement[3] = request.ownCrazyflie.roll;
+ current_xzy_rpy_measurement[4] = request.ownCrazyflie.pitch;
+ current_xzy_rpy_measurement[5] = request.ownCrazyflie.yaw;
+
+
+ // RUN THE FINITE DIFFERENCE ESTIMATOR
+ performEstimatorUpdate_forStateInterial_viaFiniteDifference();
+
+
+ // RUN THE POINT MASS KALMAN FILTER ESTIMATOR
+ performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter();
+
+
+ // FILLE IN THE STATE INERTIAL ESTIMATE TO BE USED FOR CONTROL
+ switch (estimator_method)
+ {
+ // Estimator based on finte differences
+ case ESTIMATOR_METHOD_FINITE_DIFFERENCE:
+ {
+ // Transfer the estimate
+ for(int i = 0; i < 12; ++i)
+ {
+ current_stateInertialEstimate[i] = stateInterialEstimate_viaFiniteDifference[i];
+ }
+ break;
+ }
+ // Estimator based on Point Mass Kalman Filter
+ case ESTIMATOR_METHOD_POINT_MASS_PER_DIMENSION:
+ {
+ // Transfer the estimate
+ for(int i = 0; i < 12; ++i)
+ {
+ current_stateInertialEstimate[i] = stateInterialEstimate_viaPointMassKalmanFilter[i];
+ }
+ break;
+ }
+ // Handle the exception
+ default:
+ {
+ // Display that the "estimator_method" was not recognised
+ ROS_INFO_STREAM("[DEMO CONTROLLER] ERROR: in the 'calculateControlOutput' function of the 'DemoControllerService': the 'estimator_method' is not recognised.");
+ // Transfer the finite difference estimate by default
+ for(int i = 0; i < 12; ++i)
+ {
+ current_stateInertialEstimate[i] = stateInterialEstimate_viaFiniteDifference[i];
+ }
+ break;
+ }
+ }
+
+
+ // NOW THAT THE ESTIMATORS HAVE ALL BEEN RUN, PUT THE CURRENT
+ // MEASURED DATA INTO THE CLASS VARIABLE FOR THE PREVIOUS
+ // > for (x,y,z) position
+ previous_xzy_rpy_measurement[0] = current_xzy_rpy_measurement[0];
+ previous_xzy_rpy_measurement[1] = current_xzy_rpy_measurement[1];
+ previous_xzy_rpy_measurement[2] = current_xzy_rpy_measurement[2];
+ // > for (roll,pitch,yaw) angles
+ previous_xzy_rpy_measurement[3] = current_xzy_rpy_measurement[3];
+ previous_xzy_rpy_measurement[4] = current_xzy_rpy_measurement[4];
+ previous_xzy_rpy_measurement[5] = current_xzy_rpy_measurement[5];
+}
+
+
+
+void performEstimatorUpdate_forStateInterial_viaFiniteDifference()
+{
+ // PUT IN THE CURRENT MEASUREMENT DIRECTLY
+ // > for (x,y,z) position
+ stateInterialEstimate_viaFiniteDifference[0] = current_xzy_rpy_measurement[0];
+ stateInterialEstimate_viaFiniteDifference[1] = current_xzy_rpy_measurement[1];
+ stateInterialEstimate_viaFiniteDifference[2] = current_xzy_rpy_measurement[2];
+ // > for (roll,pitch,yaw) angles
+ stateInterialEstimate_viaFiniteDifference[6] = current_xzy_rpy_measurement[3];
+ stateInterialEstimate_viaFiniteDifference[7] = current_xzy_rpy_measurement[4];
+ stateInterialEstimate_viaFiniteDifference[8] = current_xzy_rpy_measurement[5];
+
+ // COMPUTE THE VELOCITIES VIA FINITE DIFFERENCE
+ // > for (x,y,z) velocities
+ stateInterialEstimate_viaFiniteDifference[3] = (current_xzy_rpy_measurement[0] - previous_xzy_rpy_measurement[0]) * estimator_frequency;
+ stateInterialEstimate_viaFiniteDifference[4] = (current_xzy_rpy_measurement[1] - previous_xzy_rpy_measurement[1]) * estimator_frequency;
+ stateInterialEstimate_viaFiniteDifference[5] = (current_xzy_rpy_measurement[2] - previous_xzy_rpy_measurement[2]) * estimator_frequency;
+ // > for (roll,pitch,yaw) velocities
+ stateInterialEstimate_viaFiniteDifference[9] = (current_xzy_rpy_measurement[3] - previous_xzy_rpy_measurement[3]) * estimator_frequency;
+ stateInterialEstimate_viaFiniteDifference[10] = (current_xzy_rpy_measurement[4] - previous_xzy_rpy_measurement[4]) * estimator_frequency;
+ stateInterialEstimate_viaFiniteDifference[11] = (current_xzy_rpy_measurement[5] - previous_xzy_rpy_measurement[5]) * estimator_frequency;
+}
+
+
+
+void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter()
+{
+ // PERFORM THE KALMAN FILTER UPDATE STEP
+ // > First take a copy of the estimator state
+ float temp_PMKFstate[12];
+ for(int i = 0; i < 12; ++i)
+ {
+ temp_PMKFstate[i] = stateInterialEstimate_viaPointMassKalmanFilter[i];
+ }
+ // > Now perform update for:
+ // > x position and velocity:
+ stateInterialEstimate_viaPointMassKalmanFilter[0] = PMKF_Ahat_row1_for_positions[0]*temp_PMKFstate[0] + PMKF_Ahat_row1_for_positions[1]*temp_PMKFstate[3] + PMKF_Kinf_for_positions[0]*current_xzy_rpy_measurement[0];
+ stateInterialEstimate_viaPointMassKalmanFilter[3] = PMKF_Ahat_row2_for_positions[0]*temp_PMKFstate[0] + PMKF_Ahat_row2_for_positions[1]*temp_PMKFstate[3] + PMKF_Kinf_for_positions[1]*current_xzy_rpy_measurement[0];
+ // > y position and velocity:
+ stateInterialEstimate_viaPointMassKalmanFilter[1] = PMKF_Ahat_row1_for_positions[0]*temp_PMKFstate[1] + PMKF_Ahat_row1_for_positions[1]*temp_PMKFstate[4] + PMKF_Kinf_for_positions[0]*current_xzy_rpy_measurement[1];
+ stateInterialEstimate_viaPointMassKalmanFilter[4] = PMKF_Ahat_row2_for_positions[0]*temp_PMKFstate[1] + PMKF_Ahat_row2_for_positions[1]*temp_PMKFstate[4] + PMKF_Kinf_for_positions[1]*current_xzy_rpy_measurement[1];
+ // > z position and velocity:
+ stateInterialEstimate_viaPointMassKalmanFilter[2] = PMKF_Ahat_row1_for_positions[0]*temp_PMKFstate[2] + PMKF_Ahat_row1_for_positions[1]*temp_PMKFstate[5] + PMKF_Kinf_for_positions[0]*current_xzy_rpy_measurement[2];
+ stateInterialEstimate_viaPointMassKalmanFilter[5] = PMKF_Ahat_row2_for_positions[0]*temp_PMKFstate[2] + PMKF_Ahat_row2_for_positions[1]*temp_PMKFstate[5] + PMKF_Kinf_for_positions[1]*current_xzy_rpy_measurement[2];
+
+ // > roll position and velocity:
+ stateInterialEstimate_viaPointMassKalmanFilter[6] = PMKF_Ahat_row1_for_angles[0]*temp_PMKFstate[6] + PMKF_Ahat_row1_for_angles[1]*temp_PMKFstate[9] + PMKF_Kinf_for_angles[0]*current_xzy_rpy_measurement[3];
+ stateInterialEstimate_viaPointMassKalmanFilter[9] = PMKF_Ahat_row2_for_angles[0]*temp_PMKFstate[6] + PMKF_Ahat_row2_for_angles[1]*temp_PMKFstate[9] + PMKF_Kinf_for_angles[1]*current_xzy_rpy_measurement[3];
+ // > pitch position and velocity:
+ stateInterialEstimate_viaPointMassKalmanFilter[7] = PMKF_Ahat_row1_for_angles[0]*temp_PMKFstate[7] + PMKF_Ahat_row1_for_angles[1]*temp_PMKFstate[10] + PMKF_Kinf_for_angles[0]*current_xzy_rpy_measurement[4];
+ stateInterialEstimate_viaPointMassKalmanFilter[10] = PMKF_Ahat_row2_for_angles[0]*temp_PMKFstate[7] + PMKF_Ahat_row2_for_angles[1]*temp_PMKFstate[10] + PMKF_Kinf_for_angles[1]*current_xzy_rpy_measurement[4];
+ // > yaw position and velocity:
+ stateInterialEstimate_viaPointMassKalmanFilter[8] = PMKF_Ahat_row1_for_angles[0]*temp_PMKFstate[8] + PMKF_Ahat_row1_for_angles[1]*temp_PMKFstate[11] + PMKF_Kinf_for_angles[0]*current_xzy_rpy_measurement[5];
+ stateInterialEstimate_viaPointMassKalmanFilter[11] = PMKF_Ahat_row2_for_angles[0]*temp_PMKFstate[8] + PMKF_Ahat_row2_for_angles[1]*temp_PMKFstate[11] + PMKF_Kinf_for_angles[1]*current_xzy_rpy_measurement[5];
+
+
+ //DebugMsg debugMsg;
+
+ // Fill the debugging message with the data provided by Vicon
+ //debugMsg.vicon_x = request.ownCrazyflie.x;
+ //debugMsg.vicon_y = request.ownCrazyflie.y;
+ //debugMsg.vicon_z = request.ownCrazyflie.z;
+ //debugMsg.vicon_roll = request.ownCrazyflie.roll;
+ //debugMsg.vicon_pitch = request.ownCrazyflie.pitch;
+ //debugMsg.vicon_yaw = request.ownCrazyflie.yaw;
+
+ // debugMsg.value_1 = thrustAdjustment;
+ // ......................
+ // debugMsg.value_10 = your_variable_name;
+
+ //debugMsg.value_1 = stateInterialEstimate_viaPointMassKalmanFilter[6];
+ //debugMsg.value_2 = stateInterialEstimate_viaPointMassKalmanFilter[9];
+ //debugMsg.value_3 = current_xzy_rpy_measurement[3];
+
+
+ //debugMsg.value_4 = stateInterialEstimate_viaPointMassKalmanFilter[0];
+ //debugMsg.value_5 = stateInterialEstimate_viaPointMassKalmanFilter[3];
+ //debugMsg.value_6 = current_xzy_rpy_measurement[0];
+
+
+ // Publish the "debugMsg"
+ //debugPublisher.publish(debugMsg);
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// L QQQ RRRR
+// L Q Q R R
+// L Q Q RRRR
+// L Q Q R R
+// LLLLL QQ Q R R
+// ----------------------------------------------------------------------------------
+
+void calculateControlOutput_viaLQR(float stateErrorBody[12], Controller::Request &request, Controller::Response &response)
+{
+ // SWITCH BETWEEN THE DIFFERENT LQR CONTROLLER MODES:
+ switch (controller_mode)
+ {
+ // LQR controller based on the state vector:
+ // [position,velocity,angles,angular velocity]
+ // commands per motor thrusts
+ case CONTROLLER_MODE_LQR_MOTOR:
+ {
+ // Call the function that performs the control computations for this mode
+ calculateControlOutput_viaLQRforMotors(stateErrorBody,request,response);
+ break;
+ }
+ // LQR controller based on the state vector:
+ // [position,velocity,angles,angular velocity]
+ // commands actuators of total force and bodz torques
+ case CONTROLLER_MODE_LQR_ACTUATOR:
+ {
+ // Call the function that performs the control computations for this mode
+ calculateControlOutput_viaLQRforActuators(stateErrorBody,request,response);
+ break;
+ }
+ // LQR controller based on the state vector:
+ // [position,velocity,angles]
+ case CONTROLLER_MODE_LQR_RATE:
+ {
+ // Call the function that performs the control computations for this mode
+ calculateControlOutput_viaLQRforRates(stateErrorBody,request,response);
+ break;
+ }
+
+ // LQR controller based on the state vector:
+ // [position,velocity]
+ case CONTROLLER_MODE_LQR_ANGLE:
+ {
+ // Call the function that performs the control computations for this mode
+ calculateControlOutput_viaLQRforAngles(stateErrorBody,request,response);
+ break;
+ }
+
+ // LQR controller based on the state vector:
+ // [position,velocity,angles]
+ case CONTROLLER_MODE_LQR_ANGLE_RATE_NESTED:
+ {
+ // Call the function that performs the control computations for this mode
+ calculateControlOutput_viaLQRforAnglesRatesNested(stateErrorBody,request,response);
+ break;
+ }
+
+ // LQR controller based on the state vector:
+ // [position,velocity,angles]
+ case CONTROLLER_MODE_ANGLE_RESPONSE_TEST:
+ {
+ // Call the function that performs the control computations for this mode
+ calculateControlOutput_viaAngleResponseTest(stateErrorBody,request,response);
+ break;
+ }
+
+ default:
+ {
+ // Display that the "controller_mode" was not recognised
+ ROS_INFO_STREAM("[DEMO CONTROLLER] ERROR: in the 'calculateControlOutput' function of the 'DemoControllerService': the 'controller_mode' is not recognised.");
+ // Set everything in the response to zero
+ response.controlOutput.roll = 0;
+ response.controlOutput.pitch = 0;
+ response.controlOutput.yaw = 0;
+ response.controlOutput.motorCmd1 = 0;
+ response.controlOutput.motorCmd2 = 0;
+ response.controlOutput.motorCmd3 = 0;
+ response.controlOutput.motorCmd4 = 0;
+ response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
+ break;
+ }
+ }
+}
+
+
+
+
+void calculateControlOutput_viaLQRforMotors(float stateErrorBody[12], Controller::Request &request, Controller::Response &response)
+{
+ // PERFORM THE "u=Kx" LQR CONTROLLER COMPUTATION
+
+ // Instantiate the local variables for the per motor thrust
+ // adjustment. These will be requested from the Crazyflie's
+ // on-baord "inner-loop" controller
+ float motor1_thrustAdjustment = 0;
+ float motor2_thrustAdjustment = 0;
+ float motor3_thrustAdjustment = 0;
+ float motor4_thrustAdjustment = 0;
+
+ // Perform the "-Kx" LQR computation for the rates and thrust:
+ for(int i = 0; i < 12; ++i)
+ {
+ // MOTORS 1,2,3,4
+ motor1_thrustAdjustment -= gainMatrixMotor1[i] * stateErrorBody[i];
+ motor2_thrustAdjustment -= gainMatrixMotor2[i] * stateErrorBody[i];
+ motor3_thrustAdjustment -= gainMatrixMotor3[i] * stateErrorBody[i];
+ motor4_thrustAdjustment -= gainMatrixMotor4[i] * stateErrorBody[i];
+ }
+
+ //DebugMsg debugMsg;
+
+ // Fill the debugging message with the data provided by Vicon
+ //debugMsg.vicon_x = request.ownCrazyflie.x;
+ //debugMsg.vicon_y = request.ownCrazyflie.y;
+ //debugMsg.vicon_z = request.ownCrazyflie.z;
+ //debugMsg.vicon_roll = request.ownCrazyflie.roll;
+ //debugMsg.vicon_pitch = request.ownCrazyflie.pitch;
+ //debugMsg.vicon_yaw = request.ownCrazyflie.yaw;
+
+ // debugMsg.value_1 = thrustAdjustment;
+ // ......................
+ // debugMsg.value_10 = your_variable_name;
+
+ //debugMsg.value_1 = stateErrorBody[6];
+ //debugMsg.value_2 = stateErrorBody[9];
+
+ //debugMsg.value_3 = motor1_thrustAdjustment;
+ //debugMsg.value_4 = motor2_thrustAdjustment;
+ //debugMsg.value_5 = motor3_thrustAdjustment;
+ //debugMsg.value_6 = motor4_thrustAdjustment;
+
+
+
+
+ // Publish the "debugMsg"
+ //debugPublisher.publish(debugMsg);
+
+
+
+
+ motor1_thrustAdjustment = -gravity_force_quarter*0.9;
+ motor2_thrustAdjustment = -gravity_force_quarter*0.9;
+ motor3_thrustAdjustment = -gravity_force_quarter*0.9;
+ motor4_thrustAdjustment = -gravity_force_quarter*0.9;
+
+ // UPDATE THE "RETURN" THE VARIABLE NAMED "response"
+
+ // Put the roll, pitch, and yaw command to zero
+ response.controlOutput.roll = 0;
+ response.controlOutput.pitch = 0;
+ response.controlOutput.yaw = 0;
+ // > For the thrust adjustment we must add the feed-forward thrust to counter-act gravity.
+ // > NOTE: the "gravity_force_quarter" value was already divided by 4 when
+ // it was loaded/processes from the .yaml file.
+ response.controlOutput.motorCmd1 = computeMotorPolyBackward(motor1_thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd2 = computeMotorPolyBackward(motor2_thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd3 = computeMotorPolyBackward(motor3_thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd4 = computeMotorPolyBackward(motor4_thrustAdjustment + gravity_force_quarter);
+
+
+ // Specify that this controller is a rate controller
+ response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
+
+
+
+
+
+ // An alternate debugging technique is to print out data directly to the
+ // command line from which this node was launched.
+ if (shouldDisplayDebugInfo)
+ {
+ // An example of "printing out" the data from the "request" argument to the
+ // command line. This might be useful for debugging.
+ //ROS_INFO_STREAM("x-coordinate [m]: " << request.ownCrazyflie.x);
+ //ROS_INFO_STREAM("y-coordinate [m]: " << request.ownCrazyflie.y);
+ //ROS_INFO_STREAM("z-coordinate [m]: " << request.ownCrazyflie.z);
+ //ROS_INFO_STREAM("roll [deg]: " << request.ownCrazyflie.roll);
+ //ROS_INFO_STREAM("pitch [deg]: " << request.ownCrazyflie.pitch);
+ //ROS_INFO_STREAM("yaw [deg]: " << request.ownCrazyflie.yaw);
+ //ROS_INFO_STREAM("Delta t [s]: " << request.ownCrazyflie.acquiringTime);
+
+ ROS_INFO_STREAM("f1 [N]: " << motor1_thrustAdjustment);
+ ROS_INFO_STREAM("f2 [N]: " << motor2_thrustAdjustment);
+ ROS_INFO_STREAM("f3 [N]: " << motor3_thrustAdjustment);
+ ROS_INFO_STREAM("f4 [N]: " << motor4_thrustAdjustment);
+
+ }
+}
+
+void calculateControlOutput_viaLQRforActuators(float stateErrorBody[12], Controller::Request &request, Controller::Response &response)
+{
+ // PERFORM THE "u=Kx" LQR CONTROLLER COMPUTATION
+
+ // Instantiate the local variables for the per motor thrust
+ // adjustment. These will be requested from the Crazyflie's
+ // on-baord "inner-loop" controller
+ float thrustAdjustment = 0;
+ float rollTorque = 0;
+ float pitchTorque = 0;
+ float yawTorque = 0;
+
+ // Perform the "-Kx" LQR computation for the rates and thrust:
+ for(int i = 0; i < 12; ++i)
+ {
+ // MOTORS 1,2,3,4
+ thrustAdjustment -= gainMatrixThrust_TwelveStateVector[i] * stateErrorBody[i];
+ rollTorque -= gainMatrixRollTorque[i] * stateErrorBody[i];
+ pitchTorque -= gainMatrixPitchTorque[i] * stateErrorBody[i];
+ yawTorque -= gainMatrixYawTorque[i] * stateErrorBody[i];
+ }
+
+ // DISTRIBUTE POWER
+ float motor1_thrustAdjustment;
+ float motor2_thrustAdjustment;
+ float motor3_thrustAdjustment;
+ float motor4_thrustAdjustment;
+
+ float x = 0.0325;
+ float y = 0.0325;
+ float c = 0.0060;
+
+ float tt = thrustAdjustment/4.0;
+ float tx = rollTorque / y;
+ float ty = pitchTorque / x;
+ float tc = yawTorque / c;
+
+
+ motor1_thrustAdjustment = tt + 0.25 * ( -tx - ty - tc );
+ motor2_thrustAdjustment = tt + 0.25 * ( -tx + ty + tc );
+ motor3_thrustAdjustment = tt + 0.25 * ( tx + ty - tc );
+ motor4_thrustAdjustment = tt + 0.25 * ( tx - ty + tc );
+
+
+ // UPDATE THE "RETURN" THE VARIABLE NAMED "response"
+
+ // Put the computed rates and thrust into the "response" variable
+ // > For roll, pitch, and yaw:
+ response.controlOutput.roll = 0;
+ response.controlOutput.pitch = 0;
+ response.controlOutput.yaw = 0;
+ // > For the thrust adjustment we must add the feed-forward thrust to counter-act gravity.
+ // > NOTE: the "gravity_force_quarter" value was already divided by 4 when
+ // it was loaded/processes from the .yaml file.
+ response.controlOutput.motorCmd1 = computeMotorPolyBackward(motor1_thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd2 = computeMotorPolyBackward(motor2_thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd3 = computeMotorPolyBackward(motor3_thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd4 = computeMotorPolyBackward(motor4_thrustAdjustment + gravity_force_quarter);
+
+
+ // Specify that this controller is a rate controller
+ response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
+
+
+ //DebugMsg debugMsg;
+
+ // Fill the debugging message with the data provided by Vicon
+ //debugMsg.vicon_x = request.ownCrazyflie.x;
+ //debugMsg.vicon_y = request.ownCrazyflie.y;
+ //debugMsg.vicon_z = request.ownCrazyflie.z;
+ //debugMsg.vicon_roll = request.ownCrazyflie.roll;
+ //debugMsg.vicon_pitch = request.ownCrazyflie.pitch;
+ //debugMsg.vicon_yaw = request.ownCrazyflie.yaw;
+
+ // debugMsg.value_1 = thrustAdjustment;
+ // ......................
+ // debugMsg.value_10 = your_variable_name;
+
+ //debugMsg.value_1 = motor1_thrustAdjustment;
+ //debugMsg.value_2 = motor2_thrustAdjustment;
+ //debugMsg.value_3 = motor3_thrustAdjustment;
+ //debugMsg.value_4 = motor4_thrustAdjustment;
+
+
+ // Publish the "debugMsg"
+ //debugPublisher.publish(debugMsg);
+
+ // An alternate debugging technique is to print out data directly to the
+ // command line from which this node was launched.
+ if (shouldDisplayDebugInfo)
+ {
+ // An example of "printing out" the data from the "request" argument to the
+ // command line. This might be useful for debugging.
+ ROS_INFO_STREAM("x-coordinate [m]: " << request.ownCrazyflie.x);
+ ROS_INFO_STREAM("y-coordinate [m]: " << request.ownCrazyflie.y);
+ ROS_INFO_STREAM("z-coordinate [m]: " << request.ownCrazyflie.z);
+ ROS_INFO_STREAM("roll [deg]: " << request.ownCrazyflie.roll);
+ ROS_INFO_STREAM("pitch [deg]: " << request.ownCrazyflie.pitch);
+ ROS_INFO_STREAM("yaw [deg]: " << request.ownCrazyflie.yaw);
+ ROS_INFO_STREAM("Delta t [s]: " << request.ownCrazyflie.acquiringTime);
+
+ }
+}
+
+
+
+void calculateControlOutput_viaLQRforRates(float stateErrorBody[12], Controller::Request &request, Controller::Response &response)
+{
+ // PERFORM THE "u=Kx" LQR CONTROLLER COMPUTATION
+
+ // Instantiate the local variables for the following:
+ // > body frame roll rate,
+ // > body frame pitch rate,
+ // > body frame yaw rate,
+ // > total thrust adjustment.
+ // These will be requested from the Crazyflie's on-baord "inner-loop" controller
+ float rollRate_forResponse = 0;
+ float pitchRate_forResponse = 0;
+ float yawRate_forResponse = 0;
+ float thrustAdjustment = 0;
+
+ // Perform the "-Kx" LQR computation for the rates and thrust:
+ for(int i = 0; i < 9; ++i)
+ {
+ // BODY FRAME Y CONTROLLER
+ rollRate_forResponse -= gainMatrixRollRate[i] * stateErrorBody[i];
+ // BODY FRAME X CONTROLLER
+ pitchRate_forResponse -= gainMatrixPitchRate[i] * stateErrorBody[i];
+ // BODY FRAME YAW CONTROLLER
+ yawRate_forResponse -= gainMatrixYawRate[i] * stateErrorBody[i];
+ // > ALITUDE CONTROLLER (i.e., z-controller):
+ thrustAdjustment -= gainMatrixThrust_NineStateVector[i] * stateErrorBody[i];
+ }
+
+
+ // UPDATE THE "RETURN" THE VARIABLE NAMED "response"
+
+ // Put the computed rates and thrust into the "response" variable
+ // > For roll, pitch, and yaw:
+ response.controlOutput.roll = rollRate_forResponse;
+ response.controlOutput.pitch = pitchRate_forResponse;
+ response.controlOutput.yaw = yawRate_forResponse;
+ // > For the thrust adjustment we must add the feed-forward thrust to counter-act gravity.
+ // > NOTE: remember that the thrust is commanded per motor, so you sohuld
+ // consider whether the "thrustAdjustment" computed by your
+ // controller needed to be divided by 4 or not.
+ thrustAdjustment = thrustAdjustment / 4.0;
+ // > NOTE: the "gravity_force_quarter" value was already divided by 4 when
+ // it was loaded/processes from the .yaml file.
+ response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+
+
+ // Specify that this controller is a rate controller
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
+ response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
+
+
+ // An alternate debugging technique is to print out data directly to the
+ // command line from which this node was launched.
+ if (shouldDisplayDebugInfo)
+ {
+ // An example of "printing out" the data from the "request" argument to the
+ // command line. This might be useful for debugging.
+ ROS_INFO_STREAM("x-coordinate [m]: " << request.ownCrazyflie.x);
+ ROS_INFO_STREAM("y-coordinate [m]: " << request.ownCrazyflie.y);
+ ROS_INFO_STREAM("z-coordinate [m]: " << request.ownCrazyflie.z);
+ ROS_INFO_STREAM("roll [deg]: " << request.ownCrazyflie.roll);
+ ROS_INFO_STREAM("pitch [deg]: " << request.ownCrazyflie.pitch);
+ ROS_INFO_STREAM("yaw [deg]: " << request.ownCrazyflie.yaw);
+ ROS_INFO_STREAM("Delta t [s]: " << request.ownCrazyflie.acquiringTime);
+
+ // An example of "printing out" the control actions computed.
+ ROS_INFO_STREAM("thrustAdjustment = " << thrustAdjustment);
+ ROS_INFO_STREAM("controlOutput.roll = " << response.controlOutput.roll);
+ ROS_INFO_STREAM("controlOutput.pitch = " << response.controlOutput.pitch);
+ ROS_INFO_STREAM("controlOutput.yaw = " << response.controlOutput.yaw);
+
+ // An example of "printing out" the "thrust-to-command" conversion parameters.
+ ROS_INFO_STREAM("motorPoly 0:" << motorPoly[0]);
+ ROS_INFO_STREAM("motorPoly 0:" << motorPoly[1]);
+ ROS_INFO_STREAM("motorPoly 0:" << motorPoly[2]);
+
+ // An example of "printing out" the per motor 16-bit command computed.
+ ROS_INFO_STREAM("controlOutput.cmd1 = " << response.controlOutput.motorCmd1);
+ ROS_INFO_STREAM("controlOutput.cmd3 = " << response.controlOutput.motorCmd2);
+ ROS_INFO_STREAM("controlOutput.cmd2 = " << response.controlOutput.motorCmd3);
+ ROS_INFO_STREAM("controlOutput.cmd4 = " << response.controlOutput.motorCmd4);
+ }
+}
+
+
+
+
+void calculateControlOutput_viaLQRforAngles(float stateErrorBody[12], Controller::Request &request, Controller::Response &response)
+{
+ // PERFORM THE "u=Kx" LQR CONTROLLER COMPUTATION
+
+ // Instantiate the local variables for the following:
+ // > body frame roll angle,
+ // > body frame pitch angle,
+ // > total thrust adjustment.
+ // These will be requested from the Crazyflie's on-baord "inner-loop" controller
+ float rollAngle_forResponse = 0;
+ float pitchAngle_forResponse = 0;
+ float thrustAdjustment = 0;
+
+ // Perform the "-Kx" LQR computation for the rates and thrust:
+ for(int i = 0; i < 6; ++i)
+ {
+ // BODY FRAME Y CONTROLLER
+ rollAngle_forResponse -= gainMatrixRollAngle[i] * stateErrorBody[i];
+ // BODY FRAME X CONTROLLER
+ pitchAngle_forResponse -= gainMatrixPitchAngle[i] * stateErrorBody[i];
+ // > ALITUDE CONTROLLER (i.e., z-controller):
+ thrustAdjustment -= gainMatrixThrust_SixStateVector[i] * stateErrorBody[i];
+ }
+
+ // UPDATE THE "RETURN" THE VARIABLE NAMED "response"
+
+ // Put the computed rates and thrust into the "response" variable
+ // > For roll, pitch, and yaw:
+ response.controlOutput.roll = rollAngle_forResponse;
+ response.controlOutput.pitch = pitchAngle_forResponse;
+ response.controlOutput.yaw = setpoint[3];
+ // > For the thrust adjustment we must add the feed-forward thrust to counter-act gravity.
+ // > NOTE: remember that the thrust is commanded per motor, so you sohuld
+ // consider whether the "thrustAdjustment" computed by your
+ // controller needed to be divided by 4 or not.
+ thrustAdjustment = thrustAdjustment / 4.0;
+ // > NOTE: the "gravity_force_quarter" value was already divided by 4 when
+ // it was loaded/processes from the .yaml file.
+ response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+
+
+ // Specify that this controller is a rate controller
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
+ response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
+
+
+}
+
+
+
+void calculateControlOutput_viaLQRforAnglesRatesNested( float stateErrorBody[12], Controller::Request &request, Controller::Response &response)
+{
+ // PERFORM THE NESTED "u=Kx" LQR CONTROLLER COMPUTATION
+
+ // Increment the counter variable
+ lqr_angleRateNested_counter++;
+
+ if (lqr_angleRateNested_counter > 4)
+ {
+ //ROS_INFO("Outer called");
+
+ // Reset the counter to 1
+ lqr_angleRateNested_counter = 1;
+
+ // PERFORM THE OUTER "u=Kx" LQR CONTROLLER COMPUTATION
+
+ // Reset the class variable to zero for the following:
+ // > body frame roll angle,
+ // > body frame pitch angle,
+ // > body frame yaw angle,
+ // > total thrust adjustment.
+ // These will be requested from the "inner-loop" LQR controller below
+ lqr_angleRateNested_prev_rollAngle = 0;
+ lqr_angleRateNested_prev_pitchAngle = 0;
+ lqr_angleRateNested_prev_thrustAdjustment = 0;
+
+ // Perform the "-Kx" LQR computation for the rates and thrust:
+ for(int i = 0; i < 6; ++i)
+ {
+ // BODY FRAME Y CONTROLLER
+ lqr_angleRateNested_prev_rollAngle -= gainMatrixRollAngle_50Hz[i] * stateErrorBody[i];
+ // BODY FRAME X CONTROLLER
+ lqr_angleRateNested_prev_pitchAngle -= gainMatrixPitchAngle_50Hz[i] * stateErrorBody[i];
+ // > ALITUDE CONTROLLER (i.e., z-controller):
+ lqr_angleRateNested_prev_thrustAdjustment -= gainMatrixThrust_SixStateVector_50Hz[i] * stateErrorBody[i];
+ }
+
+ // BODY FRAME Z CONTROLLER
+ //lqr_angleRateNested_prev_yawAngle = setpoint[3];
+ lqr_angleRateNested_prev_yawAngle = stateErrorBody[8];
+
+
+ }
+
+ //ROS_INFO("Inner called");
+
+ // PERFORM THE INNER "u=Kx" LQR CONTROLLER COMPUTATION
+ // Instantiate the local variables for the following:
+ // > body frame roll rate,
+ // > body frame pitch rate,
+ // > body frame yaw rate,
+ // These will be requested from the Crazyflie's on-baord "inner-loop" controller
+ float rollRate_forResponse = 0;
+ float pitchRate_forResponse = 0;
+ float yawRate_forResponse = 0;
+
+ // Create the angle error to use for the inner controller
+ float temp_stateAngleError[3] = {
+ stateErrorBody[6] - lqr_angleRateNested_prev_rollAngle,
+ stateErrorBody[7] - lqr_angleRateNested_prev_pitchAngle,
+ lqr_angleRateNested_prev_yawAngle
+ };
+
+ // Perform the "-Kx" LQR computation for the rates and thrust:
+ for(int i = 0; i < 4; ++i)
+ {
+ // BODY FRAME Y CONTROLLER
+ rollRate_forResponse -= gainMatrixRollRate_Nested[i] * temp_stateAngleError[i];
+ // BODY FRAME X CONTROLLER
+ pitchRate_forResponse -= gainMatrixPitchRate_Nested[i] * temp_stateAngleError[i];
+ // BODY FRAME Z CONTROLLER
+ yawRate_forResponse -= gainMatrixYawRate_Nested[i] * temp_stateAngleError[i];
+ }
+
+
+ // UPDATE THE "RETURN" THE VARIABLE NAMED "response"
+
+ // Put the computed rates and thrust into the "response" variable
+ // > For roll, pitch, and yaw:
+ response.controlOutput.roll = rollRate_forResponse;
+ response.controlOutput.pitch = pitchRate_forResponse;
+ response.controlOutput.yaw = yawRate_forResponse;
+ // > For the thrust adjustment we must add the feed-forward thrust to counter-act gravity.
+ // > NOTE: remember that the thrust is commanded per motor, so you sohuld
+ // consider whether the "thrustAdjustment" computed by your
+ // controller needed to be divided by 4 or not.
+ float thrustAdjustment = lqr_angleRateNested_prev_thrustAdjustment / 4.0;
+ // > NOTE: the "gravity_force_quarter" value was already divided by 4 when
+ // it was loaded/processes from the .yaml file.
+ response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+
+
+ // Specify that this controller is a rate controller
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
+ response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
+
+ // Display some details (if requested)
+ if (shouldDisplayDebugInfo)
+ {
+ ROS_INFO_STREAM("thrust =" << lqr_angleRateNested_prev_thrustAdjustment );
+ ROS_INFO_STREAM("rollrate =" << response.controlOutput.roll );
+ ROS_INFO_STREAM("pitchrate =" << response.controlOutput.pitch );
+ ROS_INFO_STREAM("yawrate =" << response.controlOutput.yaw );
+ }
+}
+
+
+
+
+void calculateControlOutput_viaAngleResponseTest( float stateErrorBody[12], Controller::Request &request, Controller::Response &response)
+{
+ // PERFORM THE NESTED "u=Kx" LQR CONTROLLER COMPUTATION
+
+ // Increment the counter variable
+ angleResponseTest_counter++;
+
+ if (angleResponseTest_counter > 4)
+ {
+ //ROS_INFO("Outer called");
+
+ // Reset the counter to 1
+ angleResponseTest_counter = 1;
+
+ // PERFORM THE OUTER "u=Kx" LQR CONTROLLER COMPUTATION
+
+ // Reset the class variable to zero for the following:
+ // > body frame roll angle,
+ // > body frame pitch angle,
+ // > body frame yaw angle,
+ // > total thrust adjustment.
+ // These will be requested from the "inner-loop" LQR controller below
+ //angleResponseTest_prev_rollAngle = 0;
+ angleResponseTest_prev_pitchAngle = 0;
+ angleResponseTest_prev_thrustAdjustment = 0;
+
+ // Perform the "-Kx" LQR computation for the rates and thrust:
+ for(int i = 0; i < 6; ++i)
+ {
+ // BODY FRAME Y CONTROLLER
+ //angleResponseTest_prev_rollAngle -= gainMatrixRollAngle_50Hz[i] * stateErrorBody[i];
+ // BODY FRAME X CONTROLLER
+ angleResponseTest_prev_pitchAngle -= gainMatrixPitchAngle_50Hz[i] * stateErrorBody[i];
+ // > ALITUDE CONTROLLER (i.e., z-controller):
+ angleResponseTest_prev_thrustAdjustment -= gainMatrixThrust_SixStateVector_50Hz[i] * stateErrorBody[i];
+ }
+
+ // BODY FRAME Z CONTROLLER
+ //angleResponseTest_prev_yawAngle = setpoint[3];
+ angleResponseTest_prev_yawAngle = stateErrorBody[8];
+
+ // COMPUTE THE DISTANCE FROM THE ORIGIN
+ // > for pitch response testing
+ // if (stateErrorBody[0] > angleResponseTest_distance_meters)
+ // {
+ // angleResponseTest_prev_pitchAngle = -angleResponseTest_pitchAngle_radians;
+ // }
+ // else if (stateErrorBody[0] < (-angleResponseTest_distance_meters) )
+ // {
+ // angleResponseTest_prev_pitchAngle = angleResponseTest_pitchAngle_radians;
+ // }
+ // > for roll response testing
+ if (stateErrorBody[1] > angleResponseTest_distance_meters)
+ {
+ angleResponseTest_prev_rollAngle = angleResponseTest_pitchAngle_radians;
+ }
+ else if (stateErrorBody[1] < (-angleResponseTest_distance_meters) )
+ {
+ angleResponseTest_prev_rollAngle = -angleResponseTest_pitchAngle_radians;
+ }
+
+
+
+ }
+
+ //ROS_INFO("Inner called");
+
+ // PERFORM THE INNER "u=Kx" LQR CONTROLLER COMPUTATION
+ // Instantiate the local variables for the following:
+ // > body frame roll rate,
+ // > body frame pitch rate,
+ // > body frame yaw rate,
+ // These will be requested from the Crazyflie's on-baord "inner-loop" controller
+ float rollRate_forResponse = 0;
+ float pitchRate_forResponse = 0;
+ float yawRate_forResponse = 0;
+
+ // Create the angle error to use for the inner controller
+ float temp_stateAngleError[3] = {
+ stateErrorBody[6] - angleResponseTest_prev_rollAngle,
+ stateErrorBody[7] - angleResponseTest_prev_pitchAngle,
+ angleResponseTest_prev_yawAngle
+ };
+
+ // Perform the "-Kx" LQR computation for the rates and thrust:
+ for(int i = 0; i < 4; ++i)
+ {
+ // BODY FRAME Y CONTROLLER
+ rollRate_forResponse -= gainMatrixRollRate_Nested[i] * temp_stateAngleError[i];
+ // BODY FRAME X CONTROLLER
+ pitchRate_forResponse -= gainMatrixPitchRate_Nested[i] * temp_stateAngleError[i];
+ // BODY FRAME Z CONTROLLER
+ yawRate_forResponse -= gainMatrixYawRate_Nested[i] * temp_stateAngleError[i];
+ }
+
+
+ // UPDATE THE "RETURN" THE VARIABLE NAMED "response"
+
+ // Put the computed rates and thrust into the "response" variable
+ // > For roll, pitch, and yaw:
+ response.controlOutput.roll = rollRate_forResponse;
+ response.controlOutput.pitch = pitchRate_forResponse;
+ response.controlOutput.yaw = yawRate_forResponse;
+ // > For the thrust adjustment we must add the feed-forward thrust to counter-act gravity.
+ // > NOTE: remember that the thrust is commanded per motor, so you sohuld
+ // consider whether the "thrustAdjustment" computed by your
+ // controller needed to be divided by 4 or not.
+ float thrustAdjustment = angleResponseTest_prev_thrustAdjustment / 4.0;
+ // > NOTE: the "gravity_force_quarter" value was already divided by 4 when
+ // it was loaded/processes from the .yaml file.
+ response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+
+
+ // Specify that this controller is a rate controller
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
+ response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
+
+ // Display some details (if requested)
+ if (shouldDisplayDebugInfo)
+ {
+ ROS_INFO_STREAM("thrust =" << angleResponseTest_prev_thrustAdjustment );
+ ROS_INFO_STREAM("rollrate =" << response.controlOutput.roll );
+ ROS_INFO_STREAM("pitchrate =" << response.controlOutput.pitch );
+ ROS_INFO_STREAM("yawrate =" << response.controlOutput.yaw );
+ }
+}
+
+
+
+
+
+
+
+
+
+
+
+void construct_and_publish_debug_message(Controller::Request &request, Controller::Response &response)
+{
+ // ***********************************************************
+ // DDDD EEEEE BBBB U U GGGG M M SSSS GGGG
+ // D D E B B U U G MM MM S G
+ // D D EEE BBBB U U G M M M SSS G
+ // D D E B B U U G G M M S G G
+ // DDDD EEEEE BBBB UUU GGGG M M SSSS GGGG
+
+ // DEBUGGING CODE:
+ // As part of the D-FaLL-System we have defined a message type names"DebugMsg".
+ // By fill this message with data and publishing it you can display the data in
+ // real time using rpt plots. Instructions for using rqt plots can be found on
+ // the wiki of the D-FaLL-System repository
+
+ // Instantiate a local variable of type "DebugMsg", see the file "DebugMsg.msg"
+ // (located in the "msg" folder) to see the full structure of this message.
+ DebugMsg debugMsg;
+
+ // Fill the debugging message with the data provided by Vicon
+ debugMsg.vicon_x = request.ownCrazyflie.x;
+ debugMsg.vicon_y = request.ownCrazyflie.y;
+ debugMsg.vicon_z = request.ownCrazyflie.z;
+ debugMsg.vicon_roll = request.ownCrazyflie.roll;
+ debugMsg.vicon_pitch = request.ownCrazyflie.pitch;
+ debugMsg.vicon_yaw = request.ownCrazyflie.yaw;
+
+ // Fill in the debugging message with any other data you would like to display
+ // in real time. For example, it might be useful to display the thrust
+ // adjustment computed by the z-altitude controller.
+ // The "DebugMsg" type has 10 properties from "value_1" to "value_10", all of
+ // type "float64" that you can fill in with data you would like to plot in
+ // real-time.
+ // debugMsg.value_1 = thrustAdjustment;
+ // ......................
+ // debugMsg.value_10 = your_variable_name;
+ debugMsg.value_1 = angleResponseTest_prev_pitchAngle;
+
+ // Publish the "debugMsg"
+ debugPublisher.publish(debugMsg);
+}
+
+
+
+
+// ------------------------------------------------------------------------------
+// RRRR OOO TTTTT A TTTTT EEEEE III N N TTTTT OOO
+// R R O O T A A T E I NN N T O O
+// RRRR O O T A A T EEE I N N N T O O
+// R R O O T AAAAA T E I N NN T O O
+// R R OOO T A A T EEEEE III N N T OOO
+//
+// BBBB OOO DDDD Y Y FFFFF RRRR A M M EEEEE
+// B B O O D D Y Y F R R A A MM MM E
+// BBBB O O D D Y FFF RRRR A A M M M EEE
+// B B O O D D Y F R R AAAAA M M E
+// BBBB OOO DDDD Y F R R A A M M EEEEE
+// ------------------------------------------------------------------------------
+
+// The arguments for this function are as follows:
+// stateInertial
+// This is an array of length 9 with the estimates the error of of the following values
+// relative to the sepcifed setpoint:
+// stateInertial[0] x position of the Crazyflie relative to the inertial frame origin [meters]
+// stateInertial[1] y position of the Crazyflie relative to the inertial frame origin [meters]
+// stateInertial[2] z position of the Crazyflie relative to the inertial frame origin [meters]
+// stateInertial[3] x-axis component of the velocity of the Crazyflie in the inertial frame [meters/second]
+// stateInertial[4] y-axis component of the velocity of the Crazyflie in the inertial frame [meters/second]
+// stateInertial[5] z-axis component of the velocity of the Crazyflie in the inertial frame [meters/second]
+// stateInertial[6] The roll component of the intrinsic Euler angles [radians]
+// stateInertial[7] The pitch component of the intrinsic Euler angles [radians]
+// stateInertial[8] The yaw component of the intrinsic Euler angles [radians]
+//
+// stateBody
+// This is an empty array of length 9, this function should fill in all elements of this
+// array with the same ordering as for the "stateInertial" argument, expect that the (x,y)
+// position and (x,y) velocities are rotated into the body frame.
+//
+// yaw_measured
+// This is the yaw component of the intrinsic Euler angles in [radians] as measured by
+// the Vicon motion capture system
+//
+void convertIntoBodyFrame(float stateInertial[12], float (&stateBody)[12], float yaw_measured)
+{
+ if (shouldPerformConvertIntoBodyFrame)
+ {
+ float sinYaw = sin(yaw_measured);
+ float cosYaw = cos(yaw_measured);
+
+ // Fill in the (x,y,z) position estimates to be returned
+ stateBody[0] = stateInertial[0] * cosYaw + stateInertial[1] * sinYaw;
+ stateBody[1] = stateInertial[1] * cosYaw - stateInertial[0] * sinYaw;
+ stateBody[2] = stateInertial[2];
+
+ // Fill in the (x,y,z) velocity estimates to be returned
+ stateBody[3] = stateInertial[3] * cosYaw + stateInertial[4] * sinYaw;
+ stateBody[4] = stateInertial[4] * cosYaw - stateInertial[3] * sinYaw;
+ stateBody[5] = stateInertial[5];
+
+ // Fill in the (roll,pitch,yaw) estimates to be returned
+ stateBody[6] = stateInertial[6];
+ stateBody[7] = stateInertial[7];
+ stateBody[8] = stateInertial[8];
+
+ // Fill in the (roll,pitch,yaw) velocity estimates to be returned
+ stateBody[9] = stateInertial[9];
+ stateBody[10] = stateInertial[10];
+ stateBody[11] = stateInertial[11];
+ }
+ else
+ {
+ // Fill in the (x,y,z) position estimates to be returned
+ stateBody[0] = stateInertial[0];
+ stateBody[1] = stateInertial[1];
+ stateBody[2] = stateInertial[2];
+
+ // Fill in the (x,y,z) velocity estimates to be returned
+ stateBody[3] = stateInertial[3];
+ stateBody[4] = stateInertial[4];
+ stateBody[5] = stateInertial[5];
+
+ // Fill in the (roll,pitch,yaw) estimates to be returned
+ stateBody[6] = stateInertial[6];
+ stateBody[7] = stateInertial[7];
+ stateBody[8] = stateInertial[8];
+
+ // Fill in the (roll,pitch,yaw) velocity estimates to be returned
+ stateBody[9] = stateInertial[9];
+ stateBody[10] = stateInertial[10];
+ stateBody[11] = stateInertial[11];
+ }
+}
+
+
+
+
+void convert_stateInertial_to_bodyFrameError(float stateInertial[12], float setpoint[4], float (&bodyFrameError)[12])
+{
+ // Store the current YAW in a local variable
+ float temp_stateInertial_yaw = stateInertial[8];
+
+ // Adjust the INERTIAL (x,y,z) position for the setpoint
+ stateInertial[0] = stateInertial[0] - setpoint[0];
+ stateInertial[1] = stateInertial[1] - setpoint[1];
+ stateInertial[2] = stateInertial[2] - setpoint[2];
+
+ // Fill in the yaw angle error
+ // > This error should be "unwrapped" to be in the range
+ // ( -pi , pi )
+ // > First, get the yaw error into a local variable
+ float yawError = stateInertial[8] - setpoint[3];
+ // > Second, "unwrap" the yaw error to the interval ( -pi , pi )
+ while(yawError > PI) {yawError -= 2 * PI;}
+ while(yawError < -PI) {yawError += 2 * PI;}
+ // > Third, put the "yawError" into the "stateError" variable
+ stateInertial[8] = yawError;
+
+
+ if (yawError>(PI/6))
+ {
+ yawError = (PI/6);
+ }
+ else if (yawError<(-PI/6))
+ {
+ yawError = (-PI/6);
+ }
+
+ // CONVERSION INTO BODY FRAME
+ // Conver the state erorr from the Inertial frame into the Body frame
+ // > Note: the function "convertIntoBodyFrame" is implemented in this file
+ // and by default does not perform any conversion. The equations to convert
+ // the state error into the body frame should be implemented in that function
+ // for successful completion of the PPS exercise
+ convertIntoBodyFrame(stateInertial, bodyFrameError, temp_stateInertial_yaw);
+}
+
+
+
+
+// ------------------------------------------------------------------------------
+// N N EEEEE W W TTTTT OOO N N CCCC M M DDDD
+// NN N E W W T O O NN N C MM MM D D
+// N N N EEE W W T O O N N N --> C M M M D D
+// N NN E W W W T O O N NN C M M D D
+// N N EEEEE W W T OOO N N CCCC M M DDDD
+//
+// CCCC OOO N N V V EEEEE RRRR SSSS III OOO N N
+// C O O NN N V V E R R S I O O NN N
+// C O O N N N V V EEE RRRR SSS I O O N N N
+// C O O N NN V V E R R S I O O N NN
+// CCCC OOO N N V EEEEE R R SSSS III OOO N N
+// ------------------------------------------------------------------------------
+
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+float computeMotorPolyBackward(float thrust)
+{
+ // Compute the 16-bit command signal that generates the "thrust" force
+ float cmd = (-motorPoly[1] + sqrt(motorPoly[1] * motorPoly[1] - 4 * motorPoly[2] * (motorPoly[0] - thrust))) / (2 * motorPoly[2]);
+
+ // Saturate the signal to be 0 or in the range [1000,65000]
+ if (cmd < cmd_sixteenbit_min)
+ {
+ cmd = 0;
+ }
+ else if (cmd > cmd_sixteenbit_max)
+ {
+ cmd = cmd_sixteenbit_max;
+ }
+
+ return cmd;
+}
+
+
+
+
+// ----------------------------------------------------------------------------------
+// N N EEEEE W W SSSS EEEEE TTTTT PPPP OOO III N N TTTTT
+// NN N E W W S E T P P O O I NN N T
+// N N N EEE W W SSS EEE T PPPP O O I N N N T
+// N NN E W W W S E T P O O I N NN T
+// N N EEEEE W W SSSS EEEEE T P OOO III N N T
+//
+// GGG U U III CCCC A L L BBBB A CCCC K K
+// G G U U I C A A L L B B A A C K K
+// G U U I C A A L L BBBB A A C KKK
+// G G U U I C AAAAA L L B B AAAAA C K K
+// GGGG UUU III CCCC A A LLLLL LLLLL BBBB A A CCCC K K
+// ----------------------------------------------------------------------------------
+
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+void setpointCallback(const Setpoint& newSetpoint)
+{
+ setpoint[0] = newSetpoint.x;
+ setpoint[1] = newSetpoint.y;
+ setpoint[2] = newSetpoint.z;
+ setpoint[3] = newSetpoint.yaw;
+}
+
+
+
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+// > This function is called anytime a message is published on the topic to which the
+// class instance variable "xyz_yaw_to_follow_subscriber" is subscribed
+void xyz_yaw_to_follow_callback(const Setpoint& newSetpoint)
+{
+ //ROS_INFO("DEBUGGING: Received new setpoint from another agent");
+ // The setpoint should only be updated if allow by the respective booelan
+ if (shouldFollowAnotherAgent)
+ {
+ setpoint[0] = newSetpoint.x;
+ setpoint[1] = newSetpoint.y;
+ setpoint[2] = newSetpoint.z;
+ setpoint[3] = newSetpoint.yaw;
+ }
+}
+
+
+
+// ----------------------------------------------------------------------------------
+// CCCC U U SSSS TTTTT OOO M M
+// C U U S T O O MM MM
+// C U U SSS T O O M M M
+// C U U S T O O M M
+// CCCC UUU SSSS T OOO M M
+//
+// CCCC OOO M M M M A N N DDDD
+// C O O MM MM MM MM A A NN N D D
+// C O O M M M M M M A A N N N D D
+// C O O M M M M AAAAA N NN D D
+// CCCC OOO M M M M A A N N DDDD
+// ----------------------------------------------------------------------------------
+
+// CUSTOM COMMAND RECEIVED CALLBACK
+void customCommandReceivedCallback(const CustomButton& commandReceived)
+{
+ // Extract the data from the message
+ int custom_button_index = commandReceived.button_index;
+ float custom_command_code = commandReceived.command_code;
+
+ // Switch between the button pressed
+ switch(custom_button_index)
+ {
+
+ // > FOR CUSTOM BUTTON 1
+ case 1:
+ {
+ // Let the user know that this part of the code was triggered
+ ROS_INFO("[DEMO CONTROLLER] Custom button 1 received in controller.");
+ // Code here to respond to custom button 1
+
+ break;
+ }
+
+ // > FOR CUSTOM BUTTON 2
+ case 2:
+ {
+ // Let the user know that this part of the code was triggered
+ ROS_INFO("[DEMO CONTROLLER] Custom button 2 received in controller.");
+ // Code here to respond to custom button 2
+
+ break;
+ }
+
+ // > FOR CUSTOM BUTTON 3
+ case 3:
+ {
+ // Let the user know that this part of the code was triggered
+ ROS_INFO_STREAM("[DEMO CONTROLLER] Custom button 3 received in controller, with command code:" << custom_command_code );
+ // Code here to respond to custom button 3
+
+ break;
+ }
+
+ default:
+ {
+ // Let the user know that the command was not recognised
+ ROS_INFO_STREAM("[DEMO CONTROLLER] A custom command was received in the controller but not recognised, message.button_index = " << custom_button_index << ", and message.command_code = " << custom_command_code );
+ break;
+ }
+ }
+}
+
+
+
+// ************************************************************************************************
+// PPPP U U BBBB L III SSSS H H X X Y Y ZZZZZ Y Y A W W
+// P P U U B B L I S H H X X Y Y Z Y Y A A W W
+// PPPP U U BBBB L I SSS HHHH X Y Z Y A A W W
+// P U U B B L I S H H X X Y Z Y AAAAA W W W
+// P UUU BBBB LLLLL III SSSS H H X X Y ZZZZZ Y A A W W
+
+// PUBLISH THE CURRENT X,Y,Z, AND YAW (if required)
+void publish_current_xyz_yaw(float x, float y, float z, float yaw)
+{
+ // publish setpoint for FollowController of another student group
+ Setpoint temp_current_xyz_yaw;
+ // Fill in the x,y,z, and yaw info directly from the data provided to this
+ // function
+ temp_current_xyz_yaw.x = x;
+ temp_current_xyz_yaw.y = y;
+ temp_current_xyz_yaw.z = z;
+ temp_current_xyz_yaw.yaw = yaw;
+ my_current_xyz_yaw_publisher.publish(temp_current_xyz_yaw);
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// L OOO A DDDD
+// L O O A A D D
+// L O O A A D D
+// L O O AAAAA D D
+// LLLLL OOO A A DDDD
+//
+// PPPP A RRRR A M M EEEEE TTTTT EEEEE RRRR SSSS
+// P P A A R R A A MM MM E T E R R S
+// PPPP A A RRRR A A M M M EEE T EEE RRRR SSS
+// P AAAAA R R AAAAA M M E T E R R S
+// P A A R R A A M M EEEEE T EEEEE R R SSSS
+// ----------------------------------------------------------------------------------
+
+
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+void yamlReadyForFetchCallback(const std_msgs::Int32& msg)
+{
+ // Extract from the "msg" for which controller the and from where to fetch the YAML
+ // parameters
+ int controller_to_fetch_yaml = msg.data;
+
+ // Switch between fetching for the different controllers and from different locations
+ switch(controller_to_fetch_yaml)
+ {
+
+ // > FOR FETCHING FROM THE AGENT'S OWN PARAMETER SERVICE
+ case FETCH_YAML_DEMO_CONTROLLER_FROM_OWN_AGENT:
+ {
+ // Let the user know that this message was received
+ ROS_INFO("[DEMO CONTROLLER] Received the message that YAML parameters were (re-)loaded. > Now fetching the parameter values from this agent.");
+ // Create a node handle to the parameter service running on this agent's machine
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(namespace_to_own_agent_parameter_service);
+ // Call the function that fetches the parameters
+ fetchYamlParameters(nodeHandle_to_own_agent_parameter_service);
+ break;
+ }
+
+ // > FOR FETCHING FROM THE COORDINATOR'S PARAMETER SERVICE
+ case FETCH_YAML_DEMO_CONTROLLER_FROM_COORDINATOR:
+ {
+ // Let the user know that this message was received
+ ROS_INFO("[DEMO CONTROLLER] Received the message that YAML parameters were (re-)loaded. > Now fetching the parameter values from the coordinator.");
+ // Create a node handle to the parameter service running on the coordinator machine
+ ros::NodeHandle nodeHandle_to_coordinator_parameter_service(namespace_to_coordinator_parameter_service);
+ // Call the function that fetches the parameters
+ fetchYamlParameters(nodeHandle_to_coordinator_parameter_service);
+ break;
+ }
+
+ default:
+ {
+ // Let the user know that the command was not relevant
+ //ROS_INFO("The DemoControllerService received the message that YAML parameters were (re-)loaded");
+ //ROS_INFO("> However the parameters do not relate to this controller, hence nothing will be fetched.");
+ break;
+ }
+ }
+}
+
+
+// This function CAN BE edited for successful completion of the PPS exercise, and the
+// use of parameters fetched from the YAML file is highly recommended to make tuning of
+// your controller easier and quicker.
+void fetchYamlParameters(ros::NodeHandle& nodeHandle)
+{
+ // Here we load the parameters that are specified in the DemoController.yaml file
+
+ // Add the "DemoController" namespace to the "nodeHandle"
+ ros::NodeHandle nodeHandle_for_demoController(nodeHandle, "DemoController");
+
+ // > The mass of the crazyflie
+ cf_mass = getParameterFloat(nodeHandle_for_demoController , "mass");
+
+ // Display one of the YAML parameters to debug if it is working correctly
+ //ROS_INFO_STREAM("DEBUGGING: mass leaded from loacl file = " << cf_mass );
+
+ // > The frequency at which the "computeControlOutput" is being called, as determined
+ // by the frequency at which the Vicon system provides position and attitude data
+ vicon_frequency = getParameterFloat(nodeHandle_for_demoController, "vicon_frequency");
+
+ // > The frequency at which the "computeControlOutput" is being called, as determined
+ // by the frequency at which the Vicon system provides position and attitude data
+ control_frequency = getParameterFloat(nodeHandle_for_demoController, "control_frequency");
+
+ // > The co-efficients of the quadratic conversation from 16-bit motor command to
+ // thrust force in Newtons
+ getParameterFloatVector(nodeHandle_for_demoController, "motorPoly", motorPoly, 3);
+
+ // > The boolean for whether to execute the convert into body frame function
+ shouldPerformConvertIntoBodyFrame = getParameterBool(nodeHandle_for_demoController, "shouldPerformConvertIntoBodyFrame");
+
+ // > The boolean indicating whether the (x,y,z,yaw) of this agent should be published
+ // or not
+ shouldPublishCurrent_xyz_yaw = getParameterBool(nodeHandle_for_demoController, "shouldPublishCurrent_xyz_yaw");
+
+ // > The boolean indicating whether the (x,y,z,yaw) setpoint of this agent should adapted in
+ // an attempt to follow the "my_current_xyz_yaw_topic" from another agent
+ shouldFollowAnotherAgent = getParameterBool(nodeHandle_for_demoController, "shouldFollowAnotherAgent");
+
+ // > The ordered vector for ID's that specifies how the agents should follow eachother
+ follow_in_a_line_agentIDs.clear();
+ int temp_number_of_agents_in_a_line = getParameterIntVectorWithUnknownLength(nodeHandle_for_demoController, "follow_in_a_line_agentIDs", follow_in_a_line_agentIDs);
+ // > Double check that the sizes agree
+ if ( temp_number_of_agents_in_a_line != follow_in_a_line_agentIDs.size() )
+ {
+ // Update the user if the sizes don't agree
+ ROS_ERROR_STREAM("parameter 'follow_in_a_line_agentIDs' was loaded with two different lengths, " << temp_number_of_agents_in_a_line << " versus " << follow_in_a_line_agentIDs.size() );
+ }
+
+ // Boolean indiciating whether the "Debug Message" of this agent should be published or not
+ shouldPublishDebugMessage = getParameterBool(nodeHandle_for_demoController, "shouldPublishDebugMessage");
+
+ // Boolean indiciating whether the debugging ROS_INFO_STREAM should be displayed or not
+ shouldDisplayDebugInfo = getParameterBool(nodeHandle_for_demoController, "shouldDisplayDebugInfo");
+
+
+ // THE CONTROLLER GAINS:
+
+ // A flag for which controller to use:
+ controller_mode = getParameterInt( nodeHandle_for_demoController , "controller_mode" );
+
+ // A flag for which estimator to use:
+ estimator_method = getParameterInt( nodeHandle_for_demoController , "estimator_method" );
+
+ // The LQR Controller parameters for "LQR_MODE_MOTOR"
+ getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixMotor1", gainMatrixMotor1, 12);
+ getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixMotor2", gainMatrixMotor2, 12);
+ getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixMotor3", gainMatrixMotor3, 12);
+ getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixMotor4", gainMatrixMotor4, 12);
+
+ // The LQR Controller parameters for "LQR_MODE_MOTOR"
+ getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixThrust_TwelveStateVector", gainMatrixThrust_TwelveStateVector, 12);
+ getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixRollTorque", gainMatrixRollTorque, 12);
+ getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixPitchTorque", gainMatrixPitchTorque, 12);
+ getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixYawTorque", gainMatrixYawTorque, 12);
+
+ // The LQR Controller parameters for "LQR_MODE_RATE"
+ getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixThrust_NineStateVector", gainMatrixThrust_NineStateVector, 9);
+ getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixRollRate", gainMatrixRollRate, 9);
+ getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixPitchRate", gainMatrixPitchRate, 9);
+ getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixYawRate", gainMatrixYawRate, 9);
+
+ // The LQR Controller parameters for "LQR_MODE_ANGLE"
+ getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixThrust_SixStateVector", gainMatrixThrust_SixStateVector, 6);
+ getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixRollAngle", gainMatrixRollAngle, 6);
+ getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixPitchAngle", gainMatrixPitchAngle, 6);
+
+ // The LQR Controller parameters for "LQR_MODE_ANGLE_RATE_NESTED"
+ getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixThrust_SixStateVector_50Hz", gainMatrixThrust_SixStateVector_50Hz, 6);
+ getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixRollAngle_50Hz", gainMatrixRollAngle_50Hz, 6);
+ getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixPitchAngle_50Hz", gainMatrixPitchAngle_50Hz, 6);
+
+ getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixRollRate_Nested", gainMatrixRollRate_Nested, 3);
+ getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixPitchRate_Nested", gainMatrixPitchRate_Nested, 3);
+ getParameterFloatVector(nodeHandle_for_demoController, "gainMatrixYawRate_Nested", gainMatrixYawRate_Nested, 3);
+
+ // The parameters for the "Angle Reponse Test" controller mode
+ angleResponseTest_pitchAngle_degrees = getParameterFloat(nodeHandle_for_demoController, "angleResponseTest_pitchAngle_degrees");
+ angleResponseTest_distance_meters = getParameterFloat(nodeHandle_for_demoController, "angleResponseTest_distance_meters");
+
+ // 16-BIT COMMAND LIMITS
+ cmd_sixteenbit_min = getParameterFloat(nodeHandle_for_demoController, "command_sixteenbit_min");
+ cmd_sixteenbit_max = getParameterFloat(nodeHandle_for_demoController, "command_sixteenbit_max");
+
+
+ // THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
+ // > For the (x,y,z) position
+ getParameterFloatVector(nodeHandle_for_demoController, "PMKF_Ahat_row1_for_positions", PMKF_Ahat_row1_for_positions, 2);
+ getParameterFloatVector(nodeHandle_for_demoController, "PMKF_Ahat_row2_for_positions", PMKF_Ahat_row2_for_positions, 2);
+ getParameterFloatVector(nodeHandle_for_demoController, "PMKF_Kinf_for_positions" , PMKF_Kinf_for_positions , 2);
+ // > For the (roll,pitch,yaw) angles
+ getParameterFloatVector(nodeHandle_for_demoController, "PMKF_Ahat_row1_for_angles", PMKF_Ahat_row1_for_angles, 2);
+ getParameterFloatVector(nodeHandle_for_demoController, "PMKF_Ahat_row2_for_angles", PMKF_Ahat_row2_for_angles, 2);
+ getParameterFloatVector(nodeHandle_for_demoController, "PMKF_Kinf_for_angles" , PMKF_Kinf_for_angles , 2);
+
+
+ // DEBUGGING: Print out one of the parameters that was loaded
+ ROS_INFO_STREAM("[DEMO CONTROLLER] DEBUGGING: the fetched DemoController/mass = " << cf_mass);
+ ROS_INFO_STREAM("[DEMO CONTROLLER] DEBUGGING: the fetched DemoController/angleResponseTest_pitchAngle_degrees = " << angleResponseTest_pitchAngle_degrees);
+
+ // Call the function that computes details an values that are needed from these
+ // parameters loaded above
+ processFetchedParameters();
+
+}
+
+
+// This function CAN BE edited for successful completion of the PPS exercise, and the
+// use of parameters loaded from the YAML file is highly recommended to make tuning of
+// your controller easier and quicker.
+void processFetchedParameters()
+{
+ // Compute the feed-forward force that we need to counteract gravity (i.e., mg)
+ // > in units of [Newtons]
+ gravity_force = cf_mass * 9.81/(1000);
+ gravity_force_quarter = cf_mass * 9.81/(1000*4);
+
+ // Set that the estimator frequency is the same as the control frequency
+ estimator_frequency = vicon_frequency;
+
+ // Convert from degrees to radians
+ angleResponseTest_pitchAngle_radians = (PI/180.0) * angleResponseTest_pitchAngle_degrees;
+ angleResponseTest_prev_pitchAngle = angleResponseTest_pitchAngle_radians;
+
+ // Look-up which agent should be followed
+ if (shouldFollowAnotherAgent)
+ {
+ // Only bother if the "follow_in_a_line_agentIDs" vector has a non-zero length
+ if (follow_in_a_line_agentIDs.size() > 0)
+ {
+ // Instantiate a local boolean variable for checking whether we found
+ // our own agent ID in the list
+ bool foundMyAgentID = false;
+ // Iterate through the vector of "follow_in_a_line_agentIDs"
+ for ( int i=0 ; i<follow_in_a_line_agentIDs.size() ; i++ )
+ {
+ // Check if the current entry matches the ID of this agent
+ if (follow_in_a_line_agentIDs[i] == my_agentID)
+ {
+ // Set the boolean flag that we have found out own agent ID
+ foundMyAgentID = true;
+ //ROS_INFO_STREAM("DEBUGGING: found my agent ID at index " << i );
+ // If it is the first entry, then this agent is the leader
+ if (i==0)
+ {
+ // The leader does not follow anyone else
+ shouldFollowAnotherAgent = false;
+ agentID_to_follow = 0;
+ }
+ else
+ {
+ // The agent ID to follow is the previous entry
+ agentID_to_follow = follow_in_a_line_agentIDs[i-1];
+ shouldFollowAnotherAgent = true;
+ // Convert the agent ID to a zero padded string
+ std::ostringstream str_stream;
+ str_stream << std::setw(3) << std::setfill('0') << agentID_to_follow;
+ std::string agentID_to_follow_as_string(str_stream.str());
+ // Subscribe to the "my_current_xyz_yaw_topic" of the agent ID
+ // that this agent should be following
+ ros::NodeHandle nodeHandle("~");
+ xyz_yaw_to_follow_subscriber = nodeHandle.subscribe("/dfall/agent" + agentID_to_follow_as_string + "/DemoControllerService/my_current_xyz_yaw_topic", 1, xyz_yaw_to_follow_callback);
+ //ROS_INFO_STREAM("DEBUGGING: subscribed to agent ID = " << agentID_to_follow );
+ }
+ // Break out of the for loop as the assumption is that each agent ID
+ // appears only once in the "follow_in_a_line_agentIDs" vector of ID's
+ break;
+ }
+ }
+ // Check whether we found our own agent ID
+ if (!foundMyAgentID)
+ {
+ //ROS_INFO("DEBUGGING: not following because my ID was not found");
+ // Revert to the default of not following any agent
+ shouldFollowAnotherAgent = false;
+ agentID_to_follow = 0;
+ }
+ }
+ else
+ {
+ // As the "follow_in_a_line_agentIDs" vector has length zero, revert to the
+ // default of not following any agent
+ shouldFollowAnotherAgent = false;
+ agentID_to_follow = 0;
+ //ROS_INFO("DEBUGGING: not following because line vector has length zero");
+ }
+ }
+ else
+ {
+ // Set to its default value the integer specifying the ID of the agent that will
+ // be followed by this agent
+ agentID_to_follow = 0;
+ //ROS_INFO("DEBUGGING: not following because I was asked not to follow");
+ }
+
+ if (shouldFollowAnotherAgent)
+ {
+ ROS_INFO_STREAM("[DEMO CONTROLLER] This agent (with ID " << my_agentID << ") will now follow agent ID " << agentID_to_follow );
+ }
+}
+
+
+
+
+// ----------------------------------------------------------------------------------
+// GGGG EEEEE TTTTT PPPP A RRRR A M M ( )
+// G E T P P A A R R A A MM MM ( )
+// G EEE T PPPP A A RRRR A A M M M ( )
+// G G E T P AAAAA R R AAAAA M M ( )
+// GGGG EEEEE T P A A R R A A M M ( )
+// ----------------------------------------------------------------------------------
+
+
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name)
+{
+ float val;
+ if(!nodeHandle.getParam(name, val))
+ {
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val;
+}
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length)
+{
+ if(!nodeHandle.getParam(name, val)){
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ if(val.size() != length) {
+ ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
+ }
+}
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+int getParameterInt(ros::NodeHandle& nodeHandle, std::string name)
+{
+ int val;
+ if(!nodeHandle.getParam(name, val))
+ {
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val;
+}
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+void getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length)
+{
+ if(!nodeHandle.getParam(name, val)){
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ if(val.size() != length) {
+ ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
+ }
+}
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+int getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val)
+{
+ if(!nodeHandle.getParam(name, val)){
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val.size();
+}
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name)
+{
+ bool val;
+ if(!nodeHandle.getParam(name, val))
+ {
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val;
+}
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// M M A III N N
+// MM MM A A I NN N
+// M M M A A I N N N
+// M M AAAAA I N NN
+// M M A A III N N
+// ----------------------------------------------------------------------------------
+
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+int main(int argc, char* argv[]) {
+
+ // Starting the ROS-node
+ ros::init(argc, argv, "DemoControllerService");
+
+ // Create a "ros::NodeHandle" type local variable "nodeHandle" as the current node,
+ // the "~" indcates that "self" is the node handle assigned to this variable.
+ ros::NodeHandle nodeHandle("~");
+
+ // Get the namespace of this "DemoControllerService" node
+ std::string m_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[DEMO CONTROLLER] ros::this_node::getNamespace() = " << m_namespace);
+
+ // Get the agent ID as the "ROS_NAMESPACE" this computer.
+ // NOTES:
+ // > If you look at the "Student.launch" file in the "launch" folder, you will see
+ // the following line of code:
+ // <param name="studentID" value="$(optenv ROS_NAMESPACE)" />
+ // This line of code adds a parameter named "studentID" to the "PPSClient"
+ // > Thus, to get access to this "studentID" paremeter, we first need to get a handle
+ // to the "PPSClient" node within which this controller service is nested.
+
+ // Get the handle to the "PPSClient" node
+ ros::NodeHandle PPSClient_nodeHandle(m_namespace + "/PPSClient");
+ // Get the value of the "studentID" parameter into the instance variable "my_agentID"
+ if(!PPSClient_nodeHandle.getParam("agentID", my_agentID))
+ {
+ // Throw an error if the student ID parameter could not be obtained
+ ROS_ERROR("[DEMO CONTROLLER] Failed to get agentID from PPSClient");
+ }
+
+
+ // *********************************************************************************
+ // EVERYTHING THAT RELATES TO FETCHING PARAMETERS FROM A YAML FILE
+
+
+ // EVERYTHING FOR THE CONNECTION TO THIS AGENT's OWN PARAMETER SERVICE:
+
+ // Set the class variable "namespace_to_own_agent_parameter_service" to be a the
+ // namespace string for the parameter service that is running on the machine of this
+ // agent
+ namespace_to_own_agent_parameter_service = m_namespace + "/ParameterService";
+
+ // Create a node handle to the parameter service running on this agent's machine
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(namespace_to_own_agent_parameter_service);
+
+ // Instantiate the local variable "controllerYamlReadyForFetchSubscriber" to be a
+ // "ros::Subscriber" type variable that subscribes to the "controllerYamlReadyForFetch" topic
+ // and calls the class function "yamlReadyForFetchCallback" each time a message is
+ // received on this topic and the message is passed as an input argument to the
+ // "yamlReadyForFetchCallback" class function.
+ ros::Subscriber controllerYamlReadyForFetchSubscriber_to_agent = nodeHandle_to_own_agent_parameter_service.subscribe("controllerYamlReadyForFetch", 1, yamlReadyForFetchCallback);
+
+
+ // EVERYTHING FOR THE CONNECTION THE COORDINATOR'S PARAMETER SERVICE:
+
+ // Set the class variable "nodeHandle_to_coordinator_parameter_service" to be a node handle
+ // for the parameter service that is running on the coordinate machine
+ // NOTE: the backslash here (i.e., "/") before the name of the node ("ParameterService")
+ // is very important because it specifies that the name is global
+ namespace_to_coordinator_parameter_service = "/ParameterService";
+
+ // Create a node handle to the parameter service running on the coordinator machine
+ ros::NodeHandle nodeHandle_to_coordinator = ros::NodeHandle();
+ //ros::NodeHandle nodeHandle_to_coordinator_parameter_service = ros::NodeHandle(namespace_to_own_agent_parameter_service);
+
+
+ // Instantiate the local variable "controllerYamlReadyForFetchSubscriber" to be a
+ // "ros::Subscriber" type variable that subscribes to the "controllerYamlReadyForFetch" topic
+ // and calls the class function "yamlReadyForFetchCallback" each time a message is
+ // received on this topic and the message is passed as an input argument to the
+ // "yamlReadyForFetchCallback" class function.
+ ros::Subscriber controllerYamlReadyForFetchSubscriber_to_coordinator = nodeHandle_to_coordinator.subscribe("/ParameterService/controllerYamlReadyForFetch", 1, yamlReadyForFetchCallback);
+ //ros::Subscriber controllerYamlReadyForFetchSubscriber_to_coordinator = nodeHandle_to_coordinator_parameter_service.subscribe("controllerYamlReadyForFetch", 1, yamlReadyForFetchCallback);
+
+
+ // PRINT OUT SOME INFORMATION
+
+ // Let the user know what namespaces are being used for linking to the parameter service
+ ROS_INFO_STREAM("[DEMO CONTROLLER] the namespace strings for accessing the Paramter Services are:");
+ ROS_INFO_STREAM("[DEMO CONTROLLER] namespace_to_own_agent_parameter_service = " << namespace_to_own_agent_parameter_service);
+ ROS_INFO_STREAM("[DEMO CONTROLLER] namespace_to_coordinator_parameter_service = " << namespace_to_coordinator_parameter_service);
+
+
+ // FINALLY, FETCH ANY PARAMETERS REQUIRED FROM THESE "PARAMETER SERVICES"
+
+ // Call the class function that loads the parameters for this class.
+ fetchYamlParameters(nodeHandle_to_own_agent_parameter_service);
+
+ // *********************************************************************************
+
+
+
+ // Instantiate the instance variable "debugPublisher" to be a "ros::Publisher" that
+ // advertises under the name "DebugTopic" and is a message with the structure
+ // defined in the file "DebugMsg.msg" (located in the "msg" folder).
+ debugPublisher = nodeHandle.advertise<DebugMsg>("DebugTopic", 1);
+
+ // Instantiate the local variable "setpointSubscriber" to be a "ros::Subscriber"
+ // type variable that subscribes to the "Setpoint" topic and calls the class function
+ // "setpointCallback" each time a messaged is received on this topic and the message
+ // is passed as an input argument to the "setpointCallback" class function.
+ ros::Subscriber setpointSubscriber = nodeHandle.subscribe("Setpoint", 1, setpointCallback);
+
+ // Instantiate the local variable "service" to be a "ros::ServiceServer" type
+ // variable that advertises the service called "DemoController". This service has
+ // the input-output behaviour defined in the "Controller.srv" file (located in the
+ // "srv" folder). This service, when called, is provided with the most recent
+ // measurement of the Crazyflie and is expected to respond with the control action
+ // that should be sent via the Crazyradio and requested from the Crazyflie, i.e.,
+ // this is where the "outer loop" controller function starts. When a request is made
+ // of this service the "calculateControlOutput" function is called.
+ ros::ServiceServer service = nodeHandle.advertiseService("DemoController", calculateControlOutput);
+
+ // Create a "ros::NodeHandle" type local variable "namespace_nodeHandle" that points
+ // to the name space of this node, i.e., "d_fall_pps" as specified by the line:
+ // "using namespace d_fall_pps;"
+ // in the "DEFINES" section at the top of this file.
+ ros::NodeHandle namespace_nodeHandle(ros::this_node::getNamespace());
+
+ // Instantiate the instance variable "my_current_xyz_yaw_publisher" to be a "ros::Publisher"
+ // that advertises under the name "<my_agentID>/my_current_xyz_yaw_topic" where <my_agentID>
+ // is filled in with the student ID number of this computer. The messages published will
+ // have the structure defined in the file "Setpoint.msg" (located in the "msg" folder).
+ my_current_xyz_yaw_publisher = nodeHandle.advertise<Setpoint>("my_current_xyz_yaw_topic", 1);
+
+ // Instantiate the local variable "customCommandSubscriber" to be a "ros::Subscriber"
+ // type variable that subscribes to the "StudentCustomButton" topic and calls the class
+ // function "customCommandReceivedCallback" each time a messaged is received on this topic
+ // and the message received is passed as an input argument to the callback function.
+ ros::Subscriber customCommandReceivedSubscriber = nodeHandle.subscribe("GUIButton", 1, customCommandReceivedCallback);
+
+ // Print out some information to the user.
+ ROS_INFO("[DEMO CONTROLLER] Service ready :-)");
+
+ // Enter an endless while loop to keep the node alive.
+ ros::spin();
+
+ // Return zero if the "ross::spin" is cancelled.
+ return 0;
+}
diff --git a/pps_ws/src/d_fall_pps/src/nodes/MpcControllerService.cpp b/pps_ws/src/d_fall_pps/src/nodes/MpcControllerService.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4a4978ccd7677868e38dde86cca1bab2a0704c88
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/src/nodes/MpcControllerService.cpp
@@ -0,0 +1,976 @@
+// Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat, Angel Romero, Cyrill Burgener, Marco Mueller, Philipp Friedli
+//
+// 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:
+// Place for students to implement their controller
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// III N N CCCC L U U DDDD EEEEE SSSS
+// I NN N C L U U D D E S
+// I N N N C L U U D D EEE SSS
+// I N NN C L U U D D E S
+// III N N CCCC LLLLL UUU DDDD EEEEE SSSS
+// ----------------------------------------------------------------------------------
+
+// These various headers need to be included so that this controller service can be
+// connected with the D-FaLL system.
+
+//some useful libraries
+#include <math.h>
+#include <stdlib.h>
+#include "ros/ros.h"
+#include <ros/package.h>
+
+//the generated structs from the msg-files have to be included
+#include "d_fall_pps/ViconData.h"
+#include "d_fall_pps/Setpoint.h"
+#include "d_fall_pps/ControlCommand.h"
+#include "d_fall_pps/Controller.h"
+#include "d_fall_pps/DebugMsg.h"
+#include "d_fall_pps/CustomControllerYAML.h"
+
+// Include the Parameter Service shared definitions
+#include "nodes/ParameterServiceDefinitions.h"
+
+#include <std_msgs/Int32.h>
+
+//#include <Eigen/Dense>
+
+
+
+// ----------------------------------------------------------------------------------
+// DDDD EEEEE FFFFF III N N EEEEE SSSS
+// D D E F I NN N E S
+// D D EEE FFF I N N N EEE SSS
+// D D E F I N NN E S
+// DDDD EEEEE F III N N EEEEE SSSS
+// ----------------------------------------------------------------------------------
+
+// These constants are defined to make the code more readable and adaptable.
+
+// Universal constants
+#define PI 3.1415926535
+
+// These constants define the modes that can be used for controller the Crazyflie 2.0,
+// the constants defined here need to be in agreement with those defined in the
+// firmware running on the Crazyflie 2.0.
+// The following is a short description about each mode:
+// MOTOR_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
+// command directly to each of the motors
+// RATE_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
+// command directly to each of the motors, and additionally request the
+// body frame roll, pitch, and yaw angular rates from the PID rate
+// controllers implemented in the Crazyflie 2.0 firmware.
+// ANGE_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
+// command directly to each of the motors, and additionally request the
+// body frame roll, pitch, and yaw angles from the PID attitude
+// controllers implemented in the Crazyflie 2.0 firmware.
+
+#define CF_COMMAND_TYPE_MOTOR 6
+#define CF_COMMAND_TYPE_RATE 7
+#define CF_COMMAND_TYPE_ANGLE 8
+
+#define ESTIMATOR_METHOD_FINITE_DIFFERENCE 1
+#define ESTIMATOR_METHOD_POINT_MASS_PER_DIMENSION 2 // (DEFAULT)
+#define ESTIMATOR_METHOD_QUADROTOR_MODEL_BASED 3
+
+int estimator_method = ESTIMATOR_METHOD_POINT_MASS_PER_DIMENSION;
+
+// Boolean indiciating whether the debugging ROS_INFO_STREAM should be displayed or not
+bool shouldDisplayDebugInfo = false;
+
+float estimator_frequency = 200;
+
+// These constants define the controller used for computing the response in the
+// "calculateControlOutput" function
+// The following is a short description about each mode:
+// LQR_RATE_MODE LQR controller based on the state vector:
+// [position,velocity,angles]
+//
+// LQR_ANGLE_MODE LQR controller based on the state vector:
+// [position,velocity]
+//
+#define LQR_RATE_MODE 1 // (DEFAULT)
+#define LQR_ANGLE_MODE 2
+
+// Namespacing the package
+using namespace d_fall_pps;
+
+
+// ----------------------------------------------------------------------------------
+// V V A RRRR III A BBBB L EEEEE SSSS
+// V V A A R R I A A B B L E S
+// V V A A RRRR I A A BBBB L EEE SSS
+// V V AAAAA R R I AAAAA B B L E S
+// V A A R R III A A BBBB LLLLL EEEEE SSSS
+// ----------------------------------------------------------------------------------
+
+
+// THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
+// > For the (x,y,z) position
+std::vector<float> PMKF_Ahat_row1_for_positions (2,0.0);
+std::vector<float> PMKF_Ahat_row2_for_positions (2,0.0);
+std::vector<float> PMKF_Kinf_for_positions (2,0.0);
+// > For the (roll,pitch,yaw) angles
+std::vector<float> PMKF_Ahat_row1_for_angles (2,0.0);
+std::vector<float> PMKF_Ahat_row2_for_angles (2,0.0);
+std::vector<float> PMKF_Kinf_for_angles (2,0.0);
+
+std::vector<float> gainMatrixRollRate_Nested (3,0.0);
+std::vector<float> gainMatrixPitchRate_Nested (3,0.0);
+std::vector<float> gainMatrixYawRate_Nested (3,0.0);
+
+float current_xzy_rpy_measurement[6];
+float previous_xzy_rpy_measurement[6];
+
+
+// Variables for controller
+float cf_mass; // Crazyflie mass in grams
+std::vector<float> motorPoly(3); // Coefficients of the 16-bit command to thrust conversion
+float control_frequency; // Frequency at which the controller is running
+float gravity_force; // The weight of the Crazyflie in Newtons, i.e., mg
+
+float previous_stateErrorInertial[9]; // The location error of the Crazyflie at the "previous" time step
+
+std::vector<float> setpoint{0.0,0.0,0.4,0.0}; // The setpoints for (x,y,z) position and yaw angle, in that order
+
+
+// ROS Publisher for debugging variables
+ros::Publisher debugPublisher;
+
+
+// Variable for the namespaces for the paramter services
+// > For the paramter service of this agent
+std::string namespace_to_own_agent_parameter_service;
+// > For the parameter service of the coordinator
+std::string namespace_to_coordinator_parameter_service;
+
+// The ID of this agent, i.e., the ID of this compute
+int my_agentID = 0;
+
+
+// RELEVANT NOTES ABOUT THE VARIABLES DECLARE HERE:
+// The "CrazyflieData" type used for the "request" variable is a
+// structure as defined in the file "CrazyflieData.msg" which has the following
+// properties:
+// string crazyflieName The name given to the Crazyflie in the Vicon software
+// float64 x The x position of the Crazyflie [metres]
+// float64 y The y position of the Crazyflie [metres]
+// float64 z The z position of the Crazyflie [metres]
+// float64 roll The roll component of the intrinsic Euler angles [radians]
+// float64 pitch The pitch component of the intrinsic Euler angles [radians]
+// float64 yaw The yaw component of the intrinsic Euler angles [radians]
+// float64 acquiringTime #delta t The time elapsed since the previous "CrazyflieData" was received [seconds]
+// bool occluded A boolean indicted whether the Crazyflie for visible at the time of this measurement
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// FFFFF U U N N CCCC TTTTT III OOO N N
+// F U U NN N C T I O O NN N
+// FFF U U N N N C T I O O N N N
+// F U U N NN C T I O O N NN
+// F UUU N N CCCC T III OOO N N
+//
+// PPPP RRRR OOO TTTTT OOO TTTTT Y Y PPPP EEEEE SSSS
+// P P R R O O T O O T Y Y P P E S
+// PPPP RRRR O O T O O T Y PPPP EEE SSS
+// P R R O O T O O T Y P E S
+// P R R OOO T OOO T Y P EEEEE SSSS
+// ----------------------------------------------------------------------------------
+
+// These function prototypes are not strictly required for this code to complile, but
+// adding the function prototypes here means the the functions can be written below in
+// any order. If the function prototypes are not included then the function need to
+// written below in an order that ensure each function is implemented before it is
+// called from another function, hence why the "main" function is at the bottom.
+
+// CONTROLLER COMPUTATIONS
+bool calculateControlOutput(Controller::Request &request, Controller::Response &response);
+
+// CONVERSION FROM THRUST IN NEWTONS TO 16-BIT COMMAND
+float computeMotorPolyBackward(float thrust);
+
+// SETPOINT CHANGE CALLBACK
+void setpointCallback(const Setpoint& newSetpoint);
+
+// LOAD PARAMETERS
+float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name);
+void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length);
+int getParameterInt(ros::NodeHandle& nodeHandle, std::string name);
+void getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length);
+int getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val);
+bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name);
+
+void yamlReadyForFetchCallback(const std_msgs::Int32& msg);
+void fetchYamlParameters(ros::NodeHandle& nodeHandle);
+void processFetchedParameters();
+//void customYAMLasMessageCallback(const CustomControllerYAML& newCustomControllerParameters);
+
+void angleControlledCrazyflie(float stateInertial[12], float input_angle[4], int Ts_div, Controller::Response &response);
+void perform_estimator_update_state_interial(Controller::Request &request, float (&stateInertialEstimate)[12]);
+void performEstimatorUpdate_forStateInterial_viaFiniteDifference(float (&stateInertialEstimate)[12]);
+void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter(float (&stateInertialEstimate)[12]);
+
+
+
+
+// ----------------------------------------------------------------------------------
+// FFFFF U U N N CCCC TTTTT III OOO N N
+// F U U NN N C T I O O NN N
+// FFF U U N N N C T I O O N N N
+// F U U N NN C T I O O N NN
+// F UUU N N CCCC T III OOO N N
+//
+// III M M PPPP L EEEEE M M EEEEE N N TTTTT A TTTTT III OOO N N
+// I MM MM P P L E MM MM E NN N T A A T I O O NN N
+// I M M M PPPP L EEE M M M EEE N N N T A A T I O O N N N
+// I M M P L E M M E N NN T AAAAA T I O O N NN
+// III M M P LLLLL EEEEE M M EEEEE N N T A A T III OOO N N
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// ------------------------------------------------------------------------------
+// OOO U U TTTTT EEEEE RRRR
+// O O U U T E R R
+// O O U U T EEE RRRR
+// O O U U T E R R
+// OOO UUU T EEEEE R R
+//
+// CCCC OOO N N TTTTT RRRR OOO L L OOO OOO PPPP
+// C O O NN N T R R O O L L O O O O P P
+// C O O N N N T RRRR O O L L O O O O PPPP
+// C O O N NN T R R O O L L O O O O P
+// CCCC OOO N N T R R OOO LLLLL LLLLL OOO OOO P
+// ----------------------------------------------------------------------------------
+
+// This function is the callback that is linked to the "CustomController" service that
+// is advertised in the main function. This must have arguments that match the
+// "input-output" behaviour defined in the "Controller.srv" file (located in the "srv"
+// folder)
+//
+// The arument "request" is a structure provided to this service with the following two
+// properties:
+// request.ownCrazyflie
+// This property is itself a structure of type "CrazyflieData", which is defined in the
+// file "CrazyflieData.msg", and has the following properties
+// string crazyflieName
+// float64 x The x position of the Crazyflie [metres]
+// float64 y The y position of the Crazyflie [metres]
+// float64 z The z position of the Crazyflie [metres]
+// float64 roll The roll component of the intrinsic Euler angles [radians]
+// float64 pitch The pitch component of the intrinsic Euler angles [radians]
+// float64 yaw The yaw component of the intrinsic Euler angles [radians]
+// float64 acquiringTime #delta t The time elapsed since the previous "CrazyflieData" was received [seconds]
+// bool occluded A boolean indicted whether the Crazyflie for visible at the time of this measurement
+// The values in these properties are directly the measurement taken by the Vicon
+// motion capture system of the Crazyflie that is to be controlled by this service
+//
+// request.otherCrazyflies
+// This property is an array of "CrazyflieData" structures, what allows access to the
+// Vicon measurements of other Crazyflies.
+//
+// The argument "response" is a structure that is expected to be filled in by this
+// service by this function, it has only the following property
+// response.ControlCommand
+// This property is iteself a structure of type "ControlCommand", which is defined in
+// the file "ControlCommand.msg", and has the following properties:
+// float32 roll The command sent to the Crazyflie for the body frame x-axis
+// float32 pitch The command sent to the Crazyflie for the body frame y-axis
+// float32 yaw The command sent to the Crazyflie for the body frame z-axis
+// uint16 motorCmd1 The command sent to the Crazyflie for motor 1
+// uint16 motorCmd2 The command sent to the Crazyflie for motor 1
+// uint16 motorCmd3 The command sent to the Crazyflie for motor 1
+// uint16 motorCmd4 The command sent to the Crazyflie for motor 1
+// uint8 onboardControllerType The flag sent to the Crazyflie for indicating how to implement the command
+//
+// IMPORTANT NOTES FOR "onboardControllerType" AND AXIS CONVENTIONS
+// > The allowed values for "onboardControllerType" are in the "Defines" section at the
+// top of this file, they are MOTOR_MODE, RATE_MODE, OR ANGLE_MODE.
+// > In the PPS exercise we will only use the RATE_MODE.
+// > In RATE_MODE the ".roll", ".ptich", and ".yaw" properties of "response.ControlCommand"
+// specify the angular rate in [radians/second] that will be requested from the
+// PID controllers running in the Crazyflie 2.0 firmware.
+// > In RATE_MODE the ".motorCmd1" to ".motorCmd4" properties of "response.ControlCommand"
+// are the baseline motor commands requested from the Crazyflie, with the adjustment
+// for body rates being added on top of this in the firmware (i.e., as per the code
+// of the "distribute_power" function provided in exercise sheet 2).
+// > In RATE_MODE the axis convention for the roll, pitch, and yaw body rates returned
+// in "response.ControlCommand" should use right-hand coordinate axes with x-forward
+// and z-upwards (i.e., the positive z-axis is aligned with the direction of positive
+// thrust). To assist, teh following is an ASCII art of this convention:
+//
+// ASCII ART OF THE CRAZYFLIE 2.0 LAYOUT
+//
+// > This is a top view,
+// > M1 to M4 stand for Motor 1 to Motor 4,
+// > "CW" indicates that the motor rotates Clockwise,
+// > "CCW" indicates that the motor rotates Counter-Clockwise,
+// > By right-hand axis convention, the positive z-direction points our of the screen,
+// > This being a "top view" means tha the direction of positive thrust produced
+// by the propellers is also out of the screen.
+//
+// ____ ____
+// / \ / \
+// (CW) | M4 | x | M1 | (CCW)
+// \____/\ ^ /\____/
+// \ \ | / /
+// \ \ | / /
+// \ \______ | ______/ /
+// \ | /
+// | | |
+// y <-------------o |
+// | |
+// / _______________ \
+// / / \ \
+// / / \ \
+// ____/ / \ \____
+// / \/ \/ \
+// (CCW) | M3 | | M2 | (CW)
+// \____/ \____/
+//
+//
+//
+// This function WILL NEED TO BE edited for successful completion of the PPS exercise
+
+void angleControlledCrazyflie(float stateInertial[12], float input_angle[4], int Ts_div, Controller::Response &response)
+{
+ float roll_sp = input_angle[0];
+ float pitch_sp = input_angle[1];
+ float yaw_sp = input_angle[2];
+ float ft_sp = input_angle[3];
+
+ // initialize variables that will contain w_x, w_y and w_z
+ float w_x_sp = 0;
+ float w_y_sp = 0;
+ float w_z_sp = 0;
+
+ // Create the angle error to use for the inner controller
+ float angle_error[3] = {
+ stateInertial[6] - roll_sp,
+ stateInertial[7] - pitch_sp,
+ stateInertial[8] - yaw_sp
+ };
+ // Perform the "-Kx" LQR computation for the rates and thrust:
+ for(int i = 0; i < 3; ++i)
+ {
+ // BODY FRAME Y CONTROLLER
+ w_x_sp -= gainMatrixRollRate_Nested[i] * angle_error[i];
+ // BODY FRAME X CONTROLLER
+ w_y_sp -= gainMatrixPitchRate_Nested[i] * angle_error[i];
+ // BODY FRAME Z CONTROLLER
+ w_z_sp -= gainMatrixYawRate_Nested[i] * angle_error[i];
+ }
+
+
+ // UPDATE THE "RETURN" THE VARIABLE NAMED "response"
+
+ // Put the computed rates and thrust into the "response" variable
+ // > For roll, pitch, and yaw:
+ response.controlOutput.roll = w_x_sp;
+ response.controlOutput.pitch = w_y_sp;
+ response.controlOutput.yaw = w_z_sp;
+ // > For the thrust adjustment we must add the feed-forward thrust to counter-act gravity.
+ // > NOTE: remember that the thrust is commanded per motor, so you sohuld
+ // consider whether the "thrustAdjustment" computed by your
+ // controller needed to be divided by 4 or not.
+ // > NOTE: the "gravity_force_quarter" value was already divided by 4 when
+ // it was loaded/processes from the .yaml file.
+ response.controlOutput.motorCmd1 = computeMotorPolyBackward(ft_sp/4.0);
+ response.controlOutput.motorCmd2 = computeMotorPolyBackward(ft_sp/4.0);
+ response.controlOutput.motorCmd3 = computeMotorPolyBackward(ft_sp/4.0);
+ response.controlOutput.motorCmd4 = computeMotorPolyBackward(ft_sp/4.0);
+
+ // Specify that this controller is a rate controller
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
+ response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
+
+ // Display some details (if requested)
+ if (shouldDisplayDebugInfo)
+ {
+ ROS_INFO_STREAM("thrust =" << ft_sp);
+ ROS_INFO_STREAM("rollrate =" << w_x_sp);
+ ROS_INFO_STREAM("pitchrate =" << w_y_sp);
+ ROS_INFO_STREAM("yawrate =" << w_z_sp);
+ }
+}
+
+
+bool calculateControlOutput(Controller::Request &request, Controller::Response &response)
+{
+
+ // This is the "start" of the outer loop controller, add all your control
+ // computation here, or you may find it convienient to create functions
+ // to keep you code cleaner
+
+ // Define a local array to fill in with the state error
+ float stateInertialEstimate[12];
+
+ perform_estimator_update_state_interial(request, stateInertialEstimate);
+
+ // The estimate of the state is inside stateInertialEstimate now. Next, compute the error
+ // Should we convert into body frame for our MPC controller? Let's skip it for now, YAW is going to be zero in our case
+
+ float x0_full_state[12] = {-setpoint[0], -setpoint[1], -setpoint[2], 0, 0, 0, 0, 0, 0, 0, 0, 0};
+
+ float correction_z = -0.82*(0.4 - stateInertialEstimate[2]) - 0.22 * (0 - stateInertialEstimate[5]) + cf_mass/1000*9.8;
+
+ float input_angle[4] = {-setpoint[0], 0, 0, correction_z};
+
+ // create a function that takes as input angle references, like a crazyflie entity with input angles
+ angleControlledCrazyflie(stateInertialEstimate, input_angle, 1, response);
+
+
+
+ // Return "true" to indicate that the control computation was performed successfully
+ return true;
+}
+
+
+// ------------------------------------------------------------------------------
+// EEEEE SSSS TTTTT III M M A TTTTT III OOO N N
+// E S T I MM MM A A T I O O NN N
+// EEE SSS T I M M M A A T I O O N N N
+// E S T I M M AAAAA T I O O N NN
+// EEEEE SSSS T III M M A A T III OOO N N
+// ------------------------------------------------------------------------------
+
+
+
+void perform_estimator_update_state_interial(Controller::Request &request, float (&stateInertialEstimate)[12])
+{
+
+ // PUT THE CURRENT MEASURED DATA INTO THE CLASS VARIABLE
+ // > for (x,y,z) position
+ current_xzy_rpy_measurement[0] = request.ownCrazyflie.x;
+ current_xzy_rpy_measurement[1] = request.ownCrazyflie.y;
+ current_xzy_rpy_measurement[2] = request.ownCrazyflie.z;
+ // > for (roll,pitch,yaw) angles
+ current_xzy_rpy_measurement[3] = request.ownCrazyflie.roll;
+ current_xzy_rpy_measurement[4] = request.ownCrazyflie.pitch;
+ current_xzy_rpy_measurement[5] = request.ownCrazyflie.yaw;
+
+
+ // FILLE IN THE STATE INERTIAL ESTIMATE TO BE USED FOR CONTROL
+ switch (estimator_method)
+ {
+ // Estimator based on finte differences
+ case ESTIMATOR_METHOD_FINITE_DIFFERENCE:
+ {
+ // RUN THE FINITE DIFFERENCE ESTIMATOR and fill stateEstimate with result
+ performEstimatorUpdate_forStateInterial_viaFiniteDifference(stateInertialEstimate);
+ break;
+ }
+ // Estimator based on Point Mass Kalman Filter
+ case ESTIMATOR_METHOD_POINT_MASS_PER_DIMENSION:
+ {
+ // RUN THE POINT MASS KALMAN FILTER ESTIMATOR and fill stateEstimate with result
+ performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter(stateInertialEstimate);
+ break;
+ }
+ // Handle the exception
+ default:
+ {
+ // Display that the "estimator_method" was not recognised
+ ROS_INFO_STREAM("[DEMO CONTROLLER] ERROR: in the 'calculateControlOutput' function of the 'DemoControllerService': the 'estimator_method' is not recognised.");
+ // Transfer the finite difference estimate by default and fill stateEstimate with result
+ performEstimatorUpdate_forStateInterial_viaFiniteDifference(stateInertialEstimate);
+ break;
+ }
+ }
+
+
+ // NOW THAT THE ESTIMATORS HAVE ALL BEEN RUN, PUT THE CURRENT
+ // MEASURED DATA INTO THE CLASS VARIABLE FOR THE PREVIOUS
+ // > for (x,y,z) position
+ previous_xzy_rpy_measurement[0] = current_xzy_rpy_measurement[0];
+ previous_xzy_rpy_measurement[1] = current_xzy_rpy_measurement[1];
+ previous_xzy_rpy_measurement[2] = current_xzy_rpy_measurement[2];
+ // > for (roll,pitch,yaw) angles
+ previous_xzy_rpy_measurement[3] = current_xzy_rpy_measurement[3];
+ previous_xzy_rpy_measurement[4] = current_xzy_rpy_measurement[4];
+ previous_xzy_rpy_measurement[5] = current_xzy_rpy_measurement[5];
+}
+
+void performEstimatorUpdate_forStateInterial_viaFiniteDifference(float (&stateInertialEstimate)[12])
+{
+ // PUT IN THE CURRENT MEASUREMENT DIRECTLY
+ // > for (x,y,z) position
+ stateInertialEstimate[0] = current_xzy_rpy_measurement[0];
+ stateInertialEstimate[1] = current_xzy_rpy_measurement[1];
+ stateInertialEstimate[2] = current_xzy_rpy_measurement[2];
+ // > for (roll,pitch,yaw) angles
+ stateInertialEstimate[6] = current_xzy_rpy_measurement[3];
+ stateInertialEstimate[7] = current_xzy_rpy_measurement[4];
+ stateInertialEstimate[8] = current_xzy_rpy_measurement[5];
+
+ // COMPUTE THE VELOCITIES VIA FINITE DIFFERENCE
+ // > for (x,y,z) velocities
+ stateInertialEstimate[3] = (current_xzy_rpy_measurement[0] - previous_xzy_rpy_measurement[0]) * estimator_frequency;
+ stateInertialEstimate[4] = (current_xzy_rpy_measurement[1] - previous_xzy_rpy_measurement[1]) * estimator_frequency;
+ stateInertialEstimate[5] = (current_xzy_rpy_measurement[2] - previous_xzy_rpy_measurement[2]) * estimator_frequency;
+ // > for (roll,pitch,yaw) velocities
+ stateInertialEstimate[9] = (current_xzy_rpy_measurement[3] - previous_xzy_rpy_measurement[3]) * estimator_frequency;
+ stateInertialEstimate[10] = (current_xzy_rpy_measurement[4] - previous_xzy_rpy_measurement[4]) * estimator_frequency;
+ stateInertialEstimate[11] = (current_xzy_rpy_measurement[5] - previous_xzy_rpy_measurement[5]) * estimator_frequency;
+}
+
+
+
+void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter(float (&stateInertialEstimate)[12])
+{
+ // PERFORM THE KALMAN FILTER UPDATE STEP
+ // > First take a copy of the estimator state
+ float temp_PMKFstate[12];
+ for(int i = 0; i < 12; ++i)
+ {
+ temp_PMKFstate[i] = stateInertialEstimate[i];
+ }
+ // > Now perform update for:
+ // > x position and velocity:
+ stateInertialEstimate[0] = PMKF_Ahat_row1_for_positions[0]*temp_PMKFstate[0] + PMKF_Ahat_row1_for_positions[1]*temp_PMKFstate[3] + PMKF_Kinf_for_positions[0]*current_xzy_rpy_measurement[0];
+ stateInertialEstimate[3] = PMKF_Ahat_row2_for_positions[0]*temp_PMKFstate[0] + PMKF_Ahat_row2_for_positions[1]*temp_PMKFstate[3] + PMKF_Kinf_for_positions[1]*current_xzy_rpy_measurement[0];
+ // > y position and velocity:
+ stateInertialEstimate[1] = PMKF_Ahat_row1_for_positions[0]*temp_PMKFstate[1] + PMKF_Ahat_row1_for_positions[1]*temp_PMKFstate[4] + PMKF_Kinf_for_positions[0]*current_xzy_rpy_measurement[1];
+ stateInertialEstimate[4] = PMKF_Ahat_row2_for_positions[0]*temp_PMKFstate[1] + PMKF_Ahat_row2_for_positions[1]*temp_PMKFstate[4] + PMKF_Kinf_for_positions[1]*current_xzy_rpy_measurement[1];
+ // > z position and velocity:
+ stateInertialEstimate[2] = PMKF_Ahat_row1_for_positions[0]*temp_PMKFstate[2] + PMKF_Ahat_row1_for_positions[1]*temp_PMKFstate[5] + PMKF_Kinf_for_positions[0]*current_xzy_rpy_measurement[2];
+ stateInertialEstimate[5] = PMKF_Ahat_row2_for_positions[0]*temp_PMKFstate[2] + PMKF_Ahat_row2_for_positions[1]*temp_PMKFstate[5] + PMKF_Kinf_for_positions[1]*current_xzy_rpy_measurement[2];
+
+ // > roll position and velocity:
+ stateInertialEstimate[6] = PMKF_Ahat_row1_for_angles[0]*temp_PMKFstate[6] + PMKF_Ahat_row1_for_angles[1]*temp_PMKFstate[9] + PMKF_Kinf_for_angles[0]*current_xzy_rpy_measurement[3];
+ stateInertialEstimate[9] = PMKF_Ahat_row2_for_angles[0]*temp_PMKFstate[6] + PMKF_Ahat_row2_for_angles[1]*temp_PMKFstate[9] + PMKF_Kinf_for_angles[1]*current_xzy_rpy_measurement[3];
+ // > pitch position and velocity:
+ stateInertialEstimate[7] = PMKF_Ahat_row1_for_angles[0]*temp_PMKFstate[7] + PMKF_Ahat_row1_for_angles[1]*temp_PMKFstate[10] + PMKF_Kinf_for_angles[0]*current_xzy_rpy_measurement[4];
+ stateInertialEstimate[10] = PMKF_Ahat_row2_for_angles[0]*temp_PMKFstate[7] + PMKF_Ahat_row2_for_angles[1]*temp_PMKFstate[10] + PMKF_Kinf_for_angles[1]*current_xzy_rpy_measurement[4];
+ // > yaw position and velocity:
+ stateInertialEstimate[8] = PMKF_Ahat_row1_for_angles[0]*temp_PMKFstate[8] + PMKF_Ahat_row1_for_angles[1]*temp_PMKFstate[11] + PMKF_Kinf_for_angles[0]*current_xzy_rpy_measurement[5];
+ stateInertialEstimate[11] = PMKF_Ahat_row2_for_angles[0]*temp_PMKFstate[8] + PMKF_Ahat_row2_for_angles[1]*temp_PMKFstate[11] + PMKF_Kinf_for_angles[1]*current_xzy_rpy_measurement[5];
+
+}
+
+
+
+
+
+
+// ------------------------------------------------------------------------------
+// N N EEEEE W W TTTTT OOO N N CCCC M M DDDD
+// NN N E W W T O O NN N C MM MM D D
+// N N N EEE W W T O O N N N --> C M M M D D
+// N NN E W W W T O O N NN C M M D D
+// N N EEEEE W W T OOO N N CCCC M M DDDD
+//
+// CCCC OOO N N V V EEEEE RRRR SSSS III OOO N N
+// C O O NN N V V E R R S I O O NN N
+// C O O N N N V V EEE RRRR SSS I O O N N N
+// C O O N NN V V E R R S I O O N NN
+// CCCC OOO N N V EEEEE R R SSSS III OOO N N
+// ----------------------------------------------------------------------------------
+
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+float computeMotorPolyBackward(float thrust)
+{
+ return (-motorPoly[1] + sqrt(motorPoly[1] * motorPoly[1] - 4 * motorPoly[2] * (motorPoly[0] - thrust))) / (2 * motorPoly[2]);
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// N N EEEEE W W SSSS EEEEE TTTTT PPPP OOO III N N TTTTT
+// NN N E W W S E T P P O O I NN N T
+// N N N EEE W W SSS EEE T PPPP O O I N N N T
+// N NN E W W W S E T P O O I N NN T
+// N N EEEEE W W SSSS EEEEE T P OOO III N N T
+//
+// GGG U U III CCCC A L L BBBB A CCCC K K
+// G G U U I C A A L L B B A A C K K
+// G U U I C A A L L BBBB A A C KKK
+// G G U U I C AAAAA L L B B AAAAA C K K
+// GGGG UUU III CCCC A A LLLLL LLLLL BBBB A A CCCC K K
+// ----------------------------------------------------------------------------------
+
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+void setpointCallback(const Setpoint& newSetpoint)
+{
+ setpoint[0] = newSetpoint.x;
+ setpoint[1] = newSetpoint.y;
+ setpoint[2] = newSetpoint.z;
+ setpoint[3] = newSetpoint.yaw;
+}
+
+
+
+
+// ----------------------------------------------------------------------------------
+// L OOO A DDDD
+// L O O A A D D
+// L O O A A D D
+// L O O AAAAA D D
+// LLLLL OOO A A DDDD
+//
+// PPPP A RRRR A M M EEEEE TTTTT EEEEE RRRR SSSS
+// P P A A R R A A MM MM E T E R R S
+// PPPP A A RRRR A A M M M EEE T EEE RRRR SSS
+// P AAAAA R R AAAAA M M E T E R R S
+// P A A R R A A M M EEEEE T EEEEE R R SSSS
+// ----------------------------------------------------------------------------------
+
+
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+void yamlReadyForFetchCallback(const std_msgs::Int32& msg)
+{
+ // Extract from the "msg" for which controller the and from where to fetch the YAML
+ // parameters
+ int controller_to_fetch_yaml = msg.data;
+
+ // Switch between fetching for the different controllers and from different locations
+ switch(controller_to_fetch_yaml)
+ {
+
+ // > FOR FETCHING FROM THE AGENT'S OWN PARAMETER SERVICE
+ case FETCH_YAML_MPC_CONTROLLER_FROM_OWN_AGENT:
+ {
+ // Let the user know that this message was received
+ ROS_INFO("[MPC CONTROLLER] Received the message that YAML parameters were (re-)loaded. > Now fetching the parameter values from this agent.");
+ // Create a node handle to the parameter service running on this agent's machine
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(namespace_to_own_agent_parameter_service);
+ // Call the function that fetches the parameters
+ fetchYamlParameters(nodeHandle_to_own_agent_parameter_service);
+ break;
+ }
+
+ // > FOR FETCHING FROM THE COORDINATOR'S PARAMETER SERVICE
+ case FETCH_YAML_MPC_CONTROLLER_FROM_COORDINATOR:
+ {
+ // Let the user know that this message was received
+ ROS_INFO("[MPC CONTROLLER] Received the message that YAML parameters were (re-)loaded. > Now fetching the parameter values from the coordinator.");
+ // Create a node handle to the parameter service running on the coordinator machine
+ ros::NodeHandle nodeHandle_to_coordinator_parameter_service(namespace_to_coordinator_parameter_service);
+ // Call the function that fetches the parameters
+ fetchYamlParameters(nodeHandle_to_coordinator_parameter_service);
+ break;
+ }
+
+ default:
+ {
+ // Let the user know that the command was not relevant
+ //ROS_INFO("The CustomControllerService received the message that YAML parameters were (re-)loaded");
+ //ROS_INFO("> However the parameters do not relate to this controller, hence nothing will be fetched.");
+ break;
+ }
+ }
+}
+
+
+// This function CAN BE edited for successful completion of the PPS exercise, and the
+// use of parameters fetched from the YAML file is highly recommended to make tuning of
+// your controller easier and quicker.
+void fetchYamlParameters(ros::NodeHandle& nodeHandle)
+{
+ // Here we load the parameters that are specified in the CustomController.yaml file
+
+ // Add the "CustomController" namespace to the "nodeHandle"
+ ros::NodeHandle nodeHandle_for_MpcController(nodeHandle, "MpcController");
+
+ // > The mass of the crazyflie
+ cf_mass = getParameterFloat(nodeHandle_for_MpcController , "mass");
+
+ // Display one of the YAML parameters to debug if it is working correctly
+ //ROS_INFO_STREAM("DEBUGGING: mass leaded from loacl file = " << cf_mass );
+
+ // > The frequency at which the "computeControlOutput" is being called, as determined
+ // by the frequency at which the Vicon system provides position and attitude data
+ control_frequency = getParameterFloat(nodeHandle_for_MpcController, "control_frequency");
+
+ // > The co-efficients of the quadratic conversation from 16-bit motor command to
+ // thrust force in Newtons
+ getParameterFloatVector(nodeHandle_for_MpcController, "motorPoly", motorPoly, 3);
+
+
+ // DEBUGGING: Print out one of the parameters that was loaded
+ ROS_INFO_STREAM("[MPC CONTROLLER] DEBUGGING: the fetched CustomController/mass = " << cf_mass);
+
+ getParameterFloatVector(nodeHandle_for_MpcController, "gainMatrixRollRate_Nested", gainMatrixRollRate_Nested, 3);
+ getParameterFloatVector(nodeHandle_for_MpcController, "gainMatrixPitchRate_Nested", gainMatrixPitchRate_Nested, 3);
+ getParameterFloatVector(nodeHandle_for_MpcController, "gainMatrixYawRate_Nested", gainMatrixYawRate_Nested, 3);
+
+
+ // THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
+ // > For the (x,y,z) position
+ getParameterFloatVector(nodeHandle_for_MpcController, "PMKF_Ahat_row1_for_positions", PMKF_Ahat_row1_for_positions, 2);
+ getParameterFloatVector(nodeHandle_for_MpcController, "PMKF_Ahat_row2_for_positions", PMKF_Ahat_row2_for_positions, 2);
+ getParameterFloatVector(nodeHandle_for_MpcController, "PMKF_Kinf_for_positions" , PMKF_Kinf_for_positions , 2);
+ // > For the (roll,pitch,yaw) angles
+ getParameterFloatVector(nodeHandle_for_MpcController, "PMKF_Ahat_row1_for_angles", PMKF_Ahat_row1_for_angles, 2);
+ getParameterFloatVector(nodeHandle_for_MpcController, "PMKF_Ahat_row2_for_angles", PMKF_Ahat_row2_for_angles, 2);
+ getParameterFloatVector(nodeHandle_for_MpcController, "PMKF_Kinf_for_angles" , PMKF_Kinf_for_angles , 2);
+
+
+ // Call the function that computes details an values that are needed from these
+ // parameters loaded above
+ processFetchedParameters();
+}
+
+
+// This function CAN BE edited for successful completion of the PPS exercise, and the
+// use of parameters loaded from the YAML file is highly recommended to make tuning of
+// your controller easier and quicker.
+void processFetchedParameters()
+{
+ // Compute the feed-forward force that we need to counteract gravity (i.e., mg)
+ // > in units of [Newtons]
+ gravity_force = cf_mass * 9.81/(1000*4);
+ // DEBUGGING: Print out one of the computed quantities
+ ROS_INFO_STREAM("[MPC CONTROLLER] DEBUGGING: thus the graity force = " << gravity_force);
+}
+
+
+
+// ----------------------------------------------------------------------------------
+// GGGG EEEEE TTTTT PPPP A RRRR A M M ( )
+// G E T P P A A R R A A MM MM ( )
+// G EEE T PPPP A A RRRR A A M M M ( )
+// G G E T P AAAAA R R AAAAA M M ( )
+// GGGG EEEEE T P A A R R A A M M ( )
+// ----------------------------------------------------------------------------------
+
+
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name)
+{
+ float val;
+ if(!nodeHandle.getParam(name, val))
+ {
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val;
+}
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length)
+{
+ if(!nodeHandle.getParam(name, val)){
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ if(val.size() != length) {
+ ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
+ }
+}
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+int getParameterInt(ros::NodeHandle& nodeHandle, std::string name)
+{
+ int val;
+ if(!nodeHandle.getParam(name, val))
+ {
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val;
+}
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+void getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length)
+{
+ if(!nodeHandle.getParam(name, val)){
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ if(val.size() != length) {
+ ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
+ }
+}
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+int getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val)
+{
+ if(!nodeHandle.getParam(name, val)){
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val.size();
+}
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name)
+{
+ bool val;
+ if(!nodeHandle.getParam(name, val))
+ {
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val;
+}
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// M M A III N N
+// MM MM A A I NN N
+// M M M A A I N N N
+// M M AAAAA I N NN
+// M M A A III N N
+// ----------------------------------------------------------------------------------
+
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+int main(int argc, char* argv[]) {
+
+ // Starting the ROS-node
+ ros::init(argc, argv, "MpcControllerService");
+
+ // Create a "ros::NodeHandle" type local variable "nodeHandle" as the current node,
+ // the "~" indcates that "self" is the node handle assigned to this variable.
+ ros::NodeHandle nodeHandle("~");
+
+ // Get the namespace of this "StudentControllerService" node
+ std::string m_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[MPC CONTROLLER] ros::this_node::getNamespace() = " << m_namespace);
+
+ // Get the agent ID as the "ROS_NAMESPACE" this computer.
+ // NOTES:
+ // > If you look at the "Student.launch" file in the "launch" folder, you will see
+ // the following line of code:
+ // <param name="studentID" value="$(optenv ROS_NAMESPACE)" />
+ // This line of code adds a parameter named "studentID" to the "PPSClient"
+ // > Thus, to get access to this "studentID" paremeter, we first need to get a handle
+ // to the "PPSClient" node within which this controller service is nested.
+ // Get the handle to the "PPSClient" node
+ ros::NodeHandle PPSClient_nodeHandle(m_namespace + "/PPSClient");
+ // Get the value of the "studentID" parameter into the instance variable "my_agentID"
+ if(!PPSClient_nodeHandle.getParam("agentID", my_agentID))
+ {
+ // Throw an error if the student ID parameter could not be obtained
+ ROS_ERROR("[MPC CONTROLLER] Failed to get agentID from PPSClient");
+ }
+
+
+ // *********************************************************************************
+ // EVERYTHING THAT RELATES TO FETCHING PARAMETERS FROM A YAML FILE
+
+
+ // EVERYTHING FOR THE CONNECTION TO THIS AGENT's OWN PARAMETER SERVICE:
+
+ // Set the class variable "namespace_to_own_agent_parameter_service" to be a the
+ // namespace string for the parameter service that is running on the machine of this
+ // agent
+ namespace_to_own_agent_parameter_service = m_namespace + "/ParameterService";
+
+ // Create a node handle to the parameter service running on this agent's machine
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(namespace_to_own_agent_parameter_service);
+
+ // Instantiate the local variable "controllerYamlReadyForFetchSubscriber" to be a
+ // "ros::Subscriber" type variable that subscribes to the "controllerYamlReadyForFetch" topic
+ // and calls the class function "yamlReadyForFetchCallback" each time a message is
+ // received on this topic and the message is passed as an input argument to the
+ // "yamlReadyForFetchCallback" class function.
+ ros::Subscriber controllerYamlReadyForFetchSubscriber_to_agent = nodeHandle_to_own_agent_parameter_service.subscribe("controllerYamlReadyForFetch", 1, yamlReadyForFetchCallback);
+
+
+ // EVERYTHING FOR THE CONNECTION THE COORDINATOR'S PARAMETER SERVICE:
+
+ // Set the class variable "nodeHandle_to_coordinator_parameter_service" to be a node handle
+ // for the parameter service that is running on the coordinate machine
+ // NOTE: the backslash here (i.e., "/") before the name of the node ("ParameterService")
+ // is very important because it specifies that the name is global
+ namespace_to_coordinator_parameter_service = "/ParameterService";
+
+ // Create a node handle to the parameter service running on the coordinator machine
+ ros::NodeHandle nodeHandle_to_coordinator = ros::NodeHandle();
+ //ros::NodeHandle nodeHandle_to_coordinator_parameter_service = ros::NodeHandle(namespace_to_own_agent_parameter_service);
+
+
+ // Instantiate the local variable "controllerYamlReadyForFetchSubscriber" to be a
+ // "ros::Subscriber" type variable that subscribes to the "controllerYamlReadyForFetch" topic
+ // and calls the class function "yamlReadyForFetchCallback" each time a message is
+ // received on this topic and the message is passed as an input argument to the
+ // "yamlReadyForFetchCallback" class function.
+ ros::Subscriber controllerYamlReadyForFetchSubscriber_to_coordinator = nodeHandle_to_coordinator.subscribe("/ParameterService/controllerYamlReadyForFetch", 1, yamlReadyForFetchCallback);
+ //ros::Subscriber controllerYamlReadyForFetchSubscriber_to_coordinator = nodeHandle_to_coordinator_parameter_service.subscribe("controllerYamlReadyForFetch", 1, yamlReadyForFetchCallback);
+
+
+ // PRINT OUT SOME INFORMATION
+
+ // Let the user know what namespaces are being used for linking to the parameter service
+ ROS_INFO_STREAM("[MPC CONTROLLER] The namespace string for accessing the Paramter Services are:");
+ ROS_INFO_STREAM("[MPC CONTROLLER] namespace_to_own_agent_parameter_service = " << namespace_to_own_agent_parameter_service);
+ ROS_INFO_STREAM("[MPC CONTROLLER] namespace_to_coordinator_parameter_service = " << namespace_to_coordinator_parameter_service);
+
+
+ // FINALLY, FETCH ANY PARAMETERS REQUIRED FROM THESE "PARAMETER SERVICES"
+
+ // Call the class function that loads the parameters for this class.
+ //fetchYamlParameters(nodeHandle_to_own_agent_parameter_service);
+
+ // *********************************************************************************
+
+
+
+ // Instantiate the instance variable "debugPublisher" to be a "ros::Publisher" that
+ // advertises under the name "DebugTopic" and is a message with the structure
+ // defined in the file "DebugMsg.msg" (located in the "msg" folder).
+ debugPublisher = nodeHandle.advertise<DebugMsg>("DebugTopic", 1);
+
+ // Instantiate the local variable "setpointSubscriber" to be a "ros::Subscriber"
+ // type variable that subscribes to the "Setpoint" topic and calls the class function
+ // "setpointCallback" each time a messaged is received on this topic and the message
+ // is passed as an input argument to the "setpointCallback" class function.
+ ros::Subscriber setpointSubscriber = nodeHandle.subscribe("Setpoint", 1, setpointCallback);
+
+ // Instantiate the local variable "service" to be a "ros::ServiceServer" type
+ // variable that advertises the service called "CustomController". This service has
+ // the input-output behaviour defined in the "Controller.srv" file (located in the
+ // "srv" folder). This service, when called, is provided with the most recent
+ // measurement of the Crazyflie and is expected to respond with the control action
+ // that should be sent via the Crazyradio and requested from the Crazyflie, i.e.,
+ // this is where the "outer loop" controller function starts. When a request is made
+ // of this service the "calculateControlOutput" function is called.
+ ros::ServiceServer service = nodeHandle.advertiseService("MpcController", calculateControlOutput);
+
+ // Create a "ros::NodeHandle" type local variable "namespace_nodeHandle" that points
+ // to the name space of this node, i.e., "d_fall_pps" as specified by the line:
+ // "using namespace d_fall_pps;"
+ // in the "DEFINES" section at the top of this file.
+ ros::NodeHandle namespace_nodeHandle(ros::this_node::getNamespace());
+
+ // Print out some information to the user.
+ ROS_INFO("[MPC CONTROLLER] Service ready :-)");
+
+ // Enter an endless while loop to keep the node alive.
+ ros::spin();
+
+ // Return zero if the "ross::spin" is cancelled.
+ return 0;
+}
diff --git a/pps_ws/src/d_fall_pps/src/PPSClient.cpp b/pps_ws/src/d_fall_pps/src/nodes/PPSClient.cpp
similarity index 72%
rename from pps_ws/src/d_fall_pps/src/PPSClient.cpp
rename to pps_ws/src/d_fall_pps/src/nodes/PPSClient.cpp
index bfadefa53ca4d7fd770caa4aa25bbcf0ba2b9eb5..bebe2e1251ee4b29263b3733aa9807f563798d5d 100755
--- a/pps_ws/src/d_fall_pps/src/PPSClient.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/PPSClient.cpp
@@ -33,235 +33,13 @@
-// ----------------------------------------------------------------------------------
-// III N N CCCC L U U DDDD EEEEE SSSS
-// I NN N C L U U D D E S
-// I N N N C L U U D D EEE SSS
-// I N NN C L U U D D E S
-// III N N CCCC LLLLL UUU DDDD EEEEE SSSS
-// ----------------------------------------------------------------------------------
-
-#include "ros/ros.h"
-#include <stdlib.h>
-#include <std_msgs/String.h>
-#include <ros/package.h>
-
-#include "d_fall_pps/Controller.h"
-#include "d_fall_pps/CMQuery.h"
-
-#include "d_fall_pps/ViconData.h"
-#include "d_fall_pps/CrazyflieData.h"
-#include "d_fall_pps/ControlCommand.h"
-#include "d_fall_pps/CrazyflieContext.h"
-#include "d_fall_pps/Setpoint.h"
-#include "std_msgs/Int32.h"
-#include "std_msgs/Float32.h"
-
-#include "d_fall_pps/ControlCommand.h"
-
-// Need for having a ROS "bag" to store data for post-analysis
-//#include <rosbag/bag.h>
-
-
-
-
-// ----------------------------------------------------------------------------------
-// DDDD EEEEE FFFFF III N N EEEEE SSSS
-// D D E F I NN N E S
-// D D EEE FFF I N N N EEE SSS
-// D D E F I N NN E S
-// DDDD EEEEE F III N N EEEEE SSSS
-// ----------------------------------------------------------------------------------
-
-// Types PPS firmware
-#define TYPE_PPS_MOTORS 6
-#define TYPE_PPS_RATE 7
-#define TYPE_PPS_ANGLE 8
-
-// Types of controllers being used:
-#define SAFE_CONTROLLER 0
-#define CUSTOM_CONTROLLER 1
-
-// The constants that "command" changes in the
-// operation state of this agent
-#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
-
-// Commands for CrazyRadio
-#define CMD_RECONNECT 0
-#define CMD_DISCONNECT 1
-
-
-// CrazyRadio states:
-#define CONNECTED 0
-#define CONNECTING 1
-#define DISCONNECTED 2
-
-// For which controller parameters to load
-#define FETCH_YAML_SAFE_CONTROLLER_AGENT 1
-#define FETCH_YAML_CUSTOM_CONTROLLER_AGENT 2
-#define FETCH_YAML_SAFE_CONTROLLER_COORDINATOR 3
-#define FETCH_YAML_CUSTOM_CONTROLLER_COORDINATOR 4
-
-
-// Parameters for take off and landing. Eventually will go in YAML file
-//#define TAKE_OFF_OFFSET 1 //in meters
-//#define LANDING_DISTANCE 0.15 //in meters
-//#define DURATION_TAKE_OFF 3 // seconds
-//#define DURATION_LANDING 3 // seconds
-
-// Universal constants
-#define PI 3.141592653589
-
-// Namespacing the package
-using namespace d_fall_pps;
-
-
-
-
-
-// ----------------------------------------------------------------------------------
-// V V A RRRR III A BBBB L EEEEE SSSS
-// V V A A R R I A A B B L E S
-// V V A A RRRR I A A BBBB L EEE SSS
-// V V AAAAA R R I AAAAA B B L E S
-// V A A R R III A A BBBB LLLLL EEEEE SSSS
-// ----------------------------------------------------------------------------------
-
-//studentID, gives namespace and identifier in CentralManagerService
-int studentID;
+// INCLUDE THE HEADER
+#include "nodes/PPSClient.h"
-//the safe controller specified in the ClientConfig.yaml, is considered stable
-ros::ServiceClient safeController;
-//the custom controller specified in the ClientConfig.yaml, is considered potentially unstable
-ros::ServiceClient customController;
-
-//values for safteyCheck
-bool strictSafety;
-float angleMargin;
-
-// battery threshold
-float m_battery_threshold_while_flying;
-float m_battery_threshold_while_motors_off;
-
-
-// battery values
-
-int m_battery_state;
-float m_battery_voltage;
-
-Setpoint controller_setpoint;
-
-// variables for linear trayectory
-Setpoint current_safe_setpoint;
-double distance;
-double unit_vector[3];
-bool was_in_threshold = false;
-double distance_threshold; //to be loaded from yaml
-
-
-ros::ServiceClient centralManager;
-ros::Publisher controlCommandPublisher;
-
-// communicate with safeControllerService, setpoint, etc...
-ros::Publisher safeControllerServiceSetpointPublisher;
-
-// publisher for flying state
-ros::Publisher flyingStatePublisher;
-
-// publisher for battery state
-ros::Publisher batteryStatePublisher;
-
-// publisher to send commands to itself.
-ros::Publisher commandPublisher;
-
-// communication with crazyRadio node. Connect and disconnect
-ros::Publisher crazyRadioCommandPublisher;
-
-
-// Variable for the namespaces for the paramter services
-// > For the paramter service of this agent
-std::string namespace_to_own_agent_parameter_service;
-// > For the parameter service of the coordinator
-std::string namespace_to_coordinator_parameter_service;
-
-
-// variables for the states:
-int flying_state;
-bool changed_state_flag;
-
-// variable for crazyradio status
-int crazyradio_status;
-
-//describes the area of the crazyflie and other parameters
-CrazyflieContext context;
-
-//wheter to use safe of custom controller
-int instant_controller; //variable for the instant controller, e.g., we use safe controller for taking off and landing even if custom controller is enabled. This variable WILL change automatically
-
-// controller used:
-int controller_used;
-
-ros::Publisher controllerUsedPublisher;
-
-std::string ros_namespace;
-
-float take_off_distance;
-float landing_distance;
-float duration_take_off;
-float duration_landing;
-
-bool finished_take_off = false;
-bool finished_land = false;
-
-ros::Timer timer_takeoff;
-ros::Timer timer_land;
-
-// A ROS "bag" to store data for post-analysis
-//rosbag::Bag bag;
-// ----------------------------------------------------------------------------------
-// FFFFF U U N N CCCC TTTTT III OOO N N
-// F U U NN N C T I O O NN N
-// FFF U U N N N C T I O O N N N
-// F U U N NN C T I O O N NN
-// F UUU N N CCCC T III OOO N N
-//
-// PPPP RRRR OOO TTTTT OOO TTTTT Y Y PPPP EEEEE SSSS
-// P P R R O O T O O T Y Y P P E S
-// PPPP RRRR O O T O O T Y PPPP EEE SSS
-// P R R O O T O O T Y P E S
-// P R R OOO T OOO T Y P EEEEE SSSS
-// ----------------------------------------------------------------------------------
-
-
-// > For the LOAD PARAMETERS
-void yamlReadyForFetchCallback(const std_msgs::Int32& msg);
-void fetchYamlParametersForSafeController(ros::NodeHandle& nodeHandle);
-void fetchClientConfigParameters(ros::NodeHandle& nodeHandle);
-
-
-
-void crazyRadioCommandAllAgentsCallback(const std_msgs::Int32& msg);
-
-
-
// ----------------------------------------------------------------------------------
// FFFFF U U N N CCCC TTTTT III OOO N N
// F U U NN N C T I O O NN N
@@ -276,96 +54,26 @@ void crazyRadioCommandAllAgentsCallback(const std_msgs::Int32& msg);
// III M M P LLLLL EEEEE M M EEEEE N N T A A T III OOO N N
// ----------------------------------------------------------------------------------
-void loadSafeController() {
- ros::NodeHandle nodeHandle("~");
-
- std::string safeControllerName;
- if(!nodeHandle.getParam("safeController", safeControllerName)) {
- ROS_ERROR("Failed to get safe controller name");
- return;
- }
- ros::service::waitForService(safeControllerName);
- safeController = ros::service::createClient<Controller>(safeControllerName, true);
- ROS_INFO_STREAM("loaded safe controller: " << safeController.getService());
-}
-void loadCustomController()
-{
- ros::NodeHandle nodeHandle("~");
- std::string customControllerName;
- if(!nodeHandle.getParam("customController", customControllerName))
- {
- ROS_ERROR("Failed to get custom controller name");
- return;
- }
- customController = ros::service::createClient<Controller>(customControllerName, true);
- ROS_INFO_STREAM("loaded custom controller " << customControllerName);
-}
-void sendMessageUsingController(int controller)
-{
- // send a message in topic for the studentGUI to read it
- std_msgs::Int32 controller_used_msg;
- controller_used_msg.data = controller;
- controllerUsedPublisher.publish(controller_used_msg);
-}
-void setInstantController(int controller) //for right now, temporal use
-{
- instant_controller = controller;
- sendMessageUsingController(controller);
- switch(controller)
- {
- case SAFE_CONTROLLER:
- loadSafeController();
- break;
- case CUSTOM_CONTROLLER:
- loadCustomController();
- break;
- default:
- break;
- }
-}
-
-int getInstantController()
-{
- return instant_controller;
-}
-
-void setControllerUsed(int controller) //for permanent configs
-{
- controller_used = controller;
-
- if(flying_state == STATE_MOTORS_OFF || flying_state == STATE_FLYING)
- {
- setInstantController(controller); //if motors OFF or STATE FLYING, transparent, change is instant
- }
-}
-
-int getControllerUsed()
-{
- return controller_used;
-}
-
-
-
-//checks if crazyflie is within allowed area and if custom controller returns no data
+//checks if crazyflie is within allowed area and if demo controller returns no data
bool safetyCheck(CrazyflieData data, ControlCommand controlCommand) {
//position check
if((data.x < context.localArea.xmin) or (data.x > context.localArea.xmax)) {
- ROS_INFO_STREAM("x safety failed");
+ ROS_INFO_STREAM("[PPS CLIENT] x safety failed");
return false;
}
if((data.y < context.localArea.ymin) or (data.y > context.localArea.ymax)) {
- ROS_INFO_STREAM("y safety failed");
+ ROS_INFO_STREAM("[PPS CLIENT] y safety failed");
return false;
}
if((data.z < context.localArea.zmin) or (data.z > context.localArea.zmax)) {
- ROS_INFO_STREAM("z safety failed");
+ ROS_INFO_STREAM("[PPS CLIENT] z safety failed");
return false;
}
@@ -374,11 +82,11 @@ bool safetyCheck(CrazyflieData data, ControlCommand controlCommand) {
//the angleMargin is a value in the range (0,1). The closer to 1, the closer to 90 deg are the roll and pitch angles allowed to become before the safeController takes over
if(strictSafety){
if((data.roll > PI/2*angleMargin) or (data.roll < -PI/2*angleMargin)) {
- ROS_INFO_STREAM("roll too big.");
+ ROS_INFO_STREAM("[PPS CLIENT] roll too big.");
return false;
}
if((data.pitch > PI/2*angleMargin) or (data.pitch < -PI/2*angleMargin)) {
- ROS_INFO_STREAM("pitch too big.");
+ ROS_INFO_STREAM("[PPS CLIENT] pitch too big.");
return false;
}
}
@@ -429,11 +137,11 @@ void takeOffCF(CrazyflieData& current_local_coordinates) //local because the set
// setpoint_msg.yaw = current_local_coordinates.yaw; //previous one
setpoint_msg.yaw = 0.0;
safeControllerServiceSetpointPublisher.publish(setpoint_msg);
- ROS_INFO("Take OFF:");
- ROS_INFO("------Current coordinates:");
- ROS_INFO("X: %f, Y: %f, Z: %f", current_local_coordinates.x, current_local_coordinates.y, current_local_coordinates.z);
- ROS_INFO("------New coordinates:");
- ROS_INFO("X: %f, Y: %f, Z: %f", setpoint_msg.x, setpoint_msg.y, setpoint_msg.z);
+ ROS_INFO("[PPS CLIENT] Take OFF:");
+ ROS_INFO("[PPS CLIENT] ------Current coordinates:");
+ ROS_INFO("[PPS CLIENT] X: %f, Y: %f, Z: %f", current_local_coordinates.x, current_local_coordinates.y, current_local_coordinates.z);
+ ROS_INFO("[PPS CLIENT] ------New coordinates:");
+ ROS_INFO("[PPS CLIENT] X: %f, Y: %f, Z: %f", setpoint_msg.x, setpoint_msg.y, setpoint_msg.z);
// now, use safe controller to go to that setpoint
loadSafeController();
@@ -464,12 +172,12 @@ void changeFlyingStateTo(int new_state)
{
if(crazyradio_status == CONNECTED)
{
- ROS_INFO("Change state to: %d", new_state);
+ ROS_INFO("[PPS CLIENT] Change state to: %d", new_state);
flying_state = new_state;
}
else
{
- ROS_INFO("Disconnected and trying to change state. State goes to MOTORS OFF");
+ ROS_INFO("[PPS CLIENT] Disconnected and trying to change state. State goes to MOTORS OFF");
flying_state = STATE_MOTORS_OFF;
}
@@ -498,7 +206,8 @@ void goToControllerSetpoint()
//is called when new data from Vicon arrives
-void viconCallback(const ViconData& viconData) {
+void viconCallback(const ViconData& viconData)
+{
for(std::vector<CrazyflieData>::const_iterator it = viconData.crazyflies.begin(); it != viconData.crazyflies.end(); ++it) {
CrazyflieData global = *it;
@@ -517,7 +226,7 @@ void viconCallback(const ViconData& viconData) {
if(changed_state_flag) // stuff that will be run only once when changing state
{
changed_state_flag = false;
- ROS_INFO("STATE_MOTORS_OFF");
+ ROS_INFO("[PPS CLIENT] STATE_MOTORS_OFF");
}
break;
case STATE_TAKE_OFF: //we need to move up from where we are now.
@@ -526,7 +235,7 @@ void viconCallback(const ViconData& viconData) {
changed_state_flag = false;
takeOffCF(local);
finished_take_off = false;
- ROS_INFO("STATE_TAKE_OFF");
+ ROS_INFO("[PPS CLIENT] STATE_TAKE_OFF");
timer_takeoff = nodeHandle.createTimer(ros::Duration(duration_take_off), takeOffTimerCallback, true);
}
if(finished_take_off)
@@ -542,7 +251,7 @@ void viconCallback(const ViconData& viconData) {
changed_state_flag = false;
// need to change setpoint to the controller one
goToControllerSetpoint();
- ROS_INFO("STATE_FLYING");
+ ROS_INFO("[PPS CLIENT] STATE_FLYING");
}
break;
case STATE_LAND:
@@ -551,7 +260,7 @@ void viconCallback(const ViconData& viconData) {
changed_state_flag = false;
landCF(local);
finished_land = false;
- ROS_INFO("STATE_LAND");
+ ROS_INFO("[PPS CLIENT] STATE_LAND");
timer_takeoff = nodeHandle.createTimer(ros::Duration(duration_landing), landTimerCallback, true);
}
if(finished_land)
@@ -570,23 +279,54 @@ void viconCallback(const ViconData& viconData) {
{
if(!global.occluded) //if it is not occluded, then proceed to compute the controller output.
{
- if(getInstantController() == CUSTOM_CONTROLLER && flying_state == STATE_FLYING) // only enter in custom controller if we are not using safe controller AND the flying state is FLYING
+ // Only call an "non-safe" controller if:
+ // 1) we are not currently using safe controller, AND
+ // 2) the flying state is FLYING
+ if( (getInstantController() != SAFE_CONTROLLER) && (flying_state == STATE_FLYING) )
{
- bool success = customController.call(controllerCall);
+ // Initialise a local boolean success variable
+ bool success = false;
+
+ switch(getInstantController())
+ {
+ case DEMO_CONTROLLER:
+ success = demoController.call(controllerCall);
+ break;
+ case STUDENT_CONTROLLER:
+ success = studentController.call(controllerCall);
+ break;
+ case MPC_CONTROLLER:
+ success = mpcController.call(controllerCall);
+ break;
+ case REMOTE_CONTROLLER:
+ success = remoteController.call(controllerCall);
+ break;
+ case TUNING_CONTROLLER:
+ success = tuningController.call(controllerCall);
+ break;
+ default:
+ ROS_ERROR("[PPS CLIENT] the current controller was not recognised");
+ break;
+ }
+
+ // Ensure success and enforce safety
if(!success)
{
- ROS_ERROR("Failed to call custom controller, switching to safe controller");
- ROS_ERROR_STREAM("custom controller status: valid: " << customController.isValid() << ", exists: " << customController.exists());
- ROS_ERROR_STREAM("custom controller name: " << customController.getService());
+ ROS_ERROR("[PPS CLIENT] Failed to call a 'non-safe' controller, switching to safe controller");
+ //ROS_ERROR_STREAM("[PPS CLIENT] 'non-safe' controller status: valid: " << demoController.isValid() << ", exists: " << demoController.exists());
+ //ROS_ERROR_STREAM("[PPS CLIENT] 'non-safe' controller name: " << demoController.getService());
setInstantController(SAFE_CONTROLLER);
}
else if(!safetyCheck(global, controllerCall.response.controlOutput))
{
setInstantController(SAFE_CONTROLLER);
- ROS_INFO_STREAM("safety check failed, switching to safe controller");
+ ROS_INFO_STREAM("[PPS CLIENT] safety check failed, switching to safe controller");
}
+
+
}
- else //SAFE_CONTROLLER and state is different from flying
+ // SAFE_CONTROLLER and state is different from flying
+ else
{
calculateDistanceToCurrentSafeSetpoint(local); // update distance, it also updates the unit vector
// ROS_INFO_STREAM("distance: " << distance);
@@ -619,7 +359,7 @@ void viconCallback(const ViconData& viconData) {
bool success = safeController.call(controllerCall);
if(!success) {
- ROS_ERROR_STREAM("Failed to call safe controller, valid: " << safeController.isValid() << ", exists: " << safeController.exists());
+ ROS_ERROR_STREAM("[PPS CLIENT] Failed to call safe controller, valid: " << safeController.isValid() << ", exists: " << safeController.exists());
}
}
@@ -634,7 +374,7 @@ void viconCallback(const ViconData& viconData) {
else
{
ControlCommand zeroOutput = ControlCommand(); //everything set to zero
- zeroOutput.onboardControllerType = TYPE_PPS_MOTORS; //set to motor_mode
+ zeroOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS; //set to motor_mode
controlCommandPublisher.publish(zeroOutput);
// Putting data into the ROS "bag" for post-analysis
@@ -650,8 +390,8 @@ void viconCallback(const ViconData& viconData) {
void loadCrazyflieContext() {
CMQuery contextCall;
- contextCall.request.studentID = studentID;
- ROS_INFO_STREAM("StudentID:" << studentID);
+ contextCall.request.studentID = agentID;
+ ROS_INFO_STREAM("[PPS CLIENT] AgentID:" << agentID);
CrazyflieContext new_context;
@@ -659,7 +399,7 @@ void loadCrazyflieContext() {
if(centralManager.call(contextCall)) {
new_context = contextCall.response.crazyflieContext;
- ROS_INFO_STREAM("CrazyflieContext:\n" << new_context);
+ ROS_INFO_STREAM("[PPS CLIENT] CrazyflieContext:\n" << new_context);
if((context.crazyflieName != "") && (new_context.crazyflieName != context.crazyflieName)) //linked crazyflie name changed and it was not empty before
{
@@ -671,7 +411,7 @@ void loadCrazyflieContext() {
// msg.data = CMD_CRAZYFLY_MOTORS_OFF;
// commandPublisher.publish(msg);
- ROS_INFO("CF is now different for this student. Disconnect and turn it off");
+ ROS_INFO("[PPS CLIENT] CF is now different for this student. Disconnect and turn it off");
std_msgs::Int32 msg;
msg.data = CMD_DISCONNECT;
@@ -685,7 +425,7 @@ void loadCrazyflieContext() {
}
else
{
- ROS_ERROR("Failed to load context. Waiting for next Save in DB by teacher");
+ ROS_ERROR("[PPS CLIENT] Failed to load context. Waiting for next Save in DB by teacher");
}
}
@@ -696,15 +436,35 @@ void commandCallback(const std_msgs::Int32& commandMsg) {
switch(cmd) {
case CMD_USE_SAFE_CONTROLLER:
- ROS_INFO("USE_SAFE_CONTROLLER Command received");
+ ROS_INFO("[PPS CLIENT] USE_SAFE_CONTROLLER Command received");
setControllerUsed(SAFE_CONTROLLER);
break;
- case CMD_USE_CUSTOM_CONTROLLER:
- ROS_INFO("USE_CUSTOM_CONTROLLER Command received");
- setControllerUsed(CUSTOM_CONTROLLER);
+ case CMD_USE_DEMO_CONTROLLER:
+ ROS_INFO("[PPS CLIENT] USE_DEMO_CONTROLLER Command received");
+ setControllerUsed(DEMO_CONTROLLER);
break;
+ case CMD_USE_STUDENT_CONTROLLER:
+ ROS_INFO("[PPS CLIENT] USE_STUDENT_CONTROLLER Command received");
+ setControllerUsed(STUDENT_CONTROLLER);
+ break;
+
+ case CMD_USE_MPC_CONTROLLER:
+ ROS_INFO("[PPS CLIENT] USE_MPC_CONTROLLER Command received");
+ setControllerUsed(MPC_CONTROLLER);
+ break;
+
+ case CMD_USE_REMOTE_CONTROLLER:
+ ROS_INFO("[PPS CLIENT] USE_REMOTE_CONTROLLER Command received");
+ setControllerUsed(REMOTE_CONTROLLER);
+ break;
+
+ case CMD_USE_TUNING_CONTROLLER:
+ ROS_INFO("[PPS CLIENT] USE_TUNING_CONTROLLER Command received");
+ setControllerUsed(TUNING_CONTROLLER);
+ break;
+
case CMD_CRAZYFLY_TAKE_OFF:
if(flying_state == STATE_MOTORS_OFF)
{
@@ -723,7 +483,7 @@ void commandCallback(const std_msgs::Int32& commandMsg) {
break;
default:
- ROS_ERROR_STREAM("unexpected command number: " << cmd);
+ ROS_ERROR_STREAM("[PPS CLIENT] unexpected command number: " << cmd);
break;
}
}
@@ -793,10 +553,10 @@ void yamlReadyForFetchCallback(const std_msgs::Int32& msg)
{
// > FOR FETCHING FROM THE AGENT'S OWN PARAMETER SERVICE
- case FETCH_YAML_SAFE_CONTROLLER_AGENT:
+ case FETCH_YAML_SAFE_CONTROLLER_FROM_OWN_AGENT:
{
// Let the user know that this message was received
- ROS_INFO("The PPSClient received the message that YAML parameters were (re-)loaded for the Safe Controller. > Now fetching the parameter values from this machine.");
+ ROS_INFO("[PPS CLIENT] Received the message that YAML parameters were (re-)loaded for the Safe Controller. > Now fetching the parameter values from this machine.");
// Create a node handle to the parameter service running on this agent's machine
ros::NodeHandle nodeHandle_to_own_agent_parameter_service(namespace_to_own_agent_parameter_service);
// Call the function that fetches the parameters
@@ -805,11 +565,11 @@ void yamlReadyForFetchCallback(const std_msgs::Int32& msg)
}
// > FOR FETCHING FROM THE COORDINATOR'S PARAMETER SERVICE
- case FETCH_YAML_SAFE_CONTROLLER_COORDINATOR:
+ case FETCH_YAML_SAFE_CONTROLLER_FROM_COORDINATOR:
{
// Let the user know that this message was received
// > and also from where the paramters are being fetched
- ROS_INFO("The PPSClient received the message that YAML parameters were (re-)loaded for the Safe Controller. > Now fetching the parameter values from the coordinator.");
+ ROS_INFO("[PPS CLIENT] Received the message that YAML parameters were (re-)loaded for the Safe Controller. > Now fetching the parameter values from the coordinator.");
// Create a node handle to the parameter service running on the coordinator machine
ros::NodeHandle nodeHandle_to_coordinator_parameter_service(namespace_to_coordinator_parameter_service);
// Call the function that fetches the parameters
@@ -832,32 +592,32 @@ void fetchYamlParametersForSafeController(ros::NodeHandle& nodeHandle)
{
// Here we load the parameters that are specified in the SafeController.yaml file
- // Add the "CustomController" namespace to the "nodeHandle"
+ // Add the "SafeController" namespace to the "nodeHandle"
ros::NodeHandle nodeHandle_for_safeController(nodeHandle, "SafeController");
if(!nodeHandle_for_safeController.getParam("takeOffDistance", take_off_distance))
{
- ROS_ERROR("Failed to get takeOffDistance");
+ ROS_ERROR("[PPS CLIENT] Failed to get takeOffDistance");
}
if(!nodeHandle_for_safeController.getParam("landingDistance", landing_distance))
{
- ROS_ERROR("Failed to get landing_distance");
+ ROS_ERROR("[PPS CLIENT] Failed to get landing_distance");
}
if(!nodeHandle_for_safeController.getParam("durationTakeOff", duration_take_off))
{
- ROS_ERROR("Failed to get duration_take_off");
+ ROS_ERROR("[PPS CLIENT] Failed to get duration_take_off");
}
if(!nodeHandle_for_safeController.getParam("durationLanding", duration_landing))
{
- ROS_ERROR("Failed to get duration_landing");
+ ROS_ERROR("[PPS CLIENT] Failed to get duration_landing");
}
if(!nodeHandle_for_safeController.getParam("distanceThreshold", distance_threshold))
{
- ROS_ERROR("Failed to get distance_threshold");
+ ROS_ERROR("[PPS CLIENT] Failed to get distance_threshold");
}
}
@@ -865,23 +625,23 @@ void fetchYamlParametersForSafeController(ros::NodeHandle& nodeHandle)
// > Load the paramters from the Client Config YAML file
void fetchClientConfigParameters(ros::NodeHandle& nodeHandle)
{
- if(!nodeHandle.getParam("studentID", studentID)) {
- ROS_ERROR("Failed to get studentID");
+ if(!nodeHandle.getParam("agentID", agentID)) {
+ ROS_ERROR("[PPS CLIENT] Failed to get agentID");
}
if(!nodeHandle.getParam("strictSafety", strictSafety)) {
- ROS_ERROR("Failed to get strictSafety param");
+ ROS_ERROR("[PPS CLIENT] Failed to get strictSafety param");
return;
}
if(!nodeHandle.getParam("angleMargin", angleMargin)) {
- ROS_ERROR("Failed to get angleMargin param");
+ ROS_ERROR("[PPS CLIENT] Failed to get angleMargin param");
return;
}
if(!nodeHandle.getParam("battery_threshold_while_flying", m_battery_threshold_while_flying)) {
- ROS_ERROR("Failed to get battery_threshold_while_flying param");
+ ROS_ERROR("[PPS CLIENT] Failed to get battery_threshold_while_flying param");
return;
}
if(!nodeHandle.getParam("battery_threshold_while_motors_off", m_battery_threshold_while_motors_off)) {
- ROS_ERROR("Failed to get battery_threshold_while_motors_off param");
+ ROS_ERROR("[PPS CLIENT] Failed to get battery_threshold_while_motors_off param");
return;
}
}
@@ -893,6 +653,8 @@ void fetchClientConfigParameters(ros::NodeHandle& nodeHandle)
+
+
void crazyRadioCommandAllAgentsCallback(const std_msgs::Int32& msg)
{
// The "msg" received can be directly published on the "crazyRadioCommandPublisher"
@@ -906,6 +668,12 @@ void crazyRadioCommandAllAgentsCallback(const std_msgs::Int32& msg)
+
+
+
+
+
+
int getBatteryState()
{
return m_battery_state;
@@ -918,16 +686,16 @@ void setBatteryStateTo(int new_battery_state)
{
case BATTERY_STATE_NORMAL:
m_battery_state = BATTERY_STATE_NORMAL;
- ROS_INFO("changed battery state to normal");
+ ROS_INFO("[PPS CLIENT] changed battery state to normal");
break;
case BATTERY_STATE_LOW:
m_battery_state = BATTERY_STATE_LOW;
- ROS_INFO("changed battery state to low");
+ ROS_INFO("[PPS CLIENT] changed battery state to low");
if(flying_state != STATE_MOTORS_OFF)
changeFlyingStateTo(STATE_LAND);
break;
default:
- ROS_INFO("Unknown battery state command, set to normal");
+ ROS_INFO("[PPS CLIENT] Unknown battery state command, set to normal");
m_battery_state = BATTERY_STATE_NORMAL;
break;
}
@@ -975,7 +743,7 @@ void CFBatteryCallback(const std_msgs::Float32& msg)
{
if(getBatteryState() != BATTERY_STATE_LOW)
setBatteryStateTo(BATTERY_STATE_LOW);
- ROS_INFO("low level battery triggered");
+ ROS_INFO("[PPS CLIENT] low level battery triggered");
}
else //maybe add hysteresis somewhere here?
{
@@ -985,6 +753,173 @@ void CFBatteryCallback(const std_msgs::Float32& msg)
}
+
+
+
+
+
+
+
+
+
+
+
+
+
+void loadSafeController() {
+ ros::NodeHandle nodeHandle("~");
+
+ std::string safeControllerName;
+ if(!nodeHandle.getParam("safeController", safeControllerName)) {
+ ROS_ERROR("[PPS CLIENT] Failed to get safe controller name");
+ return;
+ }
+
+ ros::service::waitForService(safeControllerName);
+ safeController = ros::service::createClient<Controller>(safeControllerName, true);
+ ROS_INFO_STREAM("[PPS CLIENT] loaded safe controller: " << safeController.getService());
+}
+
+void loadDemoController()
+{
+ ros::NodeHandle nodeHandle("~");
+
+ std::string demoControllerName;
+ if(!nodeHandle.getParam("demoController", demoControllerName))
+ {
+ ROS_ERROR("[PPS CLIENT] Failed to get demo controller name");
+ return;
+ }
+
+ demoController = ros::service::createClient<Controller>(demoControllerName, true);
+ ROS_INFO_STREAM("[PPS CLIENT] Loaded demo controller " << demoController.getService());
+}
+
+void loadStudentController()
+{
+ ros::NodeHandle nodeHandle("~");
+
+ std::string studentControllerName;
+ if(!nodeHandle.getParam("studentController", studentControllerName))
+ {
+ ROS_ERROR("[PPS CLIENT] Failed to get student controller name");
+ return;
+ }
+
+ studentController = ros::service::createClient<Controller>(studentControllerName, true);
+ ROS_INFO_STREAM("[PPS CLIENT] Loaded student controller " << studentController.getService());
+}
+
+void loadMpcController()
+{
+ ros::NodeHandle nodeHandle("~");
+
+ std::string mpcControllerName;
+ if(!nodeHandle.getParam("mpcController", mpcControllerName))
+ {
+ ROS_ERROR("[PPS CLIENT] Failed to get mpc controller name");
+ return;
+ }
+
+ mpcController = ros::service::createClient<Controller>(mpcControllerName, true);
+ ROS_INFO_STREAM("[PPS CLIENT] Loaded mpc controller " << mpcController.getService());
+}
+
+void loadRemoteController()
+{
+ ros::NodeHandle nodeHandle("~");
+
+ std::string remoteControllerName;
+ if(!nodeHandle.getParam("remoteController", remoteControllerName))
+ {
+ ROS_ERROR("[PPS CLIENT] Failed to get remote controller name");
+ return;
+ }
+
+ remoteController = ros::service::createClient<Controller>(remoteControllerName, true);
+ ROS_INFO_STREAM("[PPS CLIENT] Loaded remote controller " << remoteController.getService());
+}
+
+void loadTuningController()
+{
+ ros::NodeHandle nodeHandle("~");
+
+ std::string tuningControllerName;
+ if(!nodeHandle.getParam("tuningController", tuningControllerName))
+ {
+ ROS_ERROR("[PPS CLIENT] Failed to get tuning controller name");
+ return;
+ }
+
+ tuningController = ros::service::createClient<Controller>(tuningControllerName, true);
+ ROS_INFO_STREAM("[PPS CLIENT] Loaded tuning controller " << tuningController.getService());
+}
+
+void sendMessageUsingController(int controller)
+{
+ // send a message in topic for the studentGUI to read it
+ std_msgs::Int32 controller_used_msg;
+ controller_used_msg.data = controller;
+ controllerUsedPublisher.publish(controller_used_msg);
+}
+
+void setInstantController(int controller) //for right now, temporal use
+{
+ instant_controller = controller;
+ sendMessageUsingController(controller);
+ switch(controller)
+ {
+ case SAFE_CONTROLLER:
+ loadSafeController();
+ break;
+ case DEMO_CONTROLLER:
+ loadDemoController();
+ break;
+ case STUDENT_CONTROLLER:
+ loadStudentController();
+ break;
+ case MPC_CONTROLLER:
+ loadMpcController();
+ break;
+ case REMOTE_CONTROLLER:
+ loadRemoteController();
+ break;
+ case TUNING_CONTROLLER:
+ loadTuningController();
+ break;
+ default:
+ break;
+ }
+}
+
+int getInstantController()
+{
+ return instant_controller;
+}
+
+void setControllerUsed(int controller) //for permanent configs
+{
+ controller_used = controller;
+
+ if(flying_state == STATE_MOTORS_OFF || flying_state == STATE_FLYING)
+ {
+ setInstantController(controller); //if motors OFF or STATE FLYING, transparent, change is instant
+ }
+}
+
+int getControllerUsed()
+{
+ return controller_used;
+}
+
+
+
+
+
+
+
+
+
// ----------------------------------------------------------------------------------
// M M A III N N
// MM MM A A I NN N
@@ -1061,7 +996,7 @@ int main(int argc, char* argv[])
if(!nodeHandle_for_safeController.getParam("defaultSetpoint", default_setpoint))
{
- ROS_ERROR_STREAM("The PPSClient could not find parameter 'defaultSetpoint', as called from main(...)");
+ ROS_ERROR_STREAM("[PPS CLIENT] Could not find parameter 'defaultSetpoint', as called from main(...)");
}
// Copy the default setpoint into the class variable
@@ -1076,7 +1011,7 @@ int main(int argc, char* argv[])
//keeps 100 messages because otherwise ViconDataPublisher would override the data immediately
ros::Subscriber viconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 100, viconCallback);
- ROS_INFO_STREAM("successfully subscribed to ViconData");
+ ROS_INFO_STREAM("[PPS CLIENT] successfully subscribed to ViconData");
//ros::Publishers to advertise the control output
controlCommandPublisher = nodeHandle.advertise <ControlCommand>("ControlCommand", 1);
diff --git a/pps_ws/src/d_fall_pps/src/ParameterService.cpp b/pps_ws/src/d_fall_pps/src/nodes/ParameterService.cpp
old mode 100755
new mode 100644
similarity index 54%
rename from pps_ws/src/d_fall_pps/src/ParameterService.cpp
rename to pps_ws/src/d_fall_pps/src/nodes/ParameterService.cpp
index cf9c361e14e6fc22ef9ff49c17b7cde5a0071981..2fd671074cab9aeb1183dafb56b120be14c1529d
--- a/pps_ws/src/d_fall_pps/src/ParameterService.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/ParameterService.cpp
@@ -33,100 +33,10 @@
-// ----------------------------------------------------------------------------------
-// III N N CCCC L U U DDDD EEEEE SSSS
-// I NN N C L U U D D E S
-// I N N N C L U U D D EEE SSS
-// I N NN C L U U D D E S
-// III N N CCCC LLLLL UUU DDDD EEEEE SSSS
-// ----------------------------------------------------------------------------------
-
-#include <stdlib.h>
-#include <string>
-
-#include <ros/ros.h>
-#include <ros/package.h>
-#include <ros/network.h>
-#include "std_msgs/Int32.h"
-//#include "std_msgs/Float32.h"
-//#include <std_msgs/String.h>
-
-#include "d_fall_pps/Controller.h"
-
-
-// ----------------------------------------------------------------------------------
-// DDDD EEEEE FFFFF III N N EEEEE SSSS
-// D D E F I NN N E S
-// D D EEE FFF I N N N EEE SSS
-// D D E F I N NN E S
-// DDDD EEEEE F III N N EEEEE SSSS
-// ----------------------------------------------------------------------------------
-
-
-// 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
-
-#define FETCH_YAML_SAFE_CONTROLLER_FROM_OWN_AGENT 1
-#define FETCH_YAML_CUSTOM_CONTROLLER_FROM_OWN_AGENT 2
-#define FETCH_YAML_SAFE_CONTROLLER_FROM_COORDINATOR 3
-#define FETCH_YAML_CUSTOM_CONTROLLER_FROM_COORDINATOR 4
-
-#define TYPE_INVALID -1
-#define TYPE_COORDINATOR 1
-#define TYPE_AGENT 2
-
-
-// Namespacing the package
-using namespace d_fall_pps;
-//using namespace std;
-
-
-
-
-// ----------------------------------------------------------------------------------
-// V V A RRRR III A BBBB L EEEEE SSSS
-// V V A A R R I A A B B L E S
-// V V A A RRRR I A A BBBB L EEE SSS
-// V V AAAAA R R I AAAAA B B L E S
-// V A A R R III A A BBBB LLLLL EEEEE SSSS
-// ----------------------------------------------------------------------------------
-
-// The type of this node, i.e., agent or a coordinator, specified as a parameter in the
-// "*.launch" file
-int my_type = 0;
-
-// The ID of this agent, i.e., the ID of this computer in the case that this computer is
-// and agent
-int my_agentID = 0;
-
-// Publisher that notifies the relevant nodes when the YAML paramters have been loaded
-// from file into ram/cache, and hence are ready to be fetched
-ros::Publisher controllerYamlReadyForFetchPublihser;
+// INCLUDE THE HEADER
+#include "nodes/ParameterService.h"
-std::string m_ros_namespace;
-
-ros::Subscriber requestLoadControllerYamlSubscriber_agent_to_self;
-
-
-// ----------------------------------------------------------------------------------
-// FFFFF U U N N CCCC TTTTT III OOO N N
-// F U U NN N C T I O O NN N
-// FFF U U N N N C T I O O N N N
-// F U U N NN C T I O O N NN
-// F UUU N N CCCC T III OOO N N
-//
-// PPPP RRRR OOO TTTTT OOO TTTTT Y Y PPPP EEEEE SSSS
-// P P R R O O T O O T Y Y P P E S
-// PPPP RRRR O O T O O T Y PPPP EEE SSS
-// P R R O O T O O T Y P E S
-// P R R OOO T OOO T Y P EEEEE SSSS
-// ----------------------------------------------------------------------------------
-
-void requestLoadControllerYamlCallback(const std_msgs::Int32& msg);
@@ -171,7 +81,7 @@ void requestLoadControllerYamlCallback(const std_msgs::Int32& msg)
// parameters should be loaded
int controller_to_load_yaml = msg.data;
- ROS_INFO_STREAM("The Parameter Service node received the message to load YAML parameters from file into cache");
+ ROS_INFO_STREAM("[PARAMETER SERVICE] Received the message to load YAML parameters from file into cache");
// Instantiate a local varaible to confirm that something can actually be loaded
@@ -185,32 +95,78 @@ void requestLoadControllerYamlCallback(const std_msgs::Int32& msg)
std::string d_fall_pps_path = ros::package::getPath("d_fall_pps");
// Switch between loading for the different controllers
- if ( (controller_to_load_yaml==LOAD_YAML_SAFE_CONTROLLER_COORDINATOR) && (my_type==TYPE_COORDINATOR) )
+ // ----------------------------------------
+ // FOR THE SAFE CONTROLLER
+ if (
+ ((controller_to_load_yaml==LOAD_YAML_SAFE_CONTROLLER_COORDINATOR) && (my_type==TYPE_COORDINATOR))
+ ||
+ ((controller_to_load_yaml==LOAD_YAML_SAFE_CONTROLLER_AGENT) && (my_type==TYPE_AGENT))
+ )
{
// Re-load the parameters of the safe controller:
- cmd = "rosparam load " + d_fall_pps_path + "/param/SafeController.yaml " + m_ros_namespace + "/SafeController";
+ cmd = "rosparam load " + d_fall_pps_path + "/param/SafeController.yaml " + m_base_namespace + "/SafeController";
}
- else if ( (controller_to_load_yaml==LOAD_YAML_SAFE_CONTROLLER_AGENT) && (my_type==TYPE_AGENT) )
+ // ----------------------------------------
+ // FOR THE DEMO CONTROLLER
+ else if (
+ ((controller_to_load_yaml==LOAD_YAML_DEMO_CONTROLLER_COORDINATOR) && (my_type==TYPE_COORDINATOR))
+ ||
+ ((controller_to_load_yaml==LOAD_YAML_DEMO_CONTROLLER_AGENT) && (my_type==TYPE_AGENT))
+ )
{
- // Re-load the parameters of the safe controller:
- cmd = "rosparam load " + d_fall_pps_path + "/param/SafeController.yaml " + m_ros_namespace + "/SafeController";
+ // Re-load the parameters of the demo controller:
+ cmd = "rosparam load " + d_fall_pps_path + "/param/DemoController.yaml " + m_base_namespace + "/DemoController";
}
- else if ( (controller_to_load_yaml==LOAD_YAML_CUSTOM_CONTROLLER_COORDINATOR) && (my_type==TYPE_COORDINATOR) )
+ // ----------------------------------------
+ // FOR THE STUDENT CONTROLLER
+ else if (
+ ((controller_to_load_yaml==LOAD_YAML_STUDENT_CONTROLLER_COORDINATOR) && (my_type==TYPE_COORDINATOR))
+ ||
+ ((controller_to_load_yaml==LOAD_YAML_STUDENT_CONTROLLER_AGENT) && (my_type==TYPE_AGENT))
+ )
{
- // Re-load the parameters of the custom controller:
- cmd = "rosparam load " + d_fall_pps_path + "/param/CustomController.yaml " + m_ros_namespace + "/CustomController";
+ // Re-load the parameters of the demo controller:
+ cmd = "rosparam load " + d_fall_pps_path + "/param/StudentController.yaml " + m_base_namespace + "/StudentController";
}
- else if ( (controller_to_load_yaml==LOAD_YAML_CUSTOM_CONTROLLER_AGENT) && (my_type==TYPE_AGENT) )
+ // ----------------------------------------
+ // FOR THE MPC CONTROLLER
+ else if (
+ ((controller_to_load_yaml==LOAD_YAML_MPC_CONTROLLER_COORDINATOR) && (my_type==TYPE_COORDINATOR))
+ ||
+ ((controller_to_load_yaml==LOAD_YAML_MPC_CONTROLLER_AGENT) && (my_type==TYPE_AGENT))
+ )
{
- // Re-load the parameters of the custom controller:
- cmd = "rosparam load " + d_fall_pps_path + "/param/CustomController.yaml " + m_ros_namespace + "/CustomController";
+ // Re-load the parameters of the demo controller:
+ cmd = "rosparam load " + d_fall_pps_path + "/param/MpcController.yaml " + m_base_namespace + "/MpcController";
+ }
+ // ----------------------------------------
+ // FOR THE REMOTE CONTROLLER
+ else if (
+ ((controller_to_load_yaml==LOAD_YAML_REMOTE_CONTROLLER_COORDINATOR) && (my_type==TYPE_COORDINATOR))
+ ||
+ ((controller_to_load_yaml==LOAD_YAML_REMOTE_CONTROLLER_AGENT) && (my_type==TYPE_AGENT))
+ )
+ {
+ // Re-load the parameters of the demo controller:
+ cmd = "rosparam load " + d_fall_pps_path + "/param/RemoteController.yaml " + m_base_namespace + "/RemoteController";
+ }
+ // ----------------------------------------
+ // FOR THE TUNING CONTROLLER
+ else if (
+ ((controller_to_load_yaml==LOAD_YAML_TUNING_CONTROLLER_COORDINATOR) && (my_type==TYPE_COORDINATOR))
+ ||
+ ((controller_to_load_yaml==LOAD_YAML_TUNING_CONTROLLER_AGENT) && (my_type==TYPE_AGENT))
+ )
+ {
+ // Re-load the parameters of the demo controller:
+ cmd = "rosparam load " + d_fall_pps_path + "/param/TuningController.yaml " + m_base_namespace + "/TuningController";
}
else
{
// Let the user know that the command was not recognised
- ROS_INFO_STREAM("> Nothing to load for this parameter service with.");
- ROS_INFO_STREAM("> The message received has 'controller_to_load_yaml' = " << controller_to_load_yaml);
- ROS_INFO_STREAM("> And the type of this Parameter Service is 'my_type' = " << my_type);
+ ROS_INFO_STREAM("[PARAMETER SERVICE] > Nothing to load for this parameter service with.");
+ ROS_INFO_STREAM("[PARAMETER SERVICE] > The message received has 'controller_to_load_yaml' = " << controller_to_load_yaml);
+ ROS_INFO_STREAM("[PARAMETER SERVICE] > And the type of this Parameter Service is 'my_type' = " << my_type);
// Set the boolean that prevents the fetch message from being sent
isValidToAttemptLoad = false;
}
@@ -221,7 +177,7 @@ void requestLoadControllerYamlCallback(const std_msgs::Int32& msg)
if (isValidToAttemptLoad)
{
// Let the user know what is about to happen
- ROS_INFO_STREAM("> The following path will be used for locating the .yaml file: " << d_fall_pps_path << " The comand line string sent to the 'system' is: " << cmd );
+ ROS_INFO_STREAM("[PARAMETER SERVICE] > The following path will be used for locating the .yaml file: " << d_fall_pps_path << " The comand line string sent to the 'system' is: " << cmd );
// Re-load the parameters by pass the command line string via a "system" call
// > i.e., this replicates pasting this string in a new terminal window and pressing enter
@@ -237,32 +193,34 @@ void requestLoadControllerYamlCallback(const std_msgs::Int32& msg)
// Instantiate a local variable for the fetch message
std_msgs::Int32 fetch_msg;
// Fill in the data of the fetch message
- switch(controller_to_load_yaml)
- {
- case (LOAD_YAML_SAFE_CONTROLLER_COORDINATOR):
- fetch_msg.data = FETCH_YAML_SAFE_CONTROLLER_FROM_COORDINATOR;
- break;
- case (LOAD_YAML_CUSTOM_CONTROLLER_COORDINATOR):
- fetch_msg.data = FETCH_YAML_CUSTOM_CONTROLLER_FROM_COORDINATOR;
- break;
- case (LOAD_YAML_SAFE_CONTROLLER_AGENT):
- fetch_msg.data = FETCH_YAML_SAFE_CONTROLLER_FROM_OWN_AGENT;
- break;
- case (LOAD_YAML_CUSTOM_CONTROLLER_AGENT):
- fetch_msg.data = FETCH_YAML_CUSTOM_CONTROLLER_FROM_OWN_AGENT;
- break;
- default:
- // Let the user know that the command was not recognised
- ROS_INFO("Unknown 'controller to load yaml' command, thus a 'ready to fetch' message will NOT be published.");
- // Set the boolean that prevents the fetch message from being sent
- isReadyForFetch = false;
- break;
- }
+ fetch_msg.data = controller_to_load_yaml;
+ // Fill in the data of the fetch message
+ // switch(controller_to_load_yaml)
+ // {
+ // case (LOAD_YAML_SAFE_CONTROLLER_COORDINATOR):
+ // fetch_msg.data = FETCH_YAML_SAFE_CONTROLLER_FROM_COORDINATOR;
+ // break;
+ // case (LOAD_YAML_DEMO_CONTROLLER_COORDINATOR):
+ // fetch_msg.data = FETCH_YAML_DEMO_CONTROLLER_FROM_COORDINATOR;
+ // break;
+ // case (LOAD_YAML_SAFE_CONTROLLER_AGENT):
+ // fetch_msg.data = FETCH_YAML_SAFE_CONTROLLER_FROM_OWN_AGENT;
+ // break;
+ // case (LOAD_YAML_DEMO_CONTROLLER_AGENT):
+ // fetch_msg.data = FETCH_YAML_DEMO_CONTROLLER_FROM_OWN_AGENT;
+ // break;
+ // default:
+ // // Let the user know that the command was not recognised
+ // ROS_INFO("Unknown 'controller to load yaml' command, thus a 'ready to fetch' message will NOT be published.");
+ // // Set the boolean that prevents the fetch message from being sent
+ // isReadyForFetch = false;
+ // break;
+ // }
// Send a message that the YAML parameter have been loaded and hence are
// ready to be fetched (i.e., using getparam())
if (isReadyForFetch)
{
- controllerYamlReadyForFetchPublihser.publish(fetch_msg);
+ controllerYamlReadyForFetchPublisher.publish(fetch_msg);
}
}
}
@@ -270,6 +228,46 @@ void requestLoadControllerYamlCallback(const std_msgs::Int32& msg)
+bool loadYamlFiles(LoadYamlFiles::Request &request, LoadYamlFiles::Response &response)
+{
+
+ std::string yamlFileName_toLoad = request.yamlFileNames[0];
+
+ ros::NodeHandle nodeHandle("~");
+
+ std::string yamlFileNamesParamters_basenamespace = "YamlFileNames/dictionary";
+
+ std::string paramterName = yamlFileNamesParamters_basenamespace + "/" + yamlFileName_toLoad;
+
+ std::string yamlFileName_from_dictionary;
+
+ if(!nodeHandle.getParam(paramterName, yamlFileName_from_dictionary))
+ {
+ ROS_ERROR_STREAM("[PARAMETER SERVICE] Missing parameter: '" << paramterName << "'");
+ return false;
+ }
+
+ // Instantiate a local variable for the command string that will be passed to the "system":
+ std::string cmd;
+
+ // Get the abolute path to "d_fall_pps":
+ std::string d_fall_pps_path = ros::package::getPath("d_fall_pps");
+
+ // Construct the system command string for (re-)loading the parameters:
+ cmd = "rosparam load " + d_fall_pps_path + "/param" + "/" + yamlFileName_from_dictionary + ".yaml " + m_base_namespace + "/" + yamlFileName_from_dictionary;
+
+ // Let the user know what is about to happen
+ ROS_INFO_STREAM("[PARAMETER SERVICE] The following path will be used for locating the .yaml file: " << d_fall_pps_path << " The comand line string sent to the 'system' is: " << cmd );
+
+ system(cmd.c_str());
+
+ // Pause breifly to ensure that the yaml file is fully loaded
+ ros::Duration(0.5).sleep();
+
+ return true;
+}
+
+
@@ -290,13 +288,18 @@ int main(int argc, char* argv[])
// the "~" indcates that "self" is the node handle assigned to this variable.
ros::NodeHandle nodeHandle("~");
+ // Get the namespace of this "ParameterService" node
+ std::string m_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[PARAMETER SERVICE] ros::this_node::getNamespace() = " << m_namespace);
+
+
// Get the value of the "type" parameter into a local string variable
std::string type_string;
if(!nodeHandle.getParam("type", type_string))
{
// Throw an error if the agent ID parameter could not be obtained
- ROS_ERROR("Failed to get type from ParameterService");
+ ROS_ERROR("[PARAMETER SERVICE] Failed to get type from ParameterService");
}
// Set the "my_type" instance variable based on this string loaded
@@ -312,7 +315,7 @@ int main(int argc, char* argv[])
{
// Set "my_type" to the value indicating that it is invlid
my_type = TYPE_INVALID;
- ROS_ERROR("The retrieve type parameter was no recognised.");
+ ROS_ERROR("[PARAMETER SERVICE] The 'type' parameter retrieved was not recognised.");
}
@@ -320,13 +323,13 @@ int main(int argc, char* argv[])
if(!nodeHandle.getParam("agentID", my_agentID))
{
// Throw an error if the agent ID parameter could not be obtained
- ROS_ERROR("Failed to get agentID from ParameterService");
+ ROS_ERROR("[PARAMETER SERVICE] Failed to get agentID from ParameterService");
}
// Publisher that notifies the relevant nodes when the YAML paramters have been loaded
// from file into ram/cache, and hence are ready to be fetched
- controllerYamlReadyForFetchPublihser = nodeHandle.advertise<std_msgs::Int32>("controllerYamlReadyForFetch", 1);
+ controllerYamlReadyForFetchPublisher = nodeHandle.advertise<std_msgs::Int32>("controllerYamlReadyForFetch", 1);
// Construct the string to the namespace of this Paramater Service
@@ -334,9 +337,10 @@ int main(int argc, char* argv[])
{
case TYPE_AGENT:
{
- //m_ros_namespace = ros::this_node::getNamespace();
- m_ros_namespace = "/" + std::to_string(my_agentID) + '/' + "ParameterService";
- ROS_INFO_STREAM("This Paramter Sercice will load .yaml file parameters into the 'base' namespace: " << m_ros_namespace);
+ //m_base_namespace = ros::this_node::getNamespace();
+ //m_base_namespace = "/agent" + my_agentID + '/' + "ParameterService";
+ m_base_namespace = m_namespace + '/' + "ParameterService";
+ ROS_INFO_STREAM("[PARAMETER SERVICE] .yaml file parameters will be loaded into the 'base' namespace: " << m_base_namespace);
break;
}
@@ -344,15 +348,16 @@ int main(int argc, char* argv[])
// > The master GUI
case TYPE_COORDINATOR:
{
- //m_ros_namespace = ros::this_node::getNamespace();
- m_ros_namespace = "/ParameterService";
- ROS_INFO_STREAM("This Paramter Sercice will load .yaml file parameters into the 'base' namespace: " << m_ros_namespace);
+ //m_base_namespace = ros::this_node::getNamespace();
+ //m_base_namespace = "/ParameterService";
+ m_base_namespace = m_namespace + '/' + "ParameterService";
+ ROS_INFO_STREAM("[PARAMETER SERVICE] .yaml file parameters will be loaded into the 'base' namespace: " << m_base_namespace);
break;
}
default:
{
- ROS_ERROR("The 'my_type' type parameter was not recognised.");
+ ROS_ERROR("[PARAMETER SERVICE] The 'my_type' type parameter was not recognised.");
break;
}
}
@@ -390,7 +395,7 @@ int main(int argc, char* argv[])
requestLoadControllerYamlSubscriber_agent_to_coordinator = nh_coordinator_for_this_agent.subscribe("/my_GUI/requestLoadControllerYaml", 1, requestLoadControllerYamlCallback);
// Inform the user what was subscribed to:
- ROS_INFO_STREAM("This Parameter Service has subscribed to 'requestLoadControllerYaml' messages from both the 'my_GUI' and the 'PPSClient'");
+ ROS_INFO_STREAM("[PARAMETER SERVICE] Subscribed to 'requestLoadControllerYaml' messages from both the 'my_GUI' and the 'PPSClient'");
break;
}
@@ -405,19 +410,29 @@ int main(int argc, char* argv[])
requestLoadControllerYamlSubscriber_coordinator_to_self = nh_coordinator_for_this_coordinator.subscribe("/my_GUI/requestLoadControllerYaml", 1, requestLoadControllerYamlCallback);
// Inform the user what was subscribed to:
- ROS_INFO_STREAM("This Parameter Service has subscribed to 'requestLoadControllerYaml' messages from 'my_GUI'");
+ ROS_INFO_STREAM("[PARAMETER SERVICE] Subscribed to 'requestLoadControllerYaml' messages from 'my_GUI'");
break;
}
default:
{
- ROS_ERROR("The 'my_type' type parameter was not recognised.");
+ ROS_ERROR("[PARAMETER SERVICE] The 'my_type' type parameter was not recognised.");
break;
}
}
- ROS_INFO("CentralManagerService ready");
+ // Advertise the service for loading Yaml Files
+ ros::ServiceServer service = nodeHandle.advertiseService("LoadYamlFiles", loadYamlFiles);
+
+
+
+ // LOAD THE LIST OF YAML FILE NAMES
+
+
+
+
+ ROS_INFO("[PARAMETER SERVICE] Service ready :-)");
ros::spin();
return 0;
diff --git a/pps_ws/src/d_fall_pps/src/nodes/RemoteControllerService.cpp b/pps_ws/src/d_fall_pps/src/nodes/RemoteControllerService.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d7fa4a2d19872f21da4187ffe81fdb3758f30864
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/src/nodes/RemoteControllerService.cpp
@@ -0,0 +1,1658 @@
+// Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat
+//
+// 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:
+// Place for students to implement their controller
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// INCLUDE THE HEADER
+#include "nodes/RemoteControllerService.h"
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// FFFFF U U N N CCCC TTTTT III OOO N N
+// F U U NN N C T I O O NN N
+// FFF U U N N N C T I O O N N N
+// F U U N NN C T I O O N NN
+// F UUU N N CCCC T III OOO N N
+//
+// III M M PPPP L EEEEE M M EEEEE N N TTTTT A TTTTT III OOO N N
+// I MM MM P P L E MM MM E NN N T A A T I O O NN N
+// I M M M PPPP L EEE M M M EEE N N N T A A T I O O N N N
+// I M M P L E M M E N NN T AAAAA T I O O N NN
+// III M M P LLLLL EEEEE M M EEEEE N N T A A T III OOO N N
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// ------------------------------------------------------------------------------
+// OOO U U TTTTT EEEEE RRRR
+// O O U U T E R R
+// O O U U T EEE RRRR
+// O O U U T E R R
+// OOO UUU T EEEEE R R
+//
+// CCCC OOO N N TTTTT RRRR OOO L L OOO OOO PPPP
+// C O O NN N T R R O O L L O O O O P P
+// C O O N N N T RRRR O O L L O O O O PPPP
+// C O O N NN T R R O O L L O O O O P
+// CCCC OOO N N T R R OOO LLLLL LLLLL OOO OOO P
+// ----------------------------------------------------------------------------------
+
+// This function is the callback that is linked to the "CustomController" service that
+// is advertised in the main function. This must have arguments that match the
+// "input-output" behaviour defined in the "Controller.srv" file (located in the "srv"
+// folder)
+//
+// The arument "request" is a structure provided to this service with the following two
+// properties:
+// request.ownCrazyflie
+// This property is itself a structure of type "CrazyflieData", which is defined in the
+// file "CrazyflieData.msg", and has the following properties
+// string crazyflieName
+// float64 x The x position of the Crazyflie [metres]
+// float64 y The y position of the Crazyflie [metres]
+// float64 z The z position of the Crazyflie [metres]
+// float64 roll The roll component of the intrinsic Euler angles [radians]
+// float64 pitch The pitch component of the intrinsic Euler angles [radians]
+// float64 yaw The yaw component of the intrinsic Euler angles [radians]
+// float64 acquiringTime #delta t The time elapsed since the previous "CrazyflieData" was received [seconds]
+// bool occluded A boolean indicted whether the Crazyflie for visible at the time of this measurement
+// The values in these properties are directly the measurement taken by the Vicon
+// motion capture system of the Crazyflie that is to be controlled by this service
+//
+// request.otherCrazyflies
+// This property is an array of "CrazyflieData" structures, what allows access to the
+// Vicon measurements of other Crazyflies.
+//
+// The argument "response" is a structure that is expected to be filled in by this
+// service by this function, it has only the following property
+// response.ControlCommand
+// This property is iteself a structure of type "ControlCommand", which is defined in
+// the file "ControlCommand.msg", and has the following properties:
+// float32 roll The command sent to the Crazyflie for the body frame x-axis
+// float32 pitch The command sent to the Crazyflie for the body frame y-axis
+// float32 yaw The command sent to the Crazyflie for the body frame z-axis
+// uint16 motorCmd1 The command sent to the Crazyflie for motor 1
+// uint16 motorCmd2 The command sent to the Crazyflie for motor 1
+// uint16 motorCmd3 The command sent to the Crazyflie for motor 1
+// uint16 motorCmd4 The command sent to the Crazyflie for motor 1
+// uint8 onboardControllerType The flag sent to the Crazyflie for indicating how to implement the command
+//
+// IMPORTANT NOTES FOR "onboardControllerType" AND AXIS CONVENTIONS
+// > The allowed values for "onboardControllerType" are in the "Defines" section at the
+// top of this file, they are MOTOR_MODE, RATE_MODE, OR ANGLE_MODE.
+// > In the PPS exercise we will only use the RATE_MODE.
+// > In RATE_MODE the ".roll", ".ptich", and ".yaw" properties of "response.ControlCommand"
+// specify the angular rate in [radians/second] that will be requested from the
+// PID controllers running in the Crazyflie 2.0 firmware.
+// > In RATE_MODE the ".motorCmd1" to ".motorCmd4" properties of "response.ControlCommand"
+// are the baseline motor commands requested from the Crazyflie, with the adjustment
+// for body rates being added on top of this in the firmware (i.e., as per the code
+// of the "distribute_power" function provided in exercise sheet 2).
+// > In RATE_MODE the axis convention for the roll, pitch, and yaw body rates returned
+// in "response.ControlCommand" should use right-hand coordinate axes with x-forward
+// and z-upwards (i.e., the positive z-axis is aligned with the direction of positive
+// thrust). To assist, teh following is an ASCII art of this convention:
+//
+// ASCII ART OF THE CRAZYFLIE 2.0 LAYOUT
+//
+// > This is a top view,
+// > M1 to M4 stand for Motor 1 to Motor 4,
+// > "CW" indicates that the motor rotates Clockwise,
+// > "CCW" indicates that the motor rotates Counter-Clockwise,
+// > By right-hand axis convention, the positive z-direction points our of the screen,
+// > This being a "top view" means tha the direction of positive thrust produced
+// by the propellers is also out of the screen.
+//
+// ____ ____
+// / \ / \
+// (CW) | M4 | x | M1 | (CCW)
+// \____/\ ^ /\____/
+// \ \ | / /
+// \ \ | / /
+// \ \______ | ______/ /
+// \ | /
+// | | |
+// y <-------------o |
+// | |
+// / _______________ \
+// / / \ \
+// / / \ \
+// ____/ / \ \____
+// / \/ \/ \
+// (CCW) | M3 | | M2 | (CW)
+// \____/ \____/
+//
+//
+//
+// This function WILL NEED TO BE edited for successful completion of the PPS exercise
+
+
+
+bool calculateControlOutput(Controller::Request &request, Controller::Response &response)
+{
+
+ // THIS IS THE START OF THE "OUTER" CONTROL LOOP
+ // > i.e., this is the control loop run on this laptop
+ // > this function is called at the frequency specified
+ // > this function performs all estimation and control
+
+
+ // PERFORM THE ESTIMATOR UPDATE FOR THE INTERIAL FRAME STATE
+ // > After this function is complete the class variable
+ // "current_stateInertialEstimate" is updated and ready
+ // to be used for subsequent controller copmutations
+ performEstimatorUpdate_forStateInterial(request);
+
+
+
+
+ if (isActive_remoteControlMode)
+ {
+ // CARRY OUT THE CONTROLLER COMPUTATIONS
+ calculateControlOutput_forRemoteControl(current_stateInertialEstimate,request,response);
+
+ }
+ else
+ {
+ // CONVERT THE CURRENT INERTIAL FRAME STATE ESTIMATE, INTO
+ // THE BODY FRAME ERROR REQUIRED BY THE CONTROLLER
+ // > Define a local array to fill in with the body frame error
+ float current_bodyFrameError[12];
+ // > Call the function to perform the conversion
+ convert_stateInertial_to_bodyFrameError(current_stateInertialEstimate,setpoint,current_bodyFrameError);
+
+
+
+ // CARRY OUT THE CONTROLLER COMPUTATIONS
+ calculateControlOutput_viaLQR(current_bodyFrameError,request,response);
+ }
+
+
+ // PUBLISH THE DEBUG MESSAGE (if required)
+ if (shouldPublishDebugMessage)
+ {
+ construct_and_publish_debug_message(request,response);
+ }
+
+ // Return "true" to indicate that the control computation was performed successfully
+ return true;
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// RRRR EEEEE M M OOO TTTTT EEEEE
+// R R E MM MM O O T E
+// RRRR EEE M M M O O T EEE
+// R R E M M O O T E
+// R R EEEEE M M OOO T EEEEE
+//
+// CCCC OOO N N TTTTT RRRR OOO L
+// C O O NN N T R R O O L
+// C O O N N N T RRRR O O L
+// C O O N NN T R R O O L
+// CCCC OOO N N T R R OOO LLLLL
+// ----------------------------------------------------------------------------------
+
+
+void calculateControlOutput_forRemoteControl(float stateInertial[12], Controller::Request &request, Controller::Response &response)
+{
+
+ // PERFORM THE INNER "u=Kx" LQR CONTROLLER COMPUTATION
+ // Instantiate the local variables for the following:
+ // > body frame roll rate,
+ // > body frame pitch rate,
+ // > body frame yaw rate,
+ // > total thrust adjustment
+ // These will be requested from the Crazyflie's on-baord "inner-loop" controller
+ float rollRate_forResponse = 0;
+ float pitchRate_forResponse = 0;
+ float yawRate_forResponse = 0;
+ float thrustAdjustment_total = 0;
+
+ // Create the angle error to use for the inner controller
+ float temp_stateAngleError[3] = {
+ stateInertial[6] - setpointFromRemote_roll,
+ stateInertial[7] - setpointFromRemote_pitch,
+ stateInertial[8] - setpointFromRemote_yaw
+ };
+
+ // Create the z-error
+ float temp_stateZError = stateInertial[2] - setpointFromRemote_z;
+
+ // Perform the "-Kx" LQR computation for the rates:
+ for(int i = 0; i < 3; ++i)
+ {
+ // BODY FRAME Y CONTROLLER
+ rollRate_forResponse -= gainMatrixRollRate_forRemoteControl[i] * temp_stateAngleError[i];
+ // BODY FRAME X CONTROLLER
+ pitchRate_forResponse -= gainMatrixPitchRate_forRemoteControl[i] * temp_stateAngleError[i];
+ // BODY FRAME Z CONTROLLER
+ yawRate_forResponse -= gainMatrixYawRate_forRemoteControl[i] * temp_stateAngleError[i];
+ }
+
+ // Perform the "-Kx" LQR computation for the thrust
+ thrustAdjustment_total -= temp_stateZError * gainMatrixThrust_SixStateVector[2];
+ thrustAdjustment_total -= stateInertial[5] * gainMatrixThrust_SixStateVector[5];
+
+
+ // UPDATE THE "RETURN" THE VARIABLE NAMED "response"
+
+ // Put the computed rates and thrust into the "response" variable
+ // > For roll, pitch, and yaw:
+ response.controlOutput.roll = rollRate_forResponse;
+ response.controlOutput.pitch = pitchRate_forResponse;
+ response.controlOutput.yaw = yawRate_forResponse;
+ // > For the thrust adjustment we must add the feed-forward thrust to counter-act gravity.
+ // > NOTE: remember that the thrust is commanded per motor, so you sohuld
+ // consider whether the "thrustAdjustment" computed by your
+ // controller needed to be divided by 4 or not.
+ float thrustAdjustment = thrustAdjustment_total / 4.0;
+ // > NOTE: the "gravity_force_quarter" value was already divided by 4 when
+ // it was loaded/processes from the .yaml file.
+ response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+
+
+ // Specify that this controller is a rate controller
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
+ response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
+
+ // Display some details (if requested)
+ if (shouldDisplayDebugInfo)
+ {
+ ROS_INFO_STREAM("thrust =" << lqr_angleRateNested_prev_thrustAdjustment );
+ ROS_INFO_STREAM("rollrate =" << response.controlOutput.roll );
+ ROS_INFO_STREAM("pitchrate =" << response.controlOutput.pitch );
+ ROS_INFO_STREAM("yawrate =" << response.controlOutput.yaw );
+ }
+
+}
+
+
+
+
+
+void viconSubscribeObjectNameCallback(const ViconSubscribeObjectName& msg)
+{
+ if (msg.shouldSubscribe)
+ {
+ // Get the object name into the class variable
+ viconObjectName_forRemote = msg.objectName;
+
+ // Create a "ros::NodeHandle" type local variable "nodeHandle" as the current node,
+ // the "~" indcates that "self" is the node handle assigned to this variable.
+ ros::NodeHandle nodeHandle("~");
+
+ // Keeps 10 messages because otherwise ViconDataPublisher would override the data immediately
+ viconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 10, viconCallback);
+ ROS_INFO_STREAM("[REMOTE CONTORLLER] successfully subscribed to ViconData");
+ }
+ else
+ {
+ // Unsubscribe from the Vicon data
+ viconSubscriber.shutdown();
+ }
+}
+
+
+
+
+
+//is called when new data from Vicon arrives
+void viconCallback(const ViconData& viconData)
+{
+ for(std::vector<CrazyflieData>::const_iterator it = viconData.crazyflies.begin(); it != viconData.crazyflies.end(); ++it)
+ {
+ CrazyflieData objectData_in_globalFrame = *it;
+
+ if(objectData_in_globalFrame.crazyflieName == viconObjectName_forRemote)
+ {
+
+ if (shouldPublishRemote_xyz_rpy)
+ {
+ // Publish the "CrayzflieData" for this object
+ remote_xyz_rpy_publisher.publish(objectData_in_globalFrame);
+ }
+
+ if (shouldDisplayRemote_xyz_rpy)
+ {
+ // Dislpaz the "CrayzflieData" for this object
+ ROS_INFO_STREAM("[REMOTE CONTROLLER] Remote [z,r,p,y] = [ " << objectData_in_globalFrame.z << " , " << objectData_in_globalFrame.roll << " , " << objectData_in_globalFrame.pitch << " , " << objectData_in_globalFrame.yaw << " ]" );
+ }
+
+ // Update the remote set-point is not occluded
+ if(!objectData_in_globalFrame.occluded)
+ {
+
+ // Update the z height variable that is used when activating
+ z_of_remote_previous_measurement = objectData_in_globalFrame.z;
+
+ if (isActive_remoteControlMode)
+ {
+ // Update the setpoint used for the "Remote Controller"
+ setpointFromRemote_roll = objectData_in_globalFrame.roll * remoteConrtolSetpointFactor_roll;
+ setpointFromRemote_pitch = objectData_in_globalFrame.pitch * remoteConrtolSetpointFactor_pitch;
+ setpointFromRemote_yaw = objectData_in_globalFrame.yaw * remoteConrtolSetpointFactor_yaw;
+ setpointFromRemote_z = (objectData_in_globalFrame.z - z_when_remote_activated) * remoteConrtolSetpointFactor_z + setpoint[2];
+
+ // Clip the roll angle to its limit
+ if (setpointFromRemote_roll>remoteControlLimit_roll)
+ {
+ setpointFromRemote_roll = remoteControlLimit_roll;
+ }
+ else if (setpointFromRemote_roll<(-remoteControlLimit_roll))
+ {
+ setpointFromRemote_roll = -remoteControlLimit_roll;
+ }
+ // Clip the pitch angle to its limit
+ if (setpointFromRemote_pitch>remoteControlLimit_pitch)
+ {
+ setpointFromRemote_pitch = remoteControlLimit_pitch;
+ }
+ else if (setpointFromRemote_pitch<(-remoteControlLimit_pitch))
+ {
+ setpointFromRemote_pitch = -remoteControlLimit_pitch;
+ }
+
+ // Publish the updated setpoint
+ CrazyflieData setpointToPublish;
+ setpointToPublish.roll = setpointFromRemote_roll;
+ setpointToPublish.pitch = setpointFromRemote_pitch;
+ setpointToPublish.yaw = setpointFromRemote_yaw;
+ setpointToPublish.z = setpointFromRemote_z;
+
+ remote_control_setpoint_publisher.publish(setpointToPublish);
+ }
+ else
+ {
+ // Update the yaw setpoint for the "de-activated" controller
+ // > this ensures a smooth transition from "de-activated" to "activated"
+ setpoint[3] = objectData_in_globalFrame.yaw * remoteConrtolSetpointFactor_yaw;
+ }
+
+ }
+ }
+ }
+}
+
+
+void shouldActivateCallback(const std_msgs::Int32& msg)
+{
+ if (msg.data)
+ {
+ ROS_INFO("[REMOTE CONTROLLER] Received message to ACTIVATE remote control mode.");
+ isActive_remoteControlMode = true;
+
+ z_when_remote_activated = z_of_remote_previous_measurement;
+ }
+ else
+ {
+ ROS_INFO("[REMOTE CONTROLLER] Received message to DE-ACTIVATE remote control mode.");
+ isActive_remoteControlMode = false;
+
+ setpointFromRemote_roll = 0.0;
+ setpointFromRemote_pitch = 0.0;
+ setpointFromRemote_yaw = 0.0;
+ setpointFromRemote_z = 0.0;
+
+ z_when_remote_activated = 0.0;
+ }
+}
+
+
+
+
+
+
+
+
+
+// ------------------------------------------------------------------------------
+// EEEEE SSSS TTTTT III M M A TTTTT III OOO N N
+// E S T I MM MM A A T I O O NN N
+// EEE SSS T I M M M A A T I O O N N N
+// E S T I M M AAAAA T I O O N NN
+// EEEEE SSSS T III M M A A T III OOO N N
+// ------------------------------------------------------------------------------
+void performEstimatorUpdate_forStateInterial(Controller::Request &request)
+{
+
+ // PUT THE CURRENT MEASURED DATA INTO THE CLASS VARIABLE
+ // > for (x,y,z) position
+ current_xzy_rpy_measurement[0] = request.ownCrazyflie.x;
+ current_xzy_rpy_measurement[1] = request.ownCrazyflie.y;
+ current_xzy_rpy_measurement[2] = request.ownCrazyflie.z;
+ // > for (roll,pitch,yaw) angles
+ current_xzy_rpy_measurement[3] = request.ownCrazyflie.roll;
+ current_xzy_rpy_measurement[4] = request.ownCrazyflie.pitch;
+ current_xzy_rpy_measurement[5] = request.ownCrazyflie.yaw;
+
+
+ // RUN THE FINITE DIFFERENCE ESTIMATOR
+ performEstimatorUpdate_forStateInterial_viaFiniteDifference();
+
+
+ // RUN THE POINT MASS KALMAN FILTER ESTIMATOR
+ performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter();
+
+
+ // FILLE IN THE STATE INERTIAL ESTIMATE TO BE USED FOR CONTROL
+ switch (estimator_method)
+ {
+ // Estimator based on finte differences
+ case ESTIMATOR_METHOD_FINITE_DIFFERENCE:
+ {
+ // Transfer the estimate
+ for(int i = 0; i < 12; ++i)
+ {
+ current_stateInertialEstimate[i] = stateInterialEstimate_viaFiniteDifference[i];
+ }
+ break;
+ }
+ // Estimator based on Point Mass Kalman Filter
+ case ESTIMATOR_METHOD_POINT_MASS_PER_DIMENSION:
+ {
+ // Transfer the estimate
+ for(int i = 0; i < 12; ++i)
+ {
+ current_stateInertialEstimate[i] = stateInterialEstimate_viaPointMassKalmanFilter[i];
+ }
+ break;
+ }
+ // Handle the exception
+ default:
+ {
+ // Display that the "estimator_method" was not recognised
+ ROS_INFO_STREAM("[REMOTE CONTROLLER] ERROR: in the 'calculateControlOutput' function of the 'RemoteControllerService': the 'estimator_method' is not recognised.");
+ // Transfer the finite difference estimate by default
+ for(int i = 0; i < 12; ++i)
+ {
+ current_stateInertialEstimate[i] = stateInterialEstimate_viaFiniteDifference[i];
+ }
+ break;
+ }
+ }
+
+
+ // NOW THAT THE ESTIMATORS HAVE ALL BEEN RUN, PUT THE CURRENT
+ // MEASURED DATA INTO THE CLASS VARIABLE FOR THE PREVIOUS
+ // > for (x,y,z) position
+ previous_xzy_rpy_measurement[0] = current_xzy_rpy_measurement[0];
+ previous_xzy_rpy_measurement[1] = current_xzy_rpy_measurement[1];
+ previous_xzy_rpy_measurement[2] = current_xzy_rpy_measurement[2];
+ // > for (roll,pitch,yaw) angles
+ previous_xzy_rpy_measurement[3] = current_xzy_rpy_measurement[3];
+ previous_xzy_rpy_measurement[4] = current_xzy_rpy_measurement[4];
+ previous_xzy_rpy_measurement[5] = current_xzy_rpy_measurement[5];
+}
+
+
+
+void performEstimatorUpdate_forStateInterial_viaFiniteDifference()
+{
+ // PUT IN THE CURRENT MEASUREMENT DIRECTLY
+ // > for (x,y,z) position
+ stateInterialEstimate_viaFiniteDifference[0] = current_xzy_rpy_measurement[0];
+ stateInterialEstimate_viaFiniteDifference[1] = current_xzy_rpy_measurement[1];
+ stateInterialEstimate_viaFiniteDifference[2] = current_xzy_rpy_measurement[2];
+ // > for (roll,pitch,yaw) angles
+ stateInterialEstimate_viaFiniteDifference[6] = current_xzy_rpy_measurement[3];
+ stateInterialEstimate_viaFiniteDifference[7] = current_xzy_rpy_measurement[4];
+ stateInterialEstimate_viaFiniteDifference[8] = current_xzy_rpy_measurement[5];
+
+ // COMPUTE THE VELOCITIES VIA FINITE DIFFERENCE
+ // > for (x,y,z) velocities
+ stateInterialEstimate_viaFiniteDifference[3] = (current_xzy_rpy_measurement[0] - previous_xzy_rpy_measurement[0]) * estimator_frequency;
+ stateInterialEstimate_viaFiniteDifference[4] = (current_xzy_rpy_measurement[1] - previous_xzy_rpy_measurement[1]) * estimator_frequency;
+ stateInterialEstimate_viaFiniteDifference[5] = (current_xzy_rpy_measurement[2] - previous_xzy_rpy_measurement[2]) * estimator_frequency;
+ // > for (roll,pitch,yaw) velocities
+ stateInterialEstimate_viaFiniteDifference[9] = (current_xzy_rpy_measurement[3] - previous_xzy_rpy_measurement[3]) * estimator_frequency;
+ stateInterialEstimate_viaFiniteDifference[10] = (current_xzy_rpy_measurement[4] - previous_xzy_rpy_measurement[4]) * estimator_frequency;
+ stateInterialEstimate_viaFiniteDifference[11] = (current_xzy_rpy_measurement[5] - previous_xzy_rpy_measurement[5]) * estimator_frequency;
+}
+
+
+
+void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter()
+{
+ // PERFORM THE KALMAN FILTER UPDATE STEP
+ // > First take a copy of the estimator state
+ float temp_PMKFstate[12];
+ for(int i = 0; i < 12; ++i)
+ {
+ temp_PMKFstate[i] = stateInterialEstimate_viaPointMassKalmanFilter[i];
+ }
+ // > Now perform update for:
+ // > x position and velocity:
+ stateInterialEstimate_viaPointMassKalmanFilter[0] = PMKF_Ahat_row1_for_positions[0]*temp_PMKFstate[0] + PMKF_Ahat_row1_for_positions[1]*temp_PMKFstate[3] + PMKF_Kinf_for_positions[0]*current_xzy_rpy_measurement[0];
+ stateInterialEstimate_viaPointMassKalmanFilter[3] = PMKF_Ahat_row2_for_positions[0]*temp_PMKFstate[0] + PMKF_Ahat_row2_for_positions[1]*temp_PMKFstate[3] + PMKF_Kinf_for_positions[1]*current_xzy_rpy_measurement[0];
+ // > y position and velocity:
+ stateInterialEstimate_viaPointMassKalmanFilter[1] = PMKF_Ahat_row1_for_positions[0]*temp_PMKFstate[1] + PMKF_Ahat_row1_for_positions[1]*temp_PMKFstate[4] + PMKF_Kinf_for_positions[0]*current_xzy_rpy_measurement[1];
+ stateInterialEstimate_viaPointMassKalmanFilter[4] = PMKF_Ahat_row2_for_positions[0]*temp_PMKFstate[1] + PMKF_Ahat_row2_for_positions[1]*temp_PMKFstate[4] + PMKF_Kinf_for_positions[1]*current_xzy_rpy_measurement[1];
+ // > z position and velocity:
+ stateInterialEstimate_viaPointMassKalmanFilter[2] = PMKF_Ahat_row1_for_positions[0]*temp_PMKFstate[2] + PMKF_Ahat_row1_for_positions[1]*temp_PMKFstate[5] + PMKF_Kinf_for_positions[0]*current_xzy_rpy_measurement[2];
+ stateInterialEstimate_viaPointMassKalmanFilter[5] = PMKF_Ahat_row2_for_positions[0]*temp_PMKFstate[2] + PMKF_Ahat_row2_for_positions[1]*temp_PMKFstate[5] + PMKF_Kinf_for_positions[1]*current_xzy_rpy_measurement[2];
+
+ // > roll position and velocity:
+ stateInterialEstimate_viaPointMassKalmanFilter[6] = PMKF_Ahat_row1_for_angles[0]*temp_PMKFstate[6] + PMKF_Ahat_row1_for_angles[1]*temp_PMKFstate[9] + PMKF_Kinf_for_angles[0]*current_xzy_rpy_measurement[3];
+ stateInterialEstimate_viaPointMassKalmanFilter[9] = PMKF_Ahat_row2_for_angles[0]*temp_PMKFstate[6] + PMKF_Ahat_row2_for_angles[1]*temp_PMKFstate[9] + PMKF_Kinf_for_angles[1]*current_xzy_rpy_measurement[3];
+ // > pitch position and velocity:
+ stateInterialEstimate_viaPointMassKalmanFilter[7] = PMKF_Ahat_row1_for_angles[0]*temp_PMKFstate[7] + PMKF_Ahat_row1_for_angles[1]*temp_PMKFstate[10] + PMKF_Kinf_for_angles[0]*current_xzy_rpy_measurement[4];
+ stateInterialEstimate_viaPointMassKalmanFilter[10] = PMKF_Ahat_row2_for_angles[0]*temp_PMKFstate[7] + PMKF_Ahat_row2_for_angles[1]*temp_PMKFstate[10] + PMKF_Kinf_for_angles[1]*current_xzy_rpy_measurement[4];
+ // > yaw position and velocity:
+ stateInterialEstimate_viaPointMassKalmanFilter[8] = PMKF_Ahat_row1_for_angles[0]*temp_PMKFstate[8] + PMKF_Ahat_row1_for_angles[1]*temp_PMKFstate[11] + PMKF_Kinf_for_angles[0]*current_xzy_rpy_measurement[5];
+ stateInterialEstimate_viaPointMassKalmanFilter[11] = PMKF_Ahat_row2_for_angles[0]*temp_PMKFstate[8] + PMKF_Ahat_row2_for_angles[1]*temp_PMKFstate[11] + PMKF_Kinf_for_angles[1]*current_xzy_rpy_measurement[5];
+}
+
+
+
+
+// ----------------------------------------------------------------------------------
+// L QQQ RRRR
+// L Q Q R R
+// L Q Q RRRR
+// L Q Q R R
+// LLLLL QQ Q R R
+// ----------------------------------------------------------------------------------
+
+void calculateControlOutput_viaLQR(float stateErrorBody[12], Controller::Request &request, Controller::Response &response)
+{
+ // SWITCH BETWEEN THE DIFFERENT LQR CONTROLLER MODES:
+ switch (controller_mode)
+ {
+ // LQR controller based on the state vector:
+ // [position,velocity,angles]
+ case CONTROLLER_MODE_LQR_RATE:
+ {
+ // Call the function that performs the control computations for this mode
+ calculateControlOutput_viaLQRforRates(stateErrorBody,request,response);
+ break;
+ }
+
+ // LQR controller based on the state vector:
+ // [position,velocity]
+ case CONTROLLER_MODE_LQR_ANGLE:
+ {
+ // Call the function that performs the control computations for this mode
+ calculateControlOutput_viaLQRforAngles(stateErrorBody,request,response);
+ break;
+ }
+
+ // LQR controller based on the state vector:
+ // [position,velocity,angles]
+ case CONTROLLER_MODE_LQR_ANGLE_RATE_NESTED:
+ {
+ // Call the function that performs the control computations for this mode
+ calculateControlOutput_viaLQRforAnglesRatesNested(stateErrorBody,request,response);
+ break;
+ }
+
+ default:
+ {
+ // Display that the "controller_mode" was not recognised
+ ROS_INFO_STREAM("[REMOTE CONTROLLER] ERROR: in the 'calculateControlOutput' function of the 'RemoteControllerService': the 'controller_mode' is not recognised.");
+ // Set everything in the response to zero
+ response.controlOutput.roll = 0;
+ response.controlOutput.pitch = 0;
+ response.controlOutput.yaw = 0;
+ response.controlOutput.motorCmd1 = 0;
+ response.controlOutput.motorCmd2 = 0;
+ response.controlOutput.motorCmd3 = 0;
+ response.controlOutput.motorCmd4 = 0;
+ response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
+ break;
+ }
+ }
+}
+
+
+
+
+void calculateControlOutput_viaLQRforRates(float stateErrorBody[12], Controller::Request &request, Controller::Response &response)
+{
+ // PERFORM THE "u=Kx" LQR CONTROLLER COMPUTATION
+
+ // Instantiate the local variables for the following:
+ // > body frame roll rate,
+ // > body frame pitch rate,
+ // > body frame yaw rate,
+ // > total thrust adjustment.
+ // These will be requested from the Crazyflie's on-baord "inner-loop" controller
+ float rollRate_forResponse = 0;
+ float pitchRate_forResponse = 0;
+ float yawRate_forResponse = 0;
+ float thrustAdjustment = 0;
+
+ // Perform the "-Kx" LQR computation for the rates and thrust:
+ for(int i = 0; i < 9; ++i)
+ {
+ // BODY FRAME Y CONTROLLER
+ rollRate_forResponse -= gainMatrixRollRate[i] * stateErrorBody[i];
+ // BODY FRAME X CONTROLLER
+ pitchRate_forResponse -= gainMatrixPitchRate[i] * stateErrorBody[i];
+ // BODY FRAME YAW CONTROLLER
+ yawRate_forResponse -= gainMatrixYawRate[i] * stateErrorBody[i];
+ // > ALITUDE CONTROLLER (i.e., z-controller):
+ thrustAdjustment -= gainMatrixThrust_NineStateVector[i] * stateErrorBody[i];
+ }
+
+
+ // UPDATE THE "RETURN" THE VARIABLE NAMED "response"
+
+ // Put the computed rates and thrust into the "response" variable
+ // > For roll, pitch, and yaw:
+ response.controlOutput.roll = rollRate_forResponse;
+ response.controlOutput.pitch = pitchRate_forResponse;
+ response.controlOutput.yaw = yawRate_forResponse;
+ // > For the thrust adjustment we must add the feed-forward thrust to counter-act gravity.
+ // > NOTE: remember that the thrust is commanded per motor, so you sohuld
+ // consider whether the "thrustAdjustment" computed by your
+ // controller needed to be divided by 4 or not.
+ thrustAdjustment = thrustAdjustment / 4.0;
+ // > NOTE: the "gravity_force_quarter" value was already divided by 4 when
+ // it was loaded/processes from the .yaml file.
+ response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+
+
+ // Specify that this controller is a rate controller
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
+ response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
+
+
+ // An alternate debugging technique is to print out data directly to the
+ // command line from which this node was launched.
+ if (shouldDisplayDebugInfo)
+ {
+ // An example of "printing out" the data from the "request" argument to the
+ // command line. This might be useful for debugging.
+ ROS_INFO_STREAM("x-coordinate [m]: " << request.ownCrazyflie.x);
+ ROS_INFO_STREAM("y-coordinate [m]: " << request.ownCrazyflie.y);
+ ROS_INFO_STREAM("z-coordinate [m]: " << request.ownCrazyflie.z);
+ ROS_INFO_STREAM("roll [deg]: " << request.ownCrazyflie.roll);
+ ROS_INFO_STREAM("pitch [deg]: " << request.ownCrazyflie.pitch);
+ ROS_INFO_STREAM("yaw [deg]: " << request.ownCrazyflie.yaw);
+ ROS_INFO_STREAM("Delta t [s]: " << request.ownCrazyflie.acquiringTime);
+
+ // An example of "printing out" the control actions computed.
+ ROS_INFO_STREAM("thrustAdjustment = " << thrustAdjustment);
+ ROS_INFO_STREAM("controlOutput.roll = " << response.controlOutput.roll);
+ ROS_INFO_STREAM("controlOutput.pitch = " << response.controlOutput.pitch);
+ ROS_INFO_STREAM("controlOutput.yaw = " << response.controlOutput.yaw);
+
+ // An example of "printing out" the "thrust-to-command" conversion parameters.
+ ROS_INFO_STREAM("motorPoly 0:" << motorPoly[0]);
+ ROS_INFO_STREAM("motorPoly 0:" << motorPoly[1]);
+ ROS_INFO_STREAM("motorPoly 0:" << motorPoly[2]);
+
+ // An example of "printing out" the per motor 16-bit command computed.
+ ROS_INFO_STREAM("controlOutput.cmd1 = " << response.controlOutput.motorCmd1);
+ ROS_INFO_STREAM("controlOutput.cmd3 = " << response.controlOutput.motorCmd2);
+ ROS_INFO_STREAM("controlOutput.cmd2 = " << response.controlOutput.motorCmd3);
+ ROS_INFO_STREAM("controlOutput.cmd4 = " << response.controlOutput.motorCmd4);
+ }
+}
+
+
+
+
+void calculateControlOutput_viaLQRforAngles(float stateErrorBody[12], Controller::Request &request, Controller::Response &response)
+{
+ // PERFORM THE "u=Kx" LQR CONTROLLER COMPUTATION
+
+ // Instantiate the local variables for the following:
+ // > body frame roll angle,
+ // > body frame pitch angle,
+ // > total thrust adjustment.
+ // These will be requested from the Crazyflie's on-baord "inner-loop" controller
+ float rollAngle_forResponse = 0;
+ float pitchAngle_forResponse = 0;
+ float thrustAdjustment = 0;
+
+ // Perform the "-Kx" LQR computation for the rates and thrust:
+ for(int i = 0; i < 6; ++i)
+ {
+ // BODY FRAME Y CONTROLLER
+ rollAngle_forResponse -= gainMatrixRollAngle[i] * stateErrorBody[i];
+ // BODY FRAME X CONTROLLER
+ pitchAngle_forResponse -= gainMatrixPitchAngle[i] * stateErrorBody[i];
+ // > ALITUDE CONTROLLER (i.e., z-controller):
+ thrustAdjustment -= gainMatrixThrust_SixStateVector[i] * stateErrorBody[i];
+ }
+
+ // UPDATE THE "RETURN" THE VARIABLE NAMED "response"
+
+ // Put the computed rates and thrust into the "response" variable
+ // > For roll, pitch, and yaw:
+ response.controlOutput.roll = rollAngle_forResponse;
+ response.controlOutput.pitch = pitchAngle_forResponse;
+ response.controlOutput.yaw = setpoint[3];
+ // > For the thrust adjustment we must add the feed-forward thrust to counter-act gravity.
+ // > NOTE: remember that the thrust is commanded per motor, so you sohuld
+ // consider whether the "thrustAdjustment" computed by your
+ // controller needed to be divided by 4 or not.
+ thrustAdjustment = thrustAdjustment / 4.0;
+ // > NOTE: the "gravity_force_quarter" value was already divided by 4 when
+ // it was loaded/processes from the .yaml file.
+ response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+
+
+ // Specify that this controller is a rate controller
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
+ response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
+
+
+}
+
+
+
+void calculateControlOutput_viaLQRforAnglesRatesNested( float stateErrorBody[12], Controller::Request &request, Controller::Response &response)
+{
+ // PERFORM THE NESTED "u=Kx" LQR CONTROLLER COMPUTATION
+
+ // Increment the counter variable
+ lqr_angleRateNested_counter++;
+
+ if (lqr_angleRateNested_counter > 4)
+ {
+ //ROS_INFO("Outer called");
+
+ // Reset the counter to 1
+ lqr_angleRateNested_counter = 1;
+
+ // PERFORM THE OUTER "u=Kx" LQR CONTROLLER COMPUTATION
+
+ // Reset the class variable to zero for the following:
+ // > body frame roll angle,
+ // > body frame pitch angle,
+ // > body frame yaw angle,
+ // > total thrust adjustment.
+ // These will be requested from the "inner-loop" LQR controller below
+ lqr_angleRateNested_prev_rollAngle = 0;
+ lqr_angleRateNested_prev_pitchAngle = 0;
+ lqr_angleRateNested_prev_thrustAdjustment = 0;
+
+ // Perform the "-Kx" LQR computation for the rates and thrust:
+ for(int i = 0; i < 6; ++i)
+ {
+ // BODY FRAME Y CONTROLLER
+ lqr_angleRateNested_prev_rollAngle -= gainMatrixRollAngle_50Hz[i] * stateErrorBody[i];
+ // BODY FRAME X CONTROLLER
+ lqr_angleRateNested_prev_pitchAngle -= gainMatrixPitchAngle_50Hz[i] * stateErrorBody[i];
+ // > ALITUDE CONTROLLER (i.e., z-controller):
+ lqr_angleRateNested_prev_thrustAdjustment -= gainMatrixThrust_SixStateVector_50Hz[i] * stateErrorBody[i];
+ }
+
+ // BODY FRAME Z CONTROLLER
+ //lqr_angleRateNested_prev_yawAngle = setpoint[3];
+ lqr_angleRateNested_prev_yawAngle = stateErrorBody[8];
+
+
+ }
+
+ //ROS_INFO("Inner called");
+
+ // PERFORM THE INNER "u=Kx" LQR CONTROLLER COMPUTATION
+ // Instantiate the local variables for the following:
+ // > body frame roll rate,
+ // > body frame pitch rate,
+ // > body frame yaw rate,
+ // These will be requested from the Crazyflie's on-baord "inner-loop" controller
+ float rollRate_forResponse = 0;
+ float pitchRate_forResponse = 0;
+ float yawRate_forResponse = 0;
+
+ // Create the angle error to use for the inner controller
+ float temp_stateAngleError[3] = {
+ stateErrorBody[6] - lqr_angleRateNested_prev_rollAngle,
+ stateErrorBody[7] - lqr_angleRateNested_prev_pitchAngle,
+ lqr_angleRateNested_prev_yawAngle
+ };
+
+ // Perform the "-Kx" LQR computation for the rates and thrust:
+ for(int i = 0; i < 4; ++i)
+ {
+ // BODY FRAME Y CONTROLLER
+ rollRate_forResponse -= gainMatrixRollRate_Nested[i] * temp_stateAngleError[i];
+ // BODY FRAME X CONTROLLER
+ pitchRate_forResponse -= gainMatrixPitchRate_Nested[i] * temp_stateAngleError[i];
+ // BODY FRAME Z CONTROLLER
+ yawRate_forResponse -= gainMatrixYawRate_Nested[i] * temp_stateAngleError[i];
+ }
+
+
+ // UPDATE THE "RETURN" THE VARIABLE NAMED "response"
+
+ // Put the computed rates and thrust into the "response" variable
+ // > For roll, pitch, and yaw:
+ response.controlOutput.roll = rollRate_forResponse;
+ response.controlOutput.pitch = pitchRate_forResponse;
+ response.controlOutput.yaw = yawRate_forResponse;
+ // > For the thrust adjustment we must add the feed-forward thrust to counter-act gravity.
+ // > NOTE: remember that the thrust is commanded per motor, so you sohuld
+ // consider whether the "thrustAdjustment" computed by your
+ // controller needed to be divided by 4 or not.
+ float thrustAdjustment = lqr_angleRateNested_prev_thrustAdjustment / 4.0;
+ // > NOTE: the "gravity_force_quarter" value was already divided by 4 when
+ // it was loaded/processes from the .yaml file.
+ response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+
+
+ // Specify that this controller is a rate controller
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
+ response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
+
+ // Display some details (if requested)
+ if (shouldDisplayDebugInfo)
+ {
+ ROS_INFO_STREAM("thrust =" << lqr_angleRateNested_prev_thrustAdjustment );
+ ROS_INFO_STREAM("rollrate =" << response.controlOutput.roll );
+ ROS_INFO_STREAM("pitchrate =" << response.controlOutput.pitch );
+ ROS_INFO_STREAM("yawrate =" << response.controlOutput.yaw );
+ }
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+void construct_and_publish_debug_message(Controller::Request &request, Controller::Response &response)
+{
+ // ***********************************************************
+ // DDDD EEEEE BBBB U U GGGG M M SSSS GGGG
+ // D D E B B U U G MM MM S G
+ // D D EEE BBBB U U G M M M SSS G
+ // D D E B B U U G G M M S G G
+ // DDDD EEEEE BBBB UUU GGGG M M SSSS GGGG
+
+ // DEBUGGING CODE:
+ // As part of the D-FaLL-System we have defined a message type names"DebugMsg".
+ // By fill this message with data and publishing it you can display the data in
+ // real time using rpt plots. Instructions for using rqt plots can be found on
+ // the wiki of the D-FaLL-System repository
+
+ // Instantiate a local variable of type "DebugMsg", see the file "DebugMsg.msg"
+ // (located in the "msg" folder) to see the full structure of this message.
+ DebugMsg debugMsg;
+
+ // Fill the debugging message with the data provided by Vicon
+ debugMsg.vicon_x = request.ownCrazyflie.x;
+ debugMsg.vicon_y = request.ownCrazyflie.y;
+ debugMsg.vicon_z = request.ownCrazyflie.z;
+ debugMsg.vicon_roll = request.ownCrazyflie.roll;
+ debugMsg.vicon_pitch = request.ownCrazyflie.pitch;
+ debugMsg.vicon_yaw = request.ownCrazyflie.yaw;
+
+ // Fill in the debugging message with any other data you would like to display
+ // in real time. For example, it might be useful to display the thrust
+ // adjustment computed by the z-altitude controller.
+ // The "DebugMsg" type has 10 properties from "value_1" to "value_10", all of
+ // type "float64" that you can fill in with data you would like to plot in
+ // real-time.
+ // debugMsg.value_1 = thrustAdjustment;
+ // ......................
+ // debugMsg.value_10 = your_variable_name;
+
+ // Publish the "debugMsg"
+ debugPublisher.publish(debugMsg);
+}
+
+
+
+
+// ------------------------------------------------------------------------------
+// RRRR OOO TTTTT A TTTTT EEEEE III N N TTTTT OOO
+// R R O O T A A T E I NN N T O O
+// RRRR O O T A A T EEE I N N N T O O
+// R R O O T AAAAA T E I N NN T O O
+// R R OOO T A A T EEEEE III N N T OOO
+//
+// BBBB OOO DDDD Y Y FFFFF RRRR A M M EEEEE
+// B B O O D D Y Y F R R A A MM MM E
+// BBBB O O D D Y FFF RRRR A A M M M EEE
+// B B O O D D Y F R R AAAAA M M E
+// BBBB OOO DDDD Y F R R A A M M EEEEE
+// ------------------------------------------------------------------------------
+
+// The arguments for this function are as follows:
+// stateInertial
+// This is an array of length 9 with the estimates the error of of the following values
+// relative to the sepcifed setpoint:
+// stateInertial[0] x position of the Crazyflie relative to the inertial frame origin [meters]
+// stateInertial[1] y position of the Crazyflie relative to the inertial frame origin [meters]
+// stateInertial[2] z position of the Crazyflie relative to the inertial frame origin [meters]
+// stateInertial[3] x-axis component of the velocity of the Crazyflie in the inertial frame [meters/second]
+// stateInertial[4] y-axis component of the velocity of the Crazyflie in the inertial frame [meters/second]
+// stateInertial[5] z-axis component of the velocity of the Crazyflie in the inertial frame [meters/second]
+// stateInertial[6] The roll component of the intrinsic Euler angles [radians]
+// stateInertial[7] The pitch component of the intrinsic Euler angles [radians]
+// stateInertial[8] The yaw component of the intrinsic Euler angles [radians]
+//
+// stateBody
+// This is an empty array of length 9, this function should fill in all elements of this
+// array with the same ordering as for the "stateInertial" argument, expect that the (x,y)
+// position and (x,y) velocities are rotated into the body frame.
+//
+// yaw_measured
+// This is the yaw component of the intrinsic Euler angles in [radians] as measured by
+// the Vicon motion capture system
+//
+void convertIntoBodyFrame(float stateInertial[12], float (&stateBody)[12], float yaw_measured)
+{
+ if (shouldPerformConvertIntoBodyFrame)
+ {
+ float sinYaw = sin(yaw_measured);
+ float cosYaw = cos(yaw_measured);
+
+ // Fill in the (x,y,z) position estimates to be returned
+ stateBody[0] = stateInertial[0] * cosYaw + stateInertial[1] * sinYaw;
+ stateBody[1] = stateInertial[1] * cosYaw - stateInertial[0] * sinYaw;
+ stateBody[2] = stateInertial[2];
+
+ // Fill in the (x,y,z) velocity estimates to be returned
+ stateBody[3] = stateInertial[3] * cosYaw + stateInertial[4] * sinYaw;
+ stateBody[4] = stateInertial[4] * cosYaw - stateInertial[3] * sinYaw;
+ stateBody[5] = stateInertial[5];
+
+ // Fill in the (roll,pitch,yaw) estimates to be returned
+ stateBody[6] = stateInertial[6];
+ stateBody[7] = stateInertial[7];
+ stateBody[8] = stateInertial[8];
+
+ // Fill in the (roll,pitch,yaw) velocity estimates to be returned
+ stateBody[9] = stateInertial[9];
+ stateBody[10] = stateInertial[10];
+ stateBody[11] = stateInertial[11];
+ }
+ else
+ {
+ // Fill in the (x,y,z) position estimates to be returned
+ stateBody[0] = stateInertial[0];
+ stateBody[1] = stateInertial[1];
+ stateBody[2] = stateInertial[2];
+
+ // Fill in the (x,y,z) velocity estimates to be returned
+ stateBody[3] = stateInertial[3];
+ stateBody[4] = stateInertial[4];
+ stateBody[5] = stateInertial[5];
+
+ // Fill in the (roll,pitch,yaw) estimates to be returned
+ stateBody[6] = stateInertial[6];
+ stateBody[7] = stateInertial[7];
+ stateBody[8] = stateInertial[8];
+
+ // Fill in the (roll,pitch,yaw) velocity estimates to be returned
+ stateBody[9] = stateInertial[9];
+ stateBody[10] = stateInertial[10];
+ stateBody[11] = stateInertial[11];
+ }
+}
+
+
+
+
+void convert_stateInertial_to_bodyFrameError(float stateInertial[12], float setpoint[4], float (&bodyFrameError)[12])
+{
+ // Store the current YAW in a local variable
+ float temp_stateInertial_yaw = stateInertial[8];
+
+ // Adjust the INERTIAL (x,y,z) position for the setpoint
+ stateInertial[0] = stateInertial[0] - setpoint[0];
+ stateInertial[1] = stateInertial[1] - setpoint[1];
+ stateInertial[2] = stateInertial[2] - setpoint[2];
+
+ // Fill in the yaw angle error
+ // > This error should be "unwrapped" to be in the range
+ // ( -pi , pi )
+ // > First, get the yaw error into a local variable
+ float yawError = stateInertial[8] - setpoint[3];
+ // > Second, "unwrap" the yaw error to the interval ( -pi , pi )
+ while(yawError > PI) {yawError -= 2 * PI;}
+ while(yawError < -PI) {yawError += 2 * PI;}
+ // > Third, put the "yawError" into the "stateError" variable
+ stateInertial[8] = yawError;
+
+
+ if (yawError>(PI/6))
+ {
+ yawError = (PI/6);
+ }
+ else if (yawError<(-PI/6))
+ {
+ yawError = (-PI/6);
+ }
+
+ // CONVERSION INTO BODY FRAME
+ // Conver the state erorr from the Inertial frame into the Body frame
+ // > Note: the function "convertIntoBodyFrame" is implemented in this file
+ // and by default does not perform any conversion. The equations to convert
+ // the state error into the body frame should be implemented in that function
+ // for successful completion of the PPS exercise
+ convertIntoBodyFrame(stateInertial, bodyFrameError, temp_stateInertial_yaw);
+}
+
+
+
+
+// ------------------------------------------------------------------------------
+// N N EEEEE W W TTTTT OOO N N CCCC M M DDDD
+// NN N E W W T O O NN N C MM MM D D
+// N N N EEE W W T O O N N N --> C M M M D D
+// N NN E W W W T O O N NN C M M D D
+// N N EEEEE W W T OOO N N CCCC M M DDDD
+//
+// CCCC OOO N N V V EEEEE RRRR SSSS III OOO N N
+// C O O NN N V V E R R S I O O NN N
+// C O O N N N V V EEE RRRR SSS I O O N N N
+// C O O N NN V V E R R S I O O N NN
+// CCCC OOO N N V EEEEE R R SSSS III OOO N N
+// ------------------------------------------------------------------------------
+
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+float computeMotorPolyBackward(float thrust)
+{
+ // Compute the 16-bit command signal that generates the "thrust" force
+ float cmd = (-motorPoly[1] + sqrt(motorPoly[1] * motorPoly[1] - 4 * motorPoly[2] * (motorPoly[0] - thrust))) / (2 * motorPoly[2]);
+
+ // Saturate the signal to be 0 or in the range [1000,65000]
+ if (cmd < cmd_sixteenbit_min)
+ {
+ cmd = 0;
+ }
+ else if (cmd > cmd_sixteenbit_max)
+ {
+ cmd = cmd_sixteenbit_max;
+ }
+
+ return cmd;
+}
+
+
+
+
+// ----------------------------------------------------------------------------------
+// N N EEEEE W W SSSS EEEEE TTTTT PPPP OOO III N N TTTTT
+// NN N E W W S E T P P O O I NN N T
+// N N N EEE W W SSS EEE T PPPP O O I N N N T
+// N NN E W W W S E T P O O I N NN T
+// N N EEEEE W W SSSS EEEEE T P OOO III N N T
+//
+// GGG U U III CCCC A L L BBBB A CCCC K K
+// G G U U I C A A L L B B A A C K K
+// G U U I C A A L L BBBB A A C KKK
+// G G U U I C AAAAA L L B B AAAAA C K K
+// GGGG UUU III CCCC A A LLLLL LLLLL BBBB A A CCCC K K
+// ----------------------------------------------------------------------------------
+
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+void setpointCallback(const Setpoint& newSetpoint)
+{
+ setpoint[0] = newSetpoint.x;
+ setpoint[1] = newSetpoint.y;
+ setpoint[2] = newSetpoint.z;
+ setpoint[3] = newSetpoint.yaw;
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// L OOO A DDDD
+// L O O A A D D
+// L O O A A D D
+// L O O AAAAA D D
+// LLLLL OOO A A DDDD
+//
+// PPPP A RRRR A M M EEEEE TTTTT EEEEE RRRR SSSS
+// P P A A R R A A MM MM E T E R R S
+// PPPP A A RRRR A A M M M EEE T EEE RRRR SSS
+// P AAAAA R R AAAAA M M E T E R R S
+// P A A R R A A M M EEEEE T EEEEE R R SSSS
+// ----------------------------------------------------------------------------------
+
+
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+void yamlReadyForFetchCallback(const std_msgs::Int32& msg)
+{
+ // Extract from the "msg" for which controller the and from where to fetch the YAML
+ // parameters
+ int controller_to_fetch_yaml = msg.data;
+
+ // Switch between fetching for the different controllers and from different locations
+ switch(controller_to_fetch_yaml)
+ {
+
+ // > FOR FETCHING FROM THE AGENT'S OWN PARAMETER SERVICE
+ case FETCH_YAML_REMOTE_CONTROLLER_FROM_OWN_AGENT:
+ {
+ // Let the user know that this message was received
+ ROS_INFO("[REMOTE CONTROLLER] Received the message that YAML parameters were (re-)loaded. > Now fetching the parameter values from this agent.");
+ // Create a node handle to the parameter service running on this agent's machine
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(namespace_to_own_agent_parameter_service);
+ // Call the function that fetches the parameters
+ fetchYamlParameters(nodeHandle_to_own_agent_parameter_service);
+ break;
+ }
+
+ // > FOR FETCHING FROM THE COORDINATOR'S PARAMETER SERVICE
+ case FETCH_YAML_REMOTE_CONTROLLER_FROM_COORDINATOR:
+ {
+ // Let the user know that this message was received
+ ROS_INFO("[REMOTE CONTROLLER] Received the message that YAML parameters were (re-)loaded. > Now fetching the parameter values from the coordinator.");
+ // Create a node handle to the parameter service running on the coordinator machine
+ ros::NodeHandle nodeHandle_to_coordinator_parameter_service(namespace_to_coordinator_parameter_service);
+ // Call the function that fetches the parameters
+ fetchYamlParameters(nodeHandle_to_coordinator_parameter_service);
+ break;
+ }
+
+ default:
+ {
+ // Let the user know that the command was not relevant
+ //ROS_INFO("The CustomControllerService received the message that YAML parameters were (re-)loaded");
+ //ROS_INFO("> However the parameters do not relate to this controller, hence nothing will be fetched.");
+ break;
+ }
+ }
+}
+
+
+// This function CAN BE edited for successful completion of the PPS exercise, and the
+// use of parameters fetched from the YAML file is highly recommended to make tuning of
+// your controller easier and quicker.
+void fetchYamlParameters(ros::NodeHandle& nodeHandle)
+{
+ // Here we load the parameters that are specified in the CustomController.yaml file
+
+ // Add the "CustomController" namespace to the "nodeHandle"
+ ros::NodeHandle nodeHandle_for_RemoteController(nodeHandle, "RemoteController");
+
+
+ // ******************************************************************************* //
+ // PARAMETERS SPECIFIC TO THE REMOTE CONTROL FEATURE
+
+ // Vicon object name of the Remote to follow
+ default_viconObjectName_forRemote = getParameterString(nodeHandle_for_RemoteController, "default_viconObjectName_forRemote");
+
+ // Boolean for whether the Remote's state should be published as a message
+ shouldPublishRemote_xyz_rpy = getParameterBool(nodeHandle_for_RemoteController, "shouldPublishRemote_xyz_rpy");
+
+ // Boolean for whether the Remote's state should be display in the terminal window
+ shouldDisplayRemote_xyz_rpy = getParameterBool(nodeHandle_for_RemoteController, "shouldDisplayRemote_xyz_rpy");
+
+ // Roll and pitch limit (in degrees for angles)
+ remoteControlLimit_roll_degrees = getParameterFloat(nodeHandle_for_RemoteController , "remoteControlLimit_roll_degrees");
+ remoteControlLimit_pitch_degrees = getParameterFloat(nodeHandle_for_RemoteController , "remoteControlLimit_pitch_degrees");
+
+ // Factor by which to reduce the remote control input
+ remoteConrtolSetpointFactor_roll = getParameterFloat(nodeHandle_for_RemoteController , "remoteConrtolSetpointFactor_roll");
+ remoteConrtolSetpointFactor_pitch = getParameterFloat(nodeHandle_for_RemoteController , "remoteConrtolSetpointFactor_pitch");
+ remoteConrtolSetpointFactor_yaw = getParameterFloat(nodeHandle_for_RemoteController , "remoteConrtolSetpointFactor_yaw");
+ remoteConrtolSetpointFactor_z = getParameterFloat(nodeHandle_for_RemoteController , "remoteConrtolSetpointFactor_z");
+
+ // LQR Gain matrix for tracking the angle commands from the Crazyflie
+ getParameterFloatVector(nodeHandle_for_RemoteController, "gainMatrixRollRate_forRemoteControl", gainMatrixRollRate_forRemoteControl, 3);
+ getParameterFloatVector(nodeHandle_for_RemoteController, "gainMatrixPitchRate_forRemoteControl", gainMatrixPitchRate_forRemoteControl, 3);
+ getParameterFloatVector(nodeHandle_for_RemoteController, "gainMatrixYawRate_forRemoteControl", gainMatrixYawRate_forRemoteControl, 3);
+
+ // DEBUGGING: Print out one of the parameters that was loaded
+ ROS_INFO_STREAM("[REMOTE CONTROLLER] DEBUGGING: the fetched RemoteController/default_viconObjectName_forRemote = " << default_viconObjectName_forRemote);
+
+ // ******************************************************************************* //
+
+
+
+ // > The mass of the crazyflie
+ cf_mass = getParameterFloat(nodeHandle_for_RemoteController , "mass");
+
+ // Display one of the YAML parameters to debug if it is working correctly
+ //ROS_INFO_STREAM("DEBUGGING: mass leaded from loacl file = " << cf_mass );
+
+ // > The frequency at which the "computeControlOutput" is being called, as determined
+ // by the frequency at which the Vicon system provides position and attitude data
+ vicon_frequency = getParameterFloat(nodeHandle_for_RemoteController, "vicon_frequency");
+
+ // > The frequency at which the "computeControlOutput" is being called, as determined
+ // by the frequency at which the Vicon system provides position and attitude data
+ control_frequency = getParameterFloat(nodeHandle_for_RemoteController, "control_frequency");
+
+ // > The co-efficients of the quadratic conversation from 16-bit motor command to
+ // thrust force in Newtons
+ getParameterFloatVector(nodeHandle_for_RemoteController, "motorPoly", motorPoly, 3);
+
+ // > The boolean for whether to execute the convert into body frame function
+ shouldPerformConvertIntoBodyFrame = getParameterBool(nodeHandle_for_RemoteController, "shouldPerformConvertIntoBodyFrame");
+
+ // Boolean indiciating whether the "Debug Message" of this agent should be published or not
+ shouldPublishDebugMessage = getParameterBool(nodeHandle_for_RemoteController, "shouldPublishDebugMessage");
+
+ // Boolean indiciating whether the debugging ROS_INFO_STREAM should be displayed or not
+ shouldDisplayDebugInfo = getParameterBool(nodeHandle_for_RemoteController, "shouldDisplayDebugInfo");
+
+
+ // THE CONTROLLER GAINS:
+
+ // A flag for which controller to use:
+ controller_mode = getParameterInt( nodeHandle_for_RemoteController , "controller_mode" );
+
+ // A flag for which estimator to use:
+ estimator_method = getParameterInt( nodeHandle_for_RemoteController , "estimator_method" );
+
+ // The LQR Controller parameters for "LQR_MODE_RATE"
+ getParameterFloatVector(nodeHandle_for_RemoteController, "gainMatrixThrust_NineStateVector", gainMatrixThrust_NineStateVector, 9);
+ getParameterFloatVector(nodeHandle_for_RemoteController, "gainMatrixRollRate", gainMatrixRollRate, 9);
+ getParameterFloatVector(nodeHandle_for_RemoteController, "gainMatrixPitchRate", gainMatrixPitchRate, 9);
+ getParameterFloatVector(nodeHandle_for_RemoteController, "gainMatrixYawRate", gainMatrixYawRate, 9);
+
+ // The LQR Controller parameters for "LQR_MODE_ANGLE"
+ getParameterFloatVector(nodeHandle_for_RemoteController, "gainMatrixThrust_SixStateVector", gainMatrixThrust_SixStateVector, 6);
+ getParameterFloatVector(nodeHandle_for_RemoteController, "gainMatrixRollAngle", gainMatrixRollAngle, 6);
+ getParameterFloatVector(nodeHandle_for_RemoteController, "gainMatrixPitchAngle", gainMatrixPitchAngle, 6);
+
+ // The LQR Controller parameters for "LQR_MODE_ANGLE_RATE_NESTED"
+ getParameterFloatVector(nodeHandle_for_RemoteController, "gainMatrixThrust_SixStateVector_50Hz", gainMatrixThrust_SixStateVector_50Hz, 6);
+ getParameterFloatVector(nodeHandle_for_RemoteController, "gainMatrixRollAngle_50Hz", gainMatrixRollAngle_50Hz, 6);
+ getParameterFloatVector(nodeHandle_for_RemoteController, "gainMatrixPitchAngle_50Hz", gainMatrixPitchAngle_50Hz, 6);
+
+ getParameterFloatVector(nodeHandle_for_RemoteController, "gainMatrixRollRate_Nested", gainMatrixRollRate_Nested, 3);
+ getParameterFloatVector(nodeHandle_for_RemoteController, "gainMatrixPitchRate_Nested", gainMatrixPitchRate_Nested, 3);
+ getParameterFloatVector(nodeHandle_for_RemoteController, "gainMatrixYawRate_Nested", gainMatrixYawRate_Nested, 3);
+
+
+ // 16-BIT COMMAND LIMITS
+ cmd_sixteenbit_min = getParameterFloat(nodeHandle_for_RemoteController, "command_sixteenbit_min");
+ cmd_sixteenbit_max = getParameterFloat(nodeHandle_for_RemoteController, "command_sixteenbit_max");
+
+
+ // THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
+ // > For the (x,y,z) position
+ getParameterFloatVector(nodeHandle_for_RemoteController, "PMKF_Ahat_row1_for_positions", PMKF_Ahat_row1_for_positions, 2);
+ getParameterFloatVector(nodeHandle_for_RemoteController, "PMKF_Ahat_row2_for_positions", PMKF_Ahat_row2_for_positions, 2);
+ getParameterFloatVector(nodeHandle_for_RemoteController, "PMKF_Kinf_for_positions" , PMKF_Kinf_for_positions , 2);
+ // > For the (roll,pitch,yaw) angles
+ getParameterFloatVector(nodeHandle_for_RemoteController, "PMKF_Ahat_row1_for_angles", PMKF_Ahat_row1_for_angles, 2);
+ getParameterFloatVector(nodeHandle_for_RemoteController, "PMKF_Ahat_row2_for_angles", PMKF_Ahat_row2_for_angles, 2);
+ getParameterFloatVector(nodeHandle_for_RemoteController, "PMKF_Kinf_for_angles" , PMKF_Kinf_for_angles , 2);
+
+
+ // DEBUGGING: Print out one of the parameters that was loaded
+ //ROS_INFO_STREAM("[REMOTE CONTROLLER] DEBUGGING: the fetched RemoteController/mass = " << cf_mass);
+
+
+ // Call the function that computes details an values that are needed from these
+ // parameters loaded above
+ processFetchedParameters();
+}
+
+
+// This function CAN BE edited for successful completion of the PPS exercise, and the
+// use of parameters loaded from the YAML file is highly recommended to make tuning of
+// your controller easier and quicker.
+void processFetchedParameters()
+{
+ // Compute the feed-forward force that we need to counteract gravity (i.e., mg)
+ // > in units of [Newtons]
+ gravity_force = cf_mass * 9.81/(1000);
+ gravity_force_quarter = cf_mass * 9.81/(1000*4);
+
+ // Set that the estimator frequency is the same as the control frequency
+ estimator_frequency = vicon_frequency;
+
+
+ // Roll and pitch limit (in degrees for angles)
+ remoteControlLimit_roll = remoteControlLimit_roll_degrees * PI / 180.0;
+ remoteControlLimit_pitch = remoteControlLimit_pitch_degrees * PI / 180.0;
+
+
+ // Use the Remote name if the variable is currently empty
+ if (viconObjectName_forRemote == "empty")
+ {
+ viconObjectName_forRemote = default_viconObjectName_forRemote;
+ }
+
+
+ // DEBUGGING: Print out one of the computed quantities
+ ROS_INFO_STREAM("[REMOTE CONTROLLER] DEBUGGING: after processing the viconObjectName_forRemote = " << viconObjectName_forRemote);
+}
+
+
+
+// ----------------------------------------------------------------------------------
+// GGGG EEEEE TTTTT PPPP A RRRR A M M ( )
+// G E T P P A A R R A A MM MM ( )
+// G EEE T PPPP A A RRRR A A M M M ( )
+// G G E T P AAAAA R R AAAAA M M ( )
+// GGGG EEEEE T P A A R R A A M M ( )
+// ----------------------------------------------------------------------------------
+
+
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name)
+{
+ float val;
+ if(!nodeHandle.getParam(name, val))
+ {
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val;
+}
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length)
+{
+ if(!nodeHandle.getParam(name, val)){
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ if(val.size() != length) {
+ ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
+ }
+}
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+int getParameterInt(ros::NodeHandle& nodeHandle, std::string name)
+{
+ int val;
+ if(!nodeHandle.getParam(name, val))
+ {
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val;
+}
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+void getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length)
+{
+ if(!nodeHandle.getParam(name, val)){
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ if(val.size() != length) {
+ ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
+ }
+}
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+int getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val)
+{
+ if(!nodeHandle.getParam(name, val)){
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val.size();
+}
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name)
+{
+ bool val;
+ if(!nodeHandle.getParam(name, val))
+ {
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val;
+}
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+std::string getParameterString(ros::NodeHandle& nodeHandle, std::string name)
+{
+ std::string val;
+ if(!nodeHandle.getParam(name, val)){
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val;
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// M M A III N N
+// MM MM A A I NN N
+// M M M A A I N N N
+// M M AAAAA I N NN
+// M M A A III N N
+// ----------------------------------------------------------------------------------
+
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+int main(int argc, char* argv[]) {
+
+ // Starting the ROS-node
+ ros::init(argc, argv, "RemoteControllerService");
+
+ // Create a "ros::NodeHandle" type local variable "nodeHandle" as the current node,
+ // the "~" indcates that "self" is the node handle assigned to this variable.
+ ros::NodeHandle nodeHandle("~");
+
+ // Get the namespace of this "StudentControllerService" node
+ // > This should be something like: "/dfall/agentXXX"
+ std::string m_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[REMOTE CONTROLLER] ros::this_node::getNamespace() = " << m_namespace);
+
+ // Get the agent ID as the "ROS_NAMESPACE" this computer.
+ // NOTES:
+ // > If you look at the "Student.launch" file in the "launch" folder, you will see
+ // the following line of code:
+ // <param name="studentID" value="$(optenv ROS_NAMESPACE)" />
+ // This line of code adds a parameter named "studentID" to the "PPSClient"
+ // > Thus, to get access to this "studentID" paremeter, we first need to get a handle
+ // to the "PPSClient" node within which this controller service is nested.
+ // Get the handle to the "PPSClient" node
+ ros::NodeHandle PPSClient_nodeHandle(m_namespace + "/PPSClient");
+ // Get the value of the "studentID" parameter into the instance variable "my_agentID"
+ if(!PPSClient_nodeHandle.getParam("agentID", my_agentID))
+ {
+ // Throw an error if the student ID parameter could not be obtained
+ ROS_ERROR("[REMOTE CONTROLLER] Failed to get agentID from PPSClient");
+ }
+
+
+ // *********************************************************************************
+ // EVERYTHING THAT RELATES TO FETCHING PARAMETERS FROM A YAML FILE
+
+
+ // EVERYTHING FOR THE CONNECTION TO THIS AGENT's OWN PARAMETER SERVICE:
+
+ // Set the class variable "namespace_to_own_agent_parameter_service" to be a the
+ // namespace string for the parameter service that is running on the machine of this
+ // agent
+ namespace_to_own_agent_parameter_service = m_namespace + "/ParameterService";
+
+ // Create a node handle to the parameter service running on this agent's machine
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(namespace_to_own_agent_parameter_service);
+
+ // Instantiate the local variable "controllerYamlReadyForFetchSubscriber" to be a
+ // "ros::Subscriber" type variable that subscribes to the "controllerYamlReadyForFetch" topic
+ // and calls the class function "yamlReadyForFetchCallback" each time a message is
+ // received on this topic and the message is passed as an input argument to the
+ // "yamlReadyForFetchCallback" class function.
+ ros::Subscriber controllerYamlReadyForFetchSubscriber_to_agent = nodeHandle_to_own_agent_parameter_service.subscribe("controllerYamlReadyForFetch", 1, yamlReadyForFetchCallback);
+
+
+ // EVERYTHING FOR THE CONNECTION THE COORDINATOR'S PARAMETER SERVICE:
+
+ // Set the class variable "nodeHandle_to_coordinator_parameter_service" to be a node handle
+ // for the parameter service that is running on the coordinate machine
+ // NOTE: the backslash here (i.e., "/") before the name of the node ("ParameterService")
+ // is very important because it specifies that the name is global
+ namespace_to_coordinator_parameter_service = "/ParameterService";
+
+ // Create a node handle to the parameter service running on the coordinator machine
+ ros::NodeHandle nodeHandle_to_coordinator = ros::NodeHandle();
+ //ros::NodeHandle nodeHandle_to_coordinator_parameter_service = ros::NodeHandle(namespace_to_own_agent_parameter_service);
+
+
+ // Instantiate the local variable "controllerYamlReadyForFetchSubscriber" to be a
+ // "ros::Subscriber" type variable that subscribes to the "controllerYamlReadyForFetch" topic
+ // and calls the class function "yamlReadyForFetchCallback" each time a message is
+ // received on this topic and the message is passed as an input argument to the
+ // "yamlReadyForFetchCallback" class function.
+ ros::Subscriber controllerYamlReadyForFetchSubscriber_to_coordinator = nodeHandle_to_coordinator.subscribe("/ParameterService/controllerYamlReadyForFetch", 1, yamlReadyForFetchCallback);
+ //ros::Subscriber controllerYamlReadyForFetchSubscriber_to_coordinator = nodeHandle_to_coordinator_parameter_service.subscribe("controllerYamlReadyForFetch", 1, yamlReadyForFetchCallback);
+
+
+ // PRINT OUT SOME INFORMATION
+
+ // Let the user know what namespaces are being used for linking to the parameter service
+ ROS_INFO_STREAM("[REMOTE CONTROLLER] The namespace string for accessing the Paramter Services are:");
+ ROS_INFO_STREAM("[REMOTE CONTROLLER] namespace_to_own_agent_parameter_service = " << namespace_to_own_agent_parameter_service);
+ ROS_INFO_STREAM("[REMOTE CONTROLLER] namespace_to_coordinator_parameter_service = " << namespace_to_coordinator_parameter_service);
+
+
+ // FINALLY, FETCH ANY PARAMETERS REQUIRED FROM THESE "PARAMETER SERVICES"
+
+ // Call the class function that loads the parameters for this class.
+ fetchYamlParameters(nodeHandle_to_own_agent_parameter_service);
+
+ // *********************************************************************************
+
+
+
+
+ //keeps 100 messages because otherwise ViconDataPublisher would override the data immediately
+ //ros::Subscriber viconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 10, viconCallback);
+ //ROS_INFO_STREAM("[REMOTE CONTORLLER] successfully subscribed to ViconData");
+
+ // Subscribe to the message that triggers Vicon subscribe/unsubscribe actions
+ ros::Subscriber viconSubscribeObjectNameSubscriber = nodeHandle.subscribe("ViconSubscribeObjectName", 1, viconSubscribeObjectNameCallback);
+
+ // Advertise the message topic of the Remote (y,roll,pitch,yaw)
+ remote_xyz_rpy_publisher = nodeHandle.advertise<CrazyflieData>("RemoteData", 1);
+
+ // Subscribe to the message that triggers activates/deactivates remote control mode
+ ros::Subscriber shouldActivateSubscriber = nodeHandle.subscribe("Activate", 1, shouldActivateCallback);
+
+ // Advertise the message topic of the Remote (y,roll,pitch,yaw)
+ remote_control_setpoint_publisher = nodeHandle.advertise<CrazyflieData>("RemoteControlSetpoint", 1);
+
+
+
+
+ // Instantiate the instance variable "debugPublisher" to be a "ros::Publisher" that
+ // advertises under the name "DebugTopic" and is a message with the structure
+ // defined in the file "DebugMsg.msg" (located in the "msg" folder).
+ debugPublisher = nodeHandle.advertise<DebugMsg>("DebugTopic", 1);
+
+ // Instantiate the local variable "setpointSubscriber" to be a "ros::Subscriber"
+ // type variable that subscribes to the "Setpoint" topic and calls the class function
+ // "setpointCallback" each time a messaged is received on this topic and the message
+ // is passed as an input argument to the "setpointCallback" class function.
+ //ros::Subscriber setpointSubscriber = nodeHandle.subscribe("Setpoint", 1, setpointCallback);
+
+ // Instantiate the local variable "service" to be a "ros::ServiceServer" type
+ // variable that advertises the service called "CustomController". This service has
+ // the input-output behaviour defined in the "Controller.srv" file (located in the
+ // "srv" folder). This service, when called, is provided with the most recent
+ // measurement of the Crazyflie and is expected to respond with the control action
+ // that should be sent via the Crazyradio and requested from the Crazyflie, i.e.,
+ // this is where the "outer loop" controller function starts. When a request is made
+ // of this service the "calculateControlOutput" function is called.
+ ros::ServiceServer service = nodeHandle.advertiseService("RemoteController", calculateControlOutput);
+
+ // Create a "ros::NodeHandle" type local variable "namespace_nodeHandle" that points
+ // to the name space of this node, i.e., "d_fall_pps" as specified by the line:
+ // "using namespace d_fall_pps;"
+ // in the "DEFINES" section at the top of this file.
+ ros::NodeHandle namespace_nodeHandle(ros::this_node::getNamespace());
+
+ // Print out some information to the user.
+ ROS_INFO("[REMOTE CONTROLLER] Service ready :-)");
+
+ // Enter an endless while loop to keep the node alive.
+ ros::spin();
+
+ // Return zero if the "ross::spin" is cancelled.
+ return 0;
+}
diff --git a/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp b/pps_ws/src/d_fall_pps/src/nodes/SafeControllerService.cpp
similarity index 73%
rename from pps_ws/src/d_fall_pps/src/SafeControllerService.cpp
rename to pps_ws/src/d_fall_pps/src/nodes/SafeControllerService.cpp
index 5e78c75b09e5829e1adbca8fe6022c7c2daa193c..6607ee9a43ae794b655342bea64395021b1df61d 100755
--- a/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/SafeControllerService.cpp
@@ -32,158 +32,14 @@
-// ----------------------------------------------------------------------------------
-// III N N CCCC L U U DDDD EEEEE SSSS
-// I NN N C L U U D D E S
-// I N N N C L U U D D EEE SSS
-// I N NN C L U U D D E S
-// III N N CCCC LLLLL UUU DDDD EEEEE SSSS
-// ----------------------------------------------------------------------------------
-
-// These various headers need to be included so that this controller service can be
-// connected with the D-FaLL system.
-
-#include <math.h>
-#include <stdlib.h>
-#include "ros/ros.h"
-#include <std_msgs/String.h>
-#include <ros/package.h>
-#include "std_msgs/Float32.h"
-
-#include "d_fall_pps/CrazyflieData.h"
-#include "d_fall_pps/Setpoint.h"
-#include "d_fall_pps/ControlCommand.h"
-#include "d_fall_pps/Controller.h"
-
-#include <std_msgs/Int32.h>
-
-
-
-
-// ----------------------------------------------------------------------------------
-// DDDD EEEEE FFFFF III N N EEEEE SSSS
-// D D E F I NN N E S
-// D D EEE FFF I N N N EEE SSS
-// D D E F I N NN E S
-// DDDD EEEEE F III N N EEEEE SSSS
-// ----------------------------------------------------------------------------------
-
-// These constants are defined to make the code more readable and adaptable.
-
-// Universal constants
-#define PI 3.1415926535
-
-// The constants define the modes that can be used for controller the Crazyflie 2.0,
-// the constants defined here need to be in agreement with those defined in the
-// firmware running on the Crazyflie 2.0.
-// The following is a short description about each mode:
-// MOTOR_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
-// command directly to each of the motors
-// RATE_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
-// command directly to each of the motors, and additionally request the
-// body frame roll, pitch, and yaw angular rates from the PID rate
-// controllers implemented in the Crazyflie 2.0 firmware.
-// ANGE_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
-// command directly to each of the motors, and additionally request the
-// body frame roll, pitch, and yaw angles from the PID attitude
-// controllers implemented in the Crazyflie 2.0 firmware.
-#define MOTOR_MODE 6
-#define RATE_MODE 7
-#define ANGLE_MODE 8
-// Constants for feching the yaml paramters
-#define FETCH_YAML_SAFE_CONTROLLER_AGENT 1
-#define FETCH_YAML_CUSTOM_CONTROLLER_AGENT 2
-#define FETCH_YAML_SAFE_CONTROLLER_COORDINATOR 3
-#define FETCH_YAML_CUSTOM_CONTROLLER_COORDINATOR 4
+// INCLUDE THE HEADER
+#include "nodes/SafeControllerService.h"
-// Namespacing the package
-using namespace d_fall_pps;
-// ----------------------------------------------------------------------------------
-// V V A RRRR III A BBBB L EEEEE SSSS
-// V V A A R R I A A B B L E S
-// V V A A RRRR I A A BBBB L EEE SSS
-// V V AAAAA R R I AAAAA B B L E S
-// V A A R R III A A BBBB LLLLL EEEEE SSSS
-// ----------------------------------------------------------------------------------
-
-std::vector<float> ffThrust(4);
-std::vector<float> feedforwardMotor(4);
-float cf_mass;
-float gravity_force;
-std::vector<float> motorPoly(3);
-
-std::vector<float> gainMatrixRoll(9);
-std::vector<float> gainMatrixPitch(9);
-std::vector<float> gainMatrixYaw(9);
-std::vector<float> gainMatrixThrust(9);
-
-//K_infinite of feedback
-std::vector<float> filterGain(6);
-//only for velocity calculation
-std::vector<float> estimatorMatrix(2);
-float prevEstimate[9];
-
-std::vector<float> setpoint(4);
-std::vector<float> defaultSetpoint(4);
-float saturationThrust;
-
-CrazyflieData previousLocation;
-
-
-// Variable for the namespaces for the paramter services
-// > For the paramter service of this agent
-std::string namespace_to_own_agent_parameter_service;
-// > For the parameter service of the coordinator
-std::string namespace_to_coordinator_parameter_service;
-
-
-
-// ----------------------------------------------------------------------------------
-// FFFFF U U N N CCCC TTTTT III OOO N N
-// F U U NN N C T I O O NN N
-// FFF U U N N N C T I O O N N N
-// F U U N NN C T I O O N NN
-// F UUU N N CCCC T III OOO N N
-//
-// PPPP RRRR OOO TTTTT OOO TTTTT Y Y PPPP EEEEE SSSS
-// P P R R O O T O O T Y Y P P E S
-// PPPP RRRR O O T O O T Y PPPP EEE SSS
-// P R R O O T O O T Y P E S
-// P R R OOO T OOO T Y P EEEEE SSSS
-// ----------------------------------------------------------------------------------
-
-// These function prototypes are not strictly required for this code to complile, but
-// adding the function prototypes here means the the functions can be written below in
-// any order. If the function prototypes are not included then the function need to
-// written below in an order that ensure each function is implemented before it is
-// called from another function, hence why the "main" function is at the bottom.
-
-// > For the CONTROL LOOP
-bool calculateControlOutput(Controller::Request &request, Controller::Response &response);
-void convertIntoBodyFrame(float est[9], float (&state)[9], float yaw_measured);
-float computeMotorPolyBackward(float thrust);
-void estimateState(Controller::Request &request, float (&est)[9]);
-
-// > For the LOAD PARAMETERS
-void yamlReadyForFetchCallback(const std_msgs::Int32& msg);
-void fetchYamlParameters(ros::NodeHandle& nodeHandle);
-void processFetchedParameters();
-
-// > For the GETPARAM()
-float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name);
-void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length);
-int getParameterInt(ros::NodeHandle& nodeHandle, std::string name);
-void getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length);
-int getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val);
-bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name);
-
-
-
// ----------------------------------------------------------------------------------
// FFFFF U U N N CCCC TTTTT III OOO N N
// F U U NN N C T I O O NN N
@@ -265,6 +121,11 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
//INFORMATION: this ugly fix was needed for the older firmware
//outYaw *= 0.5;
+ //ROS_INFO_STREAM("y-error = " << state[1]);
+ //ROS_INFO_STREAM("y-velocity = " << state[4]);
+ //ROS_INFO_STREAM("roll = " << state[6]);
+ //ROS_INFO_STREAM("rollRate = " << outRoll );
+
response.controlOutput.roll = outRoll;
response.controlOutput.pitch = outPitch;
response.controlOutput.yaw = outYaw;
@@ -383,10 +244,10 @@ void yamlReadyForFetchCallback(const std_msgs::Int32& msg)
{
// > FOR FETCHING FROM THE AGENT'S OWN PARAMETER SERVICE
- case FETCH_YAML_SAFE_CONTROLLER_AGENT:
+ case FETCH_YAML_SAFE_CONTROLLER_FROM_OWN_AGENT:
{
// Let the user know that this message was received
- ROS_INFO("The SafeControllerService received the message that YAML parameters were (re-)loaded. > Now fetching the parameter values from this machine.");
+ ROS_INFO("[SAFE CONTROLLER] Received the message that YAML parameters were (re-)loaded. > Now fetching the parameter values from this machine.");
// Create a node handle to the parameter service running on this agent's machine
ros::NodeHandle nodeHandle_to_own_agent_parameter_service(namespace_to_own_agent_parameter_service);
// Call the function that fetches the parameters
@@ -395,10 +256,10 @@ void yamlReadyForFetchCallback(const std_msgs::Int32& msg)
}
// > FOR FETCHING FROM THE COORDINATOR'S PARAMETER SERVICE
- case FETCH_YAML_SAFE_CONTROLLER_COORDINATOR:
+ case FETCH_YAML_SAFE_CONTROLLER_FROM_COORDINATOR:
{
// Let the user know that this message was received
- ROS_INFO("The SafeControllerService received the message that YAML parameters were (re-)loaded. > Now fetching the parameter values from the coordinator.");
+ ROS_INFO("[SAFE CONTROLLER] Received the message that YAML parameters were (re-)loaded. > Now fetching the parameter values from the coordinator.");
// Create a node handle to the parameter service running on the coordinator machine
ros::NodeHandle nodeHandle_to_coordinator_parameter_service(namespace_to_coordinator_parameter_service);
// Call the function that fetches the parameters
@@ -448,7 +309,7 @@ void fetchYamlParameters(ros::NodeHandle& nodeHandle)
getParameterFloatVector(nodeHandle_for_safeController, "defaultSetpoint", defaultSetpoint, 4);
// DEBUGGING: Print out one of the parameters that was loaded
- ROS_INFO_STREAM("DEBUGGING: the fetched SafeController/mass = " << cf_mass);
+ ROS_INFO_STREAM("[SAFE CONTROLLER] DEBUGGING: the fetched SafeController/mass = " << cf_mass);
// Call the function that computes details an values that are needed from these
// parameters loaded above
@@ -626,16 +487,46 @@ int main(int argc, char* argv[]) {
// PRINT OUT SOME INFORMATION
// Let the user know what namespaces are being used for linking to the parameter service
- ROS_INFO_STREAM("The namespace string for accessing the Paramter Services are:");
- ROS_INFO_STREAM("namespace_to_own_agent_parameter_service = " << namespace_to_own_agent_parameter_service);
- ROS_INFO_STREAM("namespace_to_coordinator_parameter_service = " << namespace_to_coordinator_parameter_service);
+ ROS_INFO_STREAM("[SAFE CONTROLLER] The namespace string for accessing the Paramter Services are:");
+ ROS_INFO_STREAM("[SAFE CONTROLLER] namespace_to_own_agent_parameter_service = " << namespace_to_own_agent_parameter_service);
+ ROS_INFO_STREAM("[SAFE CONTROLLER] namespace_to_coordinator_parameter_service = " << namespace_to_coordinator_parameter_service);
// FINALLY, FETCH ANY PARAMETERS REQUIRED FROM THESE "PARAMETER SERVICES"
+ // Call the class function that loads the parameters for this class.
+
+ // Sleep for some time (in seconds)
+ // ros::Duration(1.0).sleep();
+
+ // // Ask the Paramter Service to load the respective YAML file
+ // std::string namespace_to_own_agent_loadYamlFiles_service = namespace_to_own_agent_parameter_service + "/LoadYamlFiles";
+ // loadYamlFilesService_own_agent = ros::service::createClient<LoadYamlFiles>(namespace_to_own_agent_loadYamlFiles_service, true);
+ // ROS_INFO_STREAM("[SAFE CONTROLLER] Loaded service: " << loadYamlFilesService_own_agent.getService());
+
+ // LoadYamlFiles loadYamlFilesService;
+ // std::vector<std::string> yamlFileNames_to_load = {"SafeController"};
+ // loadYamlFilesService.request.yamlFileNames = yamlFileNames_to_load;
+ // bool success = loadYamlFilesService_own_agent.call(loadYamlFilesService);
+
+ // ROS_INFO_STREAM("[SAFE CONTROLLER] called Laod Yaml File service with success = " << success);
+
+ // ros::Duration(2.0).sleep();
+
+
// Call the class function that loads the parameters for this class.
fetchYamlParameters(nodeHandle_to_own_agent_parameter_service);
+
+ // DEBUGGING
+ // Add the "CustomController" namespace to the "nodeHandle"
+ //ros::NodeHandle nodeHandle_for_safeController(nodeHandle, "SafeController");
+ // > The mass of the crazyflie
+ //float temp_mass = getParameterFloat(nodeHandle_for_safeController, "mass");
+ //ROS_INFO_STREAM("[SAFE CONTROLLER] DEBUGGING: the fetched SafeController/mass = " << temp_mass);
+
+
+
// *********************************************************************************
@@ -647,7 +538,7 @@ int main(int argc, char* argv[]) {
ros::ServiceServer service = nodeHandle.advertiseService("RateController", calculateControlOutput);
- ROS_INFO("SafeControllerService ready");
+ ROS_INFO("[SAFE CONTROLLER] Service ready :-)");
// std::string package_path;
// package_path = ros::package::getPath("d_fall_pps") + "/";
diff --git a/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp b/pps_ws/src/d_fall_pps/src/nodes/StudentControllerService.cpp
similarity index 77%
rename from pps_ws/src/d_fall_pps/src/CustomControllerService.cpp
rename to pps_ws/src/d_fall_pps/src/nodes/StudentControllerService.cpp
index 194d25e546e82294be4dc8f26abf274860469eb2..a9eec98a20b1c8f8d5910b0e77956b10cbbc2d1e 100644
--- a/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/StudentControllerService.cpp
@@ -33,196 +33,10 @@
-// ----------------------------------------------------------------------------------
-// III N N CCCC L U U DDDD EEEEE SSSS
-// I NN N C L U U D D E S
-// I N N N C L U U D D EEE SSS
-// I N NN C L U U D D E S
-// III N N CCCC LLLLL UUU DDDD EEEEE SSSS
-// ----------------------------------------------------------------------------------
-
-// These various headers need to be included so that this controller service can be
-// connected with the D-FaLL system.
-
-//some useful libraries
-#include <math.h>
-#include <stdlib.h>
-#include "ros/ros.h"
-#include <ros/package.h>
-
-//the generated structs from the msg-files have to be included
-#include "d_fall_pps/ViconData.h"
-#include "d_fall_pps/Setpoint.h"
-#include "d_fall_pps/ControlCommand.h"
-#include "d_fall_pps/Controller.h"
-#include "d_fall_pps/DebugMsg.h"
-#include "d_fall_pps/CustomControllerYAML.h"
-
-#include <std_msgs/Int32.h>
-
-
-
-
-
-// ----------------------------------------------------------------------------------
-// DDDD EEEEE FFFFF III N N EEEEE SSSS
-// D D E F I NN N E S
-// D D EEE FFF I N N N EEE SSS
-// D D E F I N NN E S
-// DDDD EEEEE F III N N EEEEE SSSS
-// ----------------------------------------------------------------------------------
-
-// These constants are defined to make the code more readable and adaptable.
-
-// Universal constants
-#define PI 3.1415926535
-
-// These constants define the modes that can be used for controller the Crazyflie 2.0,
-// the constants defined here need to be in agreement with those defined in the
-// firmware running on the Crazyflie 2.0.
-// The following is a short description about each mode:
-// MOTOR_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
-// command directly to each of the motors
-// RATE_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
-// command directly to each of the motors, and additionally request the
-// body frame roll, pitch, and yaw angular rates from the PID rate
-// controllers implemented in the Crazyflie 2.0 firmware.
-// ANGE_MODE In this mode the Crazyflie will apply the requested 16-bit per motor
-// command directly to each of the motors, and additionally request the
-// body frame roll, pitch, and yaw angles from the PID attitude
-// controllers implemented in the Crazyflie 2.0 firmware.
-#define MOTOR_MODE 6
-#define RATE_MODE 7
-#define ANGLE_MODE 8
-
-// These constants define the controller used for computing the response in the
-// "calculateControlOutput" function
-// The following is a short description about each mode:
-// LQR_RATE_MODE LQR controller based on the state vector:
-// [position,velocity,angles]
-//
-// LQR_ANGLE_MODE LQR controller based on the state vector:
-// [position,velocity]
-//
-#define LQR_RATE_MODE 1 // (DEFAULT)
-#define LQR_ANGLE_MODE 2
-
-// Constants for feching the yaml paramters
-#define FETCH_YAML_SAFE_CONTROLLER_AGENT 1
-#define FETCH_YAML_CUSTOM_CONTROLLER_AGENT 2
-#define FETCH_YAML_SAFE_CONTROLLER_COORDINATOR 3
-#define FETCH_YAML_CUSTOM_CONTROLLER_COORDINATOR 4
-
-// Namespacing the package
-using namespace d_fall_pps;
-
-
-
-
-
-// ----------------------------------------------------------------------------------
-// V V A RRRR III A BBBB L EEEEE SSSS
-// V V A A R R I A A B B L E S
-// V V A A RRRR I A A BBBB L EEE SSS
-// V V AAAAA R R I AAAAA B B L E S
-// V A A R R III A A BBBB LLLLL EEEEE SSSS
-// ----------------------------------------------------------------------------------
-
-// Variables for controller
-float cf_mass; // Crazyflie mass in grams
-std::vector<float> motorPoly(3); // Coefficients of the 16-bit command to thrust conversion
-float control_frequency; // Frequency at which the controller is running
-float gravity_force; // The weight of the Crazyflie in Newtons, i.e., mg
-
-float previous_stateErrorInertial[9]; // The location error of the Crazyflie at the "previous" time step
-
-std::vector<float> setpoint{0.0,0.0,0.4,0.0}; // The setpoints for (x,y,z) position and yaw angle, in that order
+// INCLUDE THE HEADER
+#include "nodes/StudentControllerService.h"
-// The LQR Controller parameters for "LQR_RATE_MODE"
-const float gainMatrixRollRate[9] = { 0.00,-1.72, 0.00, 0.00,-1.34, 0.00, 5.12, 0.00, 0.00};
-const float gainMatrixPitchRate[9] = { 1.72, 0.00, 0.00, 1.34, 0.00, 0.00, 0.00, 5.12, 0.00};
-const float gainMatrixYawRate[9] = { 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.84};
-const float gainMatrixThrust[9] = { 0.00, 0.00, 0.25, 0.00, 0.00, 0.14, 0.00, 0.00, 0.00};
-
-
-// ROS Publisher for debugging variables
-ros::Publisher debugPublisher;
-
-
-// Variable for the namespaces for the paramter services
-// > For the paramter service of this agent
-std::string namespace_to_own_agent_parameter_service;
-// > For the parameter service of the coordinator
-std::string namespace_to_coordinator_parameter_service;
-
-// The ID of this agent, i.e., the ID of this compute
-int my_agentID = 0;
-
-
-// RELEVANT NOTES ABOUT THE VARIABLES DECLARE HERE:
-// The "CrazyflieData" type used for the "request" variable is a
-// structure as defined in the file "CrazyflieData.msg" which has the following
-// properties:
-// string crazyflieName The name given to the Crazyflie in the Vicon software
-// float64 x The x position of the Crazyflie [metres]
-// float64 y The y position of the Crazyflie [metres]
-// float64 z The z position of the Crazyflie [metres]
-// float64 roll The roll component of the intrinsic Euler angles [radians]
-// float64 pitch The pitch component of the intrinsic Euler angles [radians]
-// float64 yaw The yaw component of the intrinsic Euler angles [radians]
-// float64 acquiringTime #delta t The time elapsed since the previous "CrazyflieData" was received [seconds]
-// bool occluded A boolean indicted whether the Crazyflie for visible at the time of this measurement
-
-
-
-
-
-// ----------------------------------------------------------------------------------
-// FFFFF U U N N CCCC TTTTT III OOO N N
-// F U U NN N C T I O O NN N
-// FFF U U N N N C T I O O N N N
-// F U U N NN C T I O O N NN
-// F UUU N N CCCC T III OOO N N
-//
-// PPPP RRRR OOO TTTTT OOO TTTTT Y Y PPPP EEEEE SSSS
-// P P R R O O T O O T Y Y P P E S
-// PPPP RRRR O O T O O T Y PPPP EEE SSS
-// P R R O O T O O T Y P E S
-// P R R OOO T OOO T Y P EEEEE SSSS
-// ----------------------------------------------------------------------------------
-
-// These function prototypes are not strictly required for this code to complile, but
-// adding the function prototypes here means the the functions can be written below in
-// any order. If the function prototypes are not included then the function need to
-// written below in an order that ensure each function is implemented before it is
-// called from another function, hence why the "main" function is at the bottom.
-
-// CONTROLLER COMPUTATIONS
-bool calculateControlOutput(Controller::Request &request, Controller::Response &response);
-
-// TRANSFORMATION OF THE (x,y) INERTIAL FRAME ERROR INTO AN (x,y) BODY FRAME ERROR
-void convertIntoBodyFrame(float stateInertial[9], float (&stateBody)[9], float yaw_measured);
-
-// CONVERSION FROM THRUST IN NEWTONS TO 16-BIT COMMAND
-float computeMotorPolyBackward(float thrust);
-
-// SETPOINT CHANGE CALLBACK
-void setpointCallback(const Setpoint& newSetpoint);
-
-// LOAD PARAMETERS
-float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name);
-void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length);
-int getParameterInt(ros::NodeHandle& nodeHandle, std::string name);
-void getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length);
-int getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val);
-bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name);
-
-void yamlReadyForFetchCallback(const std_msgs::Int32& msg);
-void fetchYamlParameters(ros::NodeHandle& nodeHandle);
-void processFetchedParameters();
-//void customYAMLasMessageCallback(const CustomControllerYAML& newCustomControllerParameters);
-
@@ -259,7 +73,7 @@ void processFetchedParameters();
// CCCC OOO N N T R R OOO LLLLL LLLLL OOO OOO P
// ----------------------------------------------------------------------------------
-// This function is the callback that is linked to the "CustomController" service that
+// This function is the callback that is linked to the "StudentController" service that
// is advertised in the main function. This must have arguments that match the
// "input-output" behaviour defined in the "Controller.srv" file (located in the "srv"
// folder)
@@ -462,8 +276,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + gravity_force);
response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + gravity_force);
-
-
+
// **************************************
// BBBB OOO DDDD Y Y X X
@@ -512,9 +325,8 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// Put the computed roll rate into the "response" variable
response.controlOutput.roll = rollRate_forResponse;
-
-
-
+
+
// PREPARE AND RETURN THE VARIABLE "response"
/*choosing the Crazyflie onBoard controller type.
@@ -712,6 +524,75 @@ void setpointCallback(const Setpoint& newSetpoint)
+
+
+// ----------------------------------------------------------------------------------
+// CCCC U U SSSS TTTTT OOO M M
+// C U U S T O O MM MM
+// C U U SSS T O O M M M
+// C U U S T O O M M
+// CCCC UUU SSSS T OOO M M
+//
+// CCCC OOO M M M M A N N DDDD
+// C O O MM MM MM MM A A NN N D D
+// C O O M M M M M M A A N N N D D
+// C O O M M M M AAAAA N NN D D
+// CCCC OOO M M M M A A N N DDDD
+// ----------------------------------------------------------------------------------
+
+// CUSTOM COMMAND RECEIVED CALLBACK
+void customCommandReceivedCallback(const CustomButton& commandReceived)
+{
+ // Extract the data from the message
+ int custom_button_index = commandReceived.button_index;
+ float custom_command_code = commandReceived.command_code;
+
+ // Switch between the button pressed
+ switch(custom_button_index)
+ {
+
+ // > FOR CUSTOM BUTTON 1
+ case 1:
+ {
+ // Let the user know that this part of the code was triggered
+ ROS_INFO("[STUDENT CONTROLLER] Button 1 received in controller.");
+ // Code here to respond to custom button 1
+
+ break;
+ }
+
+ // > FOR CUSTOM BUTTON 2
+ case 2:
+ {
+ // Let the user know that this part of the code was triggered
+ ROS_INFO("[STUDENT CONTROLLER] Button 2 received in controller.");
+ // Code here to respond to custom button 2
+
+ break;
+ }
+
+ // > FOR CUSTOM BUTTON 3
+ case 3:
+ {
+ // Let the user know that this part of the code was triggered
+ ROS_INFO_STREAM("[STUDENT CONTROLLER] Button 3 received in controller, with command code:" << custom_command_code );
+ // Code here to respond to custom button 3
+
+ break;
+ }
+
+ default:
+ {
+ // Let the user know that the command was not recognised
+ ROS_INFO_STREAM("[DEMO CONTROLLER] A button clicked command was received in the controller but not recognised, message.button_index = " << custom_button_index << ", and message.command_code = " << custom_command_code );
+ break;
+ }
+ }
+}
+
+
+
+
// ----------------------------------------------------------------------------------
// L OOO A DDDD
// L O O A A D D
@@ -739,10 +620,10 @@ void yamlReadyForFetchCallback(const std_msgs::Int32& msg)
{
// > FOR FETCHING FROM THE AGENT'S OWN PARAMETER SERVICE
- case FETCH_YAML_CUSTOM_CONTROLLER_AGENT:
+ case FETCH_YAML_STUDENT_CONTROLLER_FROM_OWN_AGENT:
{
// Let the user know that this message was received
- ROS_INFO("The CustomControllerService received the message that YAML parameters were (re-)loaded. > Now fetching the parameter values from this agent.");
+ ROS_INFO("[STUDENT CONTROLLER] Received the message that YAML parameters were (re-)loaded. > Now fetching the parameter values from this agent.");
// Create a node handle to the parameter service running on this agent's machine
ros::NodeHandle nodeHandle_to_own_agent_parameter_service(namespace_to_own_agent_parameter_service);
// Call the function that fetches the parameters
@@ -751,10 +632,10 @@ void yamlReadyForFetchCallback(const std_msgs::Int32& msg)
}
// > FOR FETCHING FROM THE COORDINATOR'S PARAMETER SERVICE
- case FETCH_YAML_CUSTOM_CONTROLLER_COORDINATOR:
+ case FETCH_YAML_STUDENT_CONTROLLER_FROM_COORDINATOR:
{
// Let the user know that this message was received
- ROS_INFO("The CustomControllerService received the message that YAML parameters were (re-)loaded. > Now fetching the parameter values from the coordinator.");
+ ROS_INFO("[STUDENT CONTROLLER] Received the message that YAML parameters were (re-)loaded. > Now fetching the parameter values from the coordinator.");
// Create a node handle to the parameter service running on the coordinator machine
ros::NodeHandle nodeHandle_to_coordinator_parameter_service(namespace_to_coordinator_parameter_service);
// Call the function that fetches the parameters
@@ -765,7 +646,7 @@ void yamlReadyForFetchCallback(const std_msgs::Int32& msg)
default:
{
// Let the user know that the command was not relevant
- //ROS_INFO("The CustomControllerService received the message that YAML parameters were (re-)loaded");
+ //ROS_INFO("The StudentControllerService received the message that YAML parameters were (re-)loaded");
//ROS_INFO("> However the parameters do not relate to this controller, hence nothing will be fetched.");
break;
}
@@ -778,28 +659,28 @@ void yamlReadyForFetchCallback(const std_msgs::Int32& msg)
// your controller easier and quicker.
void fetchYamlParameters(ros::NodeHandle& nodeHandle)
{
- // Here we load the parameters that are specified in the CustomController.yaml file
+ // Here we load the parameters that are specified in the StudentController.yaml file
- // Add the "CustomController" namespace to the "nodeHandle"
- ros::NodeHandle nodeHandle_for_customController(nodeHandle, "CustomController");
+ // Add the "StudentController" namespace to the "nodeHandle"
+ ros::NodeHandle nodeHandle_for_studentController(nodeHandle, "StudentController");
// > The mass of the crazyflie
- cf_mass = getParameterFloat(nodeHandle_for_customController , "mass");
+ cf_mass = getParameterFloat(nodeHandle_for_studentController , "mass");
// Display one of the YAML parameters to debug if it is working correctly
//ROS_INFO_STREAM("DEBUGGING: mass leaded from loacl file = " << cf_mass );
// > The frequency at which the "computeControlOutput" is being called, as determined
// by the frequency at which the Vicon system provides position and attitude data
- control_frequency = getParameterFloat(nodeHandle_for_customController, "control_frequency");
+ control_frequency = getParameterFloat(nodeHandle_for_studentController, "control_frequency");
// > The co-efficients of the quadratic conversation from 16-bit motor command to
// thrust force in Newtons
- getParameterFloatVector(nodeHandle_for_customController, "motorPoly", motorPoly, 3);
+ getParameterFloatVector(nodeHandle_for_studentController, "motorPoly", motorPoly, 3);
// DEBUGGING: Print out one of the parameters that was loaded
- ROS_INFO_STREAM("DEBUGGING: the fetched CustomController/mass = " << cf_mass);
+ ROS_INFO_STREAM("[STUDENT CONTROLLER] DEBUGGING: the fetched StudentController/mass = " << cf_mass);
// Call the function that computes details an values that are needed from these
// parameters loaded above
@@ -818,7 +699,7 @@ void processFetchedParameters()
gravity_force = cf_mass * 9.81/(1000*4);
// DEBUGGING: Print out one of the computed quantities
- ROS_INFO_STREAM("DEBUGGING: thus the graity force = " << gravity_force);
+ ROS_INFO_STREAM("[STUDENT CONTROLLER] DEBUGGING: thus the graity force = " << gravity_force);
}
@@ -908,12 +789,16 @@ bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name)
int main(int argc, char* argv[]) {
// Starting the ROS-node
- ros::init(argc, argv, "CustomControllerService");
+ ros::init(argc, argv, "StudentControllerService");
// Create a "ros::NodeHandle" type local variable "nodeHandle" as the current node,
// the "~" indcates that "self" is the node handle assigned to this variable.
ros::NodeHandle nodeHandle("~");
+ // Get the namespace of this "StudentControllerService" node
+ std::string m_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[STUDENT CONTROLLER] ros::this_node::getNamespace() = " << m_namespace);
+
// Get the agent ID as the "ROS_NAMESPACE" this computer.
// NOTES:
// > If you look at the "Student.launch" file in the "launch" folder, you will see
@@ -922,15 +807,13 @@ int main(int argc, char* argv[]) {
// This line of code adds a parameter named "studentID" to the "PPSClient"
// > Thus, to get access to this "studentID" paremeter, we first need to get a handle
// to the "PPSClient" node within which this controller service is nested.
- // Get the namespace of this "CustomControllerService" node
- std::string m_namespace = ros::this_node::getNamespace();
// Get the handle to the "PPSClient" node
ros::NodeHandle PPSClient_nodeHandle(m_namespace + "/PPSClient");
// Get the value of the "studentID" parameter into the instance variable "my_agentID"
- if(!PPSClient_nodeHandle.getParam("studentID", my_agentID))
+ if(!PPSClient_nodeHandle.getParam("agentID", my_agentID))
{
// Throw an error if the student ID parameter could not be obtained
- ROS_ERROR("Failed to get studentID from FollowN_1Service");
+ ROS_ERROR("[STUDENT CONTROLLER] Failed to get agentID from PPSClient");
}
@@ -981,9 +864,9 @@ int main(int argc, char* argv[]) {
// PRINT OUT SOME INFORMATION
// Let the user know what namespaces are being used for linking to the parameter service
- ROS_INFO_STREAM("The namespace string for accessing the Paramter Services are:");
- ROS_INFO_STREAM("namespace_to_own_agent_parameter_service = " << namespace_to_own_agent_parameter_service);
- ROS_INFO_STREAM("namespace_to_coordinator_parameter_service = " << namespace_to_coordinator_parameter_service);
+ ROS_INFO_STREAM("[STUDENT CONTROLLER] The namespace string for accessing the Paramter Services are:");
+ ROS_INFO_STREAM("[STUDENT CONTROLLER] namespace_to_own_agent_parameter_service = " << namespace_to_own_agent_parameter_service);
+ ROS_INFO_STREAM("[STUDENT CONTROLLER] namespace_to_coordinator_parameter_service = " << namespace_to_coordinator_parameter_service);
// FINALLY, FETCH ANY PARAMETERS REQUIRED FROM THESE "PARAMETER SERVICES"
@@ -1007,14 +890,20 @@ int main(int argc, char* argv[]) {
ros::Subscriber setpointSubscriber = nodeHandle.subscribe("Setpoint", 1, setpointCallback);
// Instantiate the local variable "service" to be a "ros::ServiceServer" type
- // variable that advertises the service called "CustomController". This service has
+ // variable that advertises the service called "StudentController". This service has
// the input-output behaviour defined in the "Controller.srv" file (located in the
// "srv" folder). This service, when called, is provided with the most recent
// measurement of the Crazyflie and is expected to respond with the control action
// that should be sent via the Crazyradio and requested from the Crazyflie, i.e.,
// this is where the "outer loop" controller function starts. When a request is made
// of this service the "calculateControlOutput" function is called.
- ros::ServiceServer service = nodeHandle.advertiseService("CustomController", calculateControlOutput);
+ ros::ServiceServer service = nodeHandle.advertiseService("StudentController", calculateControlOutput);
+
+ // Instantiate the local variable "customCommandSubscriber" to be a "ros::Subscriber"
+ // type variable that subscribes to the "GUIButton" topic and calls the class
+ // function "customCommandReceivedCallback" each time a messaged is received on this topic
+ // and the message received is passed as an input argument to the callback function.
+ ros::Subscriber customCommandReceivedSubscriber = nodeHandle.subscribe("GUIButton", 1, customCommandReceivedCallback);
// Create a "ros::NodeHandle" type local variable "namespace_nodeHandle" that points
// to the name space of this node, i.e., "d_fall_pps" as specified by the line:
@@ -1023,7 +912,7 @@ int main(int argc, char* argv[]) {
ros::NodeHandle namespace_nodeHandle(ros::this_node::getNamespace());
// Print out some information to the user.
- ROS_INFO("CustomControllerService ready");
+ ROS_INFO("[STUDENT CONTROLLER] Service ready :-)");
// Enter an endless while loop to keep the node alive.
ros::spin();
diff --git a/pps_ws/src/d_fall_pps/src/nodes/TuningControllerService.cpp b/pps_ws/src/d_fall_pps/src/nodes/TuningControllerService.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..09bf7db145ee1701a7805768084d7f8f80c29db3
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/src/nodes/TuningControllerService.cpp
@@ -0,0 +1,1734 @@
+// Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat
+//
+// 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:
+// Place for students to implement their controller
+//
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// INCLUDE THE HEADER
+#include "nodes/TuningControllerService.h"
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// FFFFF U U N N CCCC TTTTT III OOO N N
+// F U U NN N C T I O O NN N
+// FFF U U N N N C T I O O N N N
+// F U U N NN C T I O O N NN
+// F UUU N N CCCC T III OOO N N
+//
+// III M M PPPP L EEEEE M M EEEEE N N TTTTT A TTTTT III OOO N N
+// I MM MM P P L E MM MM E NN N T A A T I O O NN N
+// I M M M PPPP L EEE M M M EEE N N N T A A T I O O N N N
+// I M M P L E M M E N NN T AAAAA T I O O N NN
+// III M M P LLLLL EEEEE M M EEEEE N N T A A T III OOO N N
+// ----------------------------------------------------------------------------------
+
+
+
+
+
+// ------------------------------------------------------------------------------
+// OOO U U TTTTT EEEEE RRRR
+// O O U U T E R R
+// O O U U T EEE RRRR
+// O O U U T E R R
+// OOO UUU T EEEEE R R
+//
+// CCCC OOO N N TTTTT RRRR OOO L L OOO OOO PPPP
+// C O O NN N T R R O O L L O O O O P P
+// C O O N N N T RRRR O O L L O O O O PPPP
+// C O O N NN T R R O O L L O O O O P
+// CCCC OOO N N T R R OOO LLLLL LLLLL OOO OOO P
+// ----------------------------------------------------------------------------------
+
+// This function is the callback that is linked to the "CustomController" service that
+// is advertised in the main function. This must have arguments that match the
+// "input-output" behaviour defined in the "Controller.srv" file (located in the "srv"
+// folder)
+//
+// The arument "request" is a structure provided to this service with the following two
+// properties:
+// request.ownCrazyflie
+// This property is itself a structure of type "CrazyflieData", which is defined in the
+// file "CrazyflieData.msg", and has the following properties
+// string crazyflieName
+// float64 x The x position of the Crazyflie [metres]
+// float64 y The y position of the Crazyflie [metres]
+// float64 z The z position of the Crazyflie [metres]
+// float64 roll The roll component of the intrinsic Euler angles [radians]
+// float64 pitch The pitch component of the intrinsic Euler angles [radians]
+// float64 yaw The yaw component of the intrinsic Euler angles [radians]
+// float64 acquiringTime #delta t The time elapsed since the previous "CrazyflieData" was received [seconds]
+// bool occluded A boolean indicted whether the Crazyflie for visible at the time of this measurement
+// The values in these properties are directly the measurement taken by the Vicon
+// motion capture system of the Crazyflie that is to be controlled by this service
+//
+// request.otherCrazyflies
+// This property is an array of "CrazyflieData" structures, what allows access to the
+// Vicon measurements of other Crazyflies.
+//
+// The argument "response" is a structure that is expected to be filled in by this
+// service by this function, it has only the following property
+// response.ControlCommand
+// This property is iteself a structure of type "ControlCommand", which is defined in
+// the file "ControlCommand.msg", and has the following properties:
+// float32 roll The command sent to the Crazyflie for the body frame x-axis
+// float32 pitch The command sent to the Crazyflie for the body frame y-axis
+// float32 yaw The command sent to the Crazyflie for the body frame z-axis
+// uint16 motorCmd1 The command sent to the Crazyflie for motor 1
+// uint16 motorCmd2 The command sent to the Crazyflie for motor 1
+// uint16 motorCmd3 The command sent to the Crazyflie for motor 1
+// uint16 motorCmd4 The command sent to the Crazyflie for motor 1
+// uint8 onboardControllerType The flag sent to the Crazyflie for indicating how to implement the command
+//
+// IMPORTANT NOTES FOR "onboardControllerType" AND AXIS CONVENTIONS
+// > The allowed values for "onboardControllerType" are in the "Defines" section at the
+// top of this file, they are MOTOR_MODE, RATE_MODE, OR ANGLE_MODE.
+// > In the PPS exercise we will only use the RATE_MODE.
+// > In RATE_MODE the ".roll", ".ptich", and ".yaw" properties of "response.ControlCommand"
+// specify the angular rate in [radians/second] that will be requested from the
+// PID controllers running in the Crazyflie 2.0 firmware.
+// > In RATE_MODE the ".motorCmd1" to ".motorCmd4" properties of "response.ControlCommand"
+// are the baseline motor commands requested from the Crazyflie, with the adjustment
+// for body rates being added on top of this in the firmware (i.e., as per the code
+// of the "distribute_power" function provided in exercise sheet 2).
+// > In RATE_MODE the axis convention for the roll, pitch, and yaw body rates returned
+// in "response.ControlCommand" should use right-hand coordinate axes with x-forward
+// and z-upwards (i.e., the positive z-axis is aligned with the direction of positive
+// thrust). To assist, teh following is an ASCII art of this convention:
+//
+// ASCII ART OF THE CRAZYFLIE 2.0 LAYOUT
+//
+// > This is a top view,
+// > M1 to M4 stand for Motor 1 to Motor 4,
+// > "CW" indicates that the motor rotates Clockwise,
+// > "CCW" indicates that the motor rotates Counter-Clockwise,
+// > By right-hand axis convention, the positive z-direction points our of the screen,
+// > This being a "top view" means tha the direction of positive thrust produced
+// by the propellers is also out of the screen.
+//
+// ____ ____
+// / \ / \
+// (CW) | M4 | x | M1 | (CCW)
+// \____/\ ^ /\____/
+// \ \ | / /
+// \ \ | / /
+// \ \______ | ______/ /
+// \ | /
+// | | |
+// y <-------------o |
+// | |
+// / _______________ \
+// / / \ \
+// / / \ \
+// ____/ / \ \____
+// / \/ \/ \
+// (CCW) | M3 | | M2 | (CW)
+// \____/ \____/
+//
+//
+//
+// This function WILL NEED TO BE edited for successful completion of the PPS exercise
+
+
+
+bool calculateControlOutput(Controller::Request &request, Controller::Response &response)
+{
+
+ // THIS IS THE START OF THE "OUTER" CONTROL LOOP
+ // > i.e., this is the control loop run on this laptop
+ // > this function is called at the frequency specified
+ // > this function performs all estimation and control
+
+
+ // PERFORM THE ESTIMATOR UPDATE FOR THE INTERIAL FRAME STATE
+ // > After this function is complete the class variable
+ // "current_stateInertialEstimate" is updated and ready
+ // to be used for subsequent controller copmutations
+ performEstimatorUpdate_forStateInterial(request);
+
+
+ // CONVERT THE CURRENT INERTIAL FRAME STATE ESTIMATE, INTO
+ // THE BODY FRAME ERROR REQUIRED BY THE CONTROLLER
+ // > Define a local array to fill in with the body frame error
+ float current_bodyFrameError[12];
+ // > Call the function to perform the conversion
+ convert_stateInertial_to_bodyFrameError(current_stateInertialEstimate,setpoint,current_bodyFrameError);
+
+
+ // UPDATE THE SETPOINT FOR FLYING IN A CIRCLE - IF ACTIVE
+ if (isActive_fly_test_circle)
+ {
+ update_setpoint_for_test_circle();
+ }
+
+
+ // CARRY OUT THE CONTROLLER COMPUTATIONS
+ calculateControlOutput_viaLQR(current_bodyFrameError,request,response);
+
+
+ // PUBLISH THE DEBUG MESSAGE (if required)
+ if (shouldPublishDebugMessage)
+ {
+ construct_and_publish_debug_message(request,response);
+ }
+
+ // Return "true" to indicate that the control computation was performed successfully
+ return true;
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// RRRR EEEEE M M OOO TTTTT EEEEE
+// R R E MM MM O O T E
+// RRRR EEE M M M O O T EEE
+// R R E M M O O T E
+// R R EEEEE M M OOO T EEEEE
+//
+// CCCC OOO N N TTTTT RRRR OOO L
+// C O O NN N T R R O O L
+// C O O N N N T RRRR O O L
+// C O O N NN T R R O O L
+// CCCC OOO N N T R R OOO LLLLL
+// ----------------------------------------------------------------------------------
+
+
+// ACTIVATE THE TESTS
+void activateTestCallback(const std_msgs::Int32& msg)
+{
+
+ // Get the test index from the message
+ int test_index = msg.data;
+
+ switch (test_index)
+ {
+ // Test the HORIZONTAL controller
+ case 1:
+ {
+ // Display what test will be activated
+ ROS_INFO("[TUNING CONTROLLER] Received message to perform HORIZONTAL test");
+
+ // Ensure the circle is not active
+ if (isActive_fly_test_circle)
+ {
+ isActive_fly_test_circle = false;
+ }
+
+ switch (test_horizontal_currentpoint)
+ {
+ // Currently at setpoint 1 => change to setpoint 2
+ case 1:
+ {
+ setpoint[0] = test_horizontal_setpoint2[0];
+ setpoint[1] = test_horizontal_setpoint2[1];
+ setpoint[2] = test_horizontal_setpoint2[2];
+ setpoint[3] = test_horizontal_setpoint2[3];
+ test_horizontal_currentpoint = 2;
+ break;
+ }
+ // Currently at setpoint 2 => change to setpoint 1
+ case 2:
+ {
+ setpoint[0] = test_horizontal_setpoint1[0];
+ setpoint[1] = test_horizontal_setpoint1[1];
+ setpoint[2] = test_horizontal_setpoint1[2];
+ setpoint[3] = test_horizontal_setpoint1[3];
+ test_horizontal_currentpoint = 1;
+ break;
+ }
+ // Handle the exception
+ default:
+ {
+ // Display that the "estimator_method" was not recognised
+ ROS_INFO_STREAM("[TUNING CONTROLLER] ERROR: in the 'activateTestCallback' function of the 'TuningControllerService': the 'test_horizontal_currentpoint' is not recognised.");
+ break;
+ }
+ }
+ break;
+ }
+
+
+ // Test the VERTICAL controller
+ case 2:
+ {
+ // Display what test will be activated
+ ROS_INFO("[TUNING CONTROLLER] Received message to perform VERTICAL test");
+
+ // Ensure the circle is not active
+ if (isActive_fly_test_circle)
+ {
+ isActive_fly_test_circle = false;
+ }
+
+ switch (test_vertical_currentpoint)
+ {
+ // Currently at setpoint 1 => change to setpoint 2
+ case 1:
+ {
+ setpoint[0] = test_vertical_setpoint2[0];
+ setpoint[1] = test_vertical_setpoint2[1];
+ setpoint[2] = test_vertical_setpoint2[2];
+ setpoint[3] = test_vertical_setpoint2[3];
+ test_vertical_currentpoint = 2;
+ break;
+ }
+ // Currently at setpoint 2 => change to setpoint 1
+ case 2:
+ {
+ setpoint[0] = test_vertical_setpoint1[0];
+ setpoint[1] = test_vertical_setpoint1[1];
+ setpoint[2] = test_vertical_setpoint1[2];
+ setpoint[3] = test_vertical_setpoint1[3];
+ test_vertical_currentpoint = 1;
+ break;
+ }
+ // Handle the exception
+ default:
+ {
+ // Display that the "estimator_method" was not recognised
+ ROS_INFO_STREAM("[TUNING CONTROLLER] ERROR: in the 'activateTestCallback' function of the 'TuningControllerService': the 'test_vertical_currentpoint' is not recognised.");
+ break;
+ }
+ }
+ break;
+ }
+
+
+ // Test the HEADING controller
+ case 3:
+ {
+ // Display what test will be activated
+ ROS_INFO("[TUNING CONTROLLER] Received message to perform HEADING test");
+
+ // Ensure the circle is not active
+ if (isActive_fly_test_circle)
+ {
+ isActive_fly_test_circle = false;
+ }
+
+ switch (test_heading_currentpoint)
+ {
+ // Currently at setpoint 1 => change to setpoint 2
+ case 1:
+ {
+ setpoint[0] = test_heading_setpoint2[0];
+ setpoint[1] = test_heading_setpoint2[1];
+ setpoint[2] = test_heading_setpoint2[2];
+ setpoint[3] = test_heading_setpoint2[3];
+ test_heading_currentpoint = 2;
+ break;
+ }
+ // Currently at setpoint 2 => change to setpoint 1
+ case 2:
+ {
+ setpoint[0] = test_heading_setpoint1[0];
+ setpoint[1] = test_heading_setpoint1[1];
+ setpoint[2] = test_heading_setpoint1[2];
+ setpoint[3] = test_heading_setpoint1[3];
+ test_heading_currentpoint = 1;
+ break;
+ }
+ // Handle the exception
+ default:
+ {
+ // Display that the "estimator_method" was not recognised
+ ROS_INFO_STREAM("[TUNING CONTROLLER] ERROR: in the 'activateTestCallback' function of the 'TuningControllerService': the 'test_heading_currentpoint' is not recognised.");
+ break;
+ }
+ }
+ break;
+ }
+
+
+ // Test ALL the controllers
+ case 4:
+ {
+ // Display what test will be activated
+ ROS_INFO("[TUNING CONTROLLER] Received message to perform ALL test");
+
+ // Ensure the circle is not active
+ if (isActive_fly_test_circle)
+ {
+ isActive_fly_test_circle = false;
+ }
+
+ switch (test_all_currentpoint)
+ {
+ // Currently at setpoint 1 => change to setpoint 2
+ case 1:
+ {
+ setpoint[0] = test_all_setpoint2[0];
+ setpoint[1] = test_all_setpoint2[1];
+ setpoint[2] = test_all_setpoint2[2];
+ setpoint[3] = test_all_setpoint2[3];
+ test_all_currentpoint = 2;
+ break;
+ }
+ // Currently at setpoint 2 => change to setpoint 1
+ case 2:
+ {
+ setpoint[0] = test_all_setpoint1[0];
+ setpoint[1] = test_all_setpoint1[1];
+ setpoint[2] = test_all_setpoint1[2];
+ setpoint[3] = test_all_setpoint1[3];
+ test_all_currentpoint = 1;
+ break;
+ }
+ // Handle the exception
+ default:
+ {
+ // Display that the "estimator_method" was not recognised
+ ROS_INFO_STREAM("[TUNING CONTROLLER] ERROR: in the 'activateTestCallback' function of the 'TuningControllerService': the 'test_all_currentpoint' is not recognised.");
+ break;
+ }
+ }
+ break;
+ }
+
+ // Test FLY A CIRCLE
+ case 5:
+ {
+ // Display what test will be activated
+ ROS_INFO("[TUNING CONTROLLER] Received message to FLY IN A CIRCLE");
+
+ // Check that not already flying a circle
+ if (!isActive_fly_test_circle)
+ {
+ // Set the boolean to make the circle active
+ isActive_fly_test_circle = true;
+
+ // (Re-)set the tick counter to zero
+ test_circle_ticks_since_start = 0;
+
+ }
+ break;
+ }
+
+
+ // Handle the exception
+ default:
+ {
+ // Display that the "estimator_method" was not recognised
+ ROS_INFO_STREAM("[TUNING CONTROLLER] ERROR: in the 'activateTestCallback' function of the 'TuningControllerService': the 'test_index' is not recognised.");
+ break;
+ }
+ }
+}
+
+
+void update_setpoint_for_test_circle()
+{
+ // Compute the time since start
+ float time_since_start = float(test_circle_ticks_since_start) / vicon_frequency;
+
+ // Allow time to reach the start point
+ if (time_since_start < test_circle_time_to_reach_start)
+ {
+ // Set the initial setpoint
+ setpoint[0] = test_circle_radius;
+ setpoint[1] = 0.0;
+ setpoint[2] = test_circle_height;
+ setpoint[3] = 0.0;
+ }
+ else if (time_since_start < (2.2*test_circle_seconds_per_rev) )
+ {
+ // Compute the current point in the period
+ float omega_t = ((time_since_start-test_circle_time_to_reach_start)/test_circle_seconds_per_rev) * (2*PI);
+ // Set the current setpoint on the circle
+ setpoint[0] = test_circle_radius * cos(omega_t);
+ setpoint[1] = test_circle_radius * sin(omega_t);
+ setpoint[2] = test_circle_height;
+ setpoint[3] = test_circle_pirouette_per_rev * omega_t;
+ }
+ else
+ {
+ // Turn of the flz circle mode
+ isActive_fly_test_circle = false;
+ // Return to the center
+ setpoint[0] = 0.0;
+ setpoint[1] = 0.0;
+ setpoint[2] = test_circle_height;
+ setpoint[3] = 0.0;
+ }
+
+ // Increment the tick counter
+ test_circle_ticks_since_start++;
+}
+
+
+
+// CHANGE THE GAIN FOR THE HORIZONTAL CONTROLLER
+void horizontalGainCallback(const std_msgs::Int32& msg)
+{
+ // Get the value from the message
+ float value = float(msg.data) / 1000.0;
+
+ // Compute the new multiplier value
+ multiplier_horizontal = multiplier_horizontal_min + value * (multiplier_horizontal_max-multiplier_horizontal_min);
+
+ // Display the value received
+ ROS_INFO_STREAM("[TUNING CONTROLLER] Received new HORIZONTAL gain with value " << value << ", the multiplier now = " << multiplier_horizontal );
+}
+
+// CHANGE THE GAIN FOR THE VERTICAL CONTROLLER
+void verticalGainCallback(const std_msgs::Int32& msg)
+{
+ // Get the value from the message
+ float value = float(msg.data) / 1000.0;
+
+ // Compute the new multiplier value
+ multiplier_vertical = multiplier_vertical_min + value * (multiplier_vertical_max-multiplier_vertical_min);
+
+ // Display the value received
+ ROS_INFO_STREAM("[TUNING CONTROLLER] Received new VERTICAL gain with value " << value << ", the multiplier now = " << multiplier_vertical );
+}
+
+// CHANGE THE GAIN FOR THE HEADING CONTROLLER
+void headingGainCallback(const std_msgs::Int32& msg)
+{
+ // Get the value from the message
+ float value = float(msg.data) / 1000.0;
+
+ // Compute the new multiplier value
+ multiplier_heading = multiplier_heading_min + value * (multiplier_heading_max-multiplier_heading_min);
+
+ // Display the value received
+ ROS_INFO_STREAM("[TUNING CONTROLLER] Received new HEADING gain with value " << value << ", the multiplier now = " << multiplier_heading );
+}
+
+
+
+
+
+
+
+
+
+// ------------------------------------------------------------------------------
+// EEEEE SSSS TTTTT III M M A TTTTT III OOO N N
+// E S T I MM MM A A T I O O NN N
+// EEE SSS T I M M M A A T I O O N N N
+// E S T I M M AAAAA T I O O N NN
+// EEEEE SSSS T III M M A A T III OOO N N
+// ------------------------------------------------------------------------------
+void performEstimatorUpdate_forStateInterial(Controller::Request &request)
+{
+
+ // PUT THE CURRENT MEASURED DATA INTO THE CLASS VARIABLE
+ // > for (x,y,z) position
+ current_xzy_rpy_measurement[0] = request.ownCrazyflie.x;
+ current_xzy_rpy_measurement[1] = request.ownCrazyflie.y;
+ current_xzy_rpy_measurement[2] = request.ownCrazyflie.z;
+ // > for (roll,pitch,yaw) angles
+ current_xzy_rpy_measurement[3] = request.ownCrazyflie.roll;
+ current_xzy_rpy_measurement[4] = request.ownCrazyflie.pitch;
+ current_xzy_rpy_measurement[5] = request.ownCrazyflie.yaw;
+
+
+ // RUN THE FINITE DIFFERENCE ESTIMATOR
+ performEstimatorUpdate_forStateInterial_viaFiniteDifference();
+
+
+ // RUN THE POINT MASS KALMAN FILTER ESTIMATOR
+ performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter();
+
+
+ // FILLE IN THE STATE INERTIAL ESTIMATE TO BE USED FOR CONTROL
+ switch (estimator_method)
+ {
+ // Estimator based on finte differences
+ case ESTIMATOR_METHOD_FINITE_DIFFERENCE:
+ {
+ // Transfer the estimate
+ for(int i = 0; i < 12; ++i)
+ {
+ current_stateInertialEstimate[i] = stateInterialEstimate_viaFiniteDifference[i];
+ }
+ break;
+ }
+ // Estimator based on Point Mass Kalman Filter
+ case ESTIMATOR_METHOD_POINT_MASS_PER_DIMENSION:
+ {
+ // Transfer the estimate
+ for(int i = 0; i < 12; ++i)
+ {
+ current_stateInertialEstimate[i] = stateInterialEstimate_viaPointMassKalmanFilter[i];
+ }
+ break;
+ }
+ // Handle the exception
+ default:
+ {
+ // Display that the "estimator_method" was not recognised
+ ROS_INFO_STREAM("[TUNING CONTROLLER] ERROR: in the 'calculateControlOutput' function of the 'TuningControllerService': the 'estimator_method' is not recognised.");
+ // Transfer the finite difference estimate by default
+ for(int i = 0; i < 12; ++i)
+ {
+ current_stateInertialEstimate[i] = stateInterialEstimate_viaFiniteDifference[i];
+ }
+ break;
+ }
+ }
+
+
+ // NOW THAT THE ESTIMATORS HAVE ALL BEEN RUN, PUT THE CURRENT
+ // MEASURED DATA INTO THE CLASS VARIABLE FOR THE PREVIOUS
+ // > for (x,y,z) position
+ previous_xzy_rpy_measurement[0] = current_xzy_rpy_measurement[0];
+ previous_xzy_rpy_measurement[1] = current_xzy_rpy_measurement[1];
+ previous_xzy_rpy_measurement[2] = current_xzy_rpy_measurement[2];
+ // > for (roll,pitch,yaw) angles
+ previous_xzy_rpy_measurement[3] = current_xzy_rpy_measurement[3];
+ previous_xzy_rpy_measurement[4] = current_xzy_rpy_measurement[4];
+ previous_xzy_rpy_measurement[5] = current_xzy_rpy_measurement[5];
+}
+
+
+
+void performEstimatorUpdate_forStateInterial_viaFiniteDifference()
+{
+ // PUT IN THE CURRENT MEASUREMENT DIRECTLY
+ // > for (x,y,z) position
+ stateInterialEstimate_viaFiniteDifference[0] = current_xzy_rpy_measurement[0];
+ stateInterialEstimate_viaFiniteDifference[1] = current_xzy_rpy_measurement[1];
+ stateInterialEstimate_viaFiniteDifference[2] = current_xzy_rpy_measurement[2];
+ // > for (roll,pitch,yaw) angles
+ stateInterialEstimate_viaFiniteDifference[6] = current_xzy_rpy_measurement[3];
+ stateInterialEstimate_viaFiniteDifference[7] = current_xzy_rpy_measurement[4];
+ stateInterialEstimate_viaFiniteDifference[8] = current_xzy_rpy_measurement[5];
+
+ // COMPUTE THE VELOCITIES VIA FINITE DIFFERENCE
+ // > for (x,y,z) velocities
+ stateInterialEstimate_viaFiniteDifference[3] = (current_xzy_rpy_measurement[0] - previous_xzy_rpy_measurement[0]) * estimator_frequency;
+ stateInterialEstimate_viaFiniteDifference[4] = (current_xzy_rpy_measurement[1] - previous_xzy_rpy_measurement[1]) * estimator_frequency;
+ stateInterialEstimate_viaFiniteDifference[5] = (current_xzy_rpy_measurement[2] - previous_xzy_rpy_measurement[2]) * estimator_frequency;
+ // > for (roll,pitch,yaw) velocities
+ stateInterialEstimate_viaFiniteDifference[9] = (current_xzy_rpy_measurement[3] - previous_xzy_rpy_measurement[3]) * estimator_frequency;
+ stateInterialEstimate_viaFiniteDifference[10] = (current_xzy_rpy_measurement[4] - previous_xzy_rpy_measurement[4]) * estimator_frequency;
+ stateInterialEstimate_viaFiniteDifference[11] = (current_xzy_rpy_measurement[5] - previous_xzy_rpy_measurement[5]) * estimator_frequency;
+}
+
+
+
+void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter()
+{
+ // PERFORM THE KALMAN FILTER UPDATE STEP
+ // > First take a copy of the estimator state
+ float temp_PMKFstate[12];
+ for(int i = 0; i < 12; ++i)
+ {
+ temp_PMKFstate[i] = stateInterialEstimate_viaPointMassKalmanFilter[i];
+ }
+ // > Now perform update for:
+ // > x position and velocity:
+ stateInterialEstimate_viaPointMassKalmanFilter[0] = PMKF_Ahat_row1_for_positions[0]*temp_PMKFstate[0] + PMKF_Ahat_row1_for_positions[1]*temp_PMKFstate[3] + PMKF_Kinf_for_positions[0]*current_xzy_rpy_measurement[0];
+ stateInterialEstimate_viaPointMassKalmanFilter[3] = PMKF_Ahat_row2_for_positions[0]*temp_PMKFstate[0] + PMKF_Ahat_row2_for_positions[1]*temp_PMKFstate[3] + PMKF_Kinf_for_positions[1]*current_xzy_rpy_measurement[0];
+ // > y position and velocity:
+ stateInterialEstimate_viaPointMassKalmanFilter[1] = PMKF_Ahat_row1_for_positions[0]*temp_PMKFstate[1] + PMKF_Ahat_row1_for_positions[1]*temp_PMKFstate[4] + PMKF_Kinf_for_positions[0]*current_xzy_rpy_measurement[1];
+ stateInterialEstimate_viaPointMassKalmanFilter[4] = PMKF_Ahat_row2_for_positions[0]*temp_PMKFstate[1] + PMKF_Ahat_row2_for_positions[1]*temp_PMKFstate[4] + PMKF_Kinf_for_positions[1]*current_xzy_rpy_measurement[1];
+ // > z position and velocity:
+ stateInterialEstimate_viaPointMassKalmanFilter[2] = PMKF_Ahat_row1_for_positions[0]*temp_PMKFstate[2] + PMKF_Ahat_row1_for_positions[1]*temp_PMKFstate[5] + PMKF_Kinf_for_positions[0]*current_xzy_rpy_measurement[2];
+ stateInterialEstimate_viaPointMassKalmanFilter[5] = PMKF_Ahat_row2_for_positions[0]*temp_PMKFstate[2] + PMKF_Ahat_row2_for_positions[1]*temp_PMKFstate[5] + PMKF_Kinf_for_positions[1]*current_xzy_rpy_measurement[2];
+
+ // > roll position and velocity:
+ stateInterialEstimate_viaPointMassKalmanFilter[6] = PMKF_Ahat_row1_for_angles[0]*temp_PMKFstate[6] + PMKF_Ahat_row1_for_angles[1]*temp_PMKFstate[9] + PMKF_Kinf_for_angles[0]*current_xzy_rpy_measurement[3];
+ stateInterialEstimate_viaPointMassKalmanFilter[9] = PMKF_Ahat_row2_for_angles[0]*temp_PMKFstate[6] + PMKF_Ahat_row2_for_angles[1]*temp_PMKFstate[9] + PMKF_Kinf_for_angles[1]*current_xzy_rpy_measurement[3];
+ // > pitch position and velocity:
+ stateInterialEstimate_viaPointMassKalmanFilter[7] = PMKF_Ahat_row1_for_angles[0]*temp_PMKFstate[7] + PMKF_Ahat_row1_for_angles[1]*temp_PMKFstate[10] + PMKF_Kinf_for_angles[0]*current_xzy_rpy_measurement[4];
+ stateInterialEstimate_viaPointMassKalmanFilter[10] = PMKF_Ahat_row2_for_angles[0]*temp_PMKFstate[7] + PMKF_Ahat_row2_for_angles[1]*temp_PMKFstate[10] + PMKF_Kinf_for_angles[1]*current_xzy_rpy_measurement[4];
+ // > yaw position and velocity:
+ stateInterialEstimate_viaPointMassKalmanFilter[8] = PMKF_Ahat_row1_for_angles[0]*temp_PMKFstate[8] + PMKF_Ahat_row1_for_angles[1]*temp_PMKFstate[11] + PMKF_Kinf_for_angles[0]*current_xzy_rpy_measurement[5];
+ stateInterialEstimate_viaPointMassKalmanFilter[11] = PMKF_Ahat_row2_for_angles[0]*temp_PMKFstate[8] + PMKF_Ahat_row2_for_angles[1]*temp_PMKFstate[11] + PMKF_Kinf_for_angles[1]*current_xzy_rpy_measurement[5];
+}
+
+
+
+
+// ----------------------------------------------------------------------------------
+// L QQQ RRRR
+// L Q Q R R
+// L Q Q RRRR
+// L Q Q R R
+// LLLLL QQ Q R R
+// ----------------------------------------------------------------------------------
+
+void calculateControlOutput_viaLQR(float stateErrorBody[12], Controller::Request &request, Controller::Response &response)
+{
+ // SWITCH BETWEEN THE DIFFERENT LQR CONTROLLER MODES:
+ switch (controller_mode)
+ {
+ // LQR controller based on the state vector:
+ // [position,velocity,angles]
+ case CONTROLLER_MODE_LQR_RATE:
+ {
+ // Call the function that performs the control computations for this mode
+ calculateControlOutput_viaLQRforRates(stateErrorBody,request,response);
+ break;
+ }
+
+ // LQR controller based on the state vector:
+ // [position,velocity]
+ case CONTROLLER_MODE_LQR_ANGLE:
+ {
+ // Call the function that performs the control computations for this mode
+ calculateControlOutput_viaLQRforAngles(stateErrorBody,request,response);
+ break;
+ }
+
+ // LQR controller based on the state vector:
+ // [position,velocity,angles]
+ case CONTROLLER_MODE_LQR_ANGLE_RATE_NESTED:
+ {
+ // Call the function that performs the control computations for this mode
+ calculateControlOutput_viaLQRforAnglesRatesNested(stateErrorBody,request,response);
+ break;
+ }
+
+ default:
+ {
+ // Display that the "controller_mode" was not recognised
+ ROS_INFO_STREAM("[TUNING CONTROLLER] ERROR: in the 'calculateControlOutput' function of the 'TuningControllerService': the 'controller_mode' is not recognised.");
+ // Set everything in the response to zero
+ response.controlOutput.roll = 0;
+ response.controlOutput.pitch = 0;
+ response.controlOutput.yaw = 0;
+ response.controlOutput.motorCmd1 = 0;
+ response.controlOutput.motorCmd2 = 0;
+ response.controlOutput.motorCmd3 = 0;
+ response.controlOutput.motorCmd4 = 0;
+ response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
+ break;
+ }
+ }
+}
+
+
+
+
+void calculateControlOutput_viaLQRforRates(float stateErrorBody[12], Controller::Request &request, Controller::Response &response)
+{
+ // PERFORM THE "u=Kx" LQR CONTROLLER COMPUTATION
+
+ // Instantiate the local variables for the following:
+ // > body frame roll rate,
+ // > body frame pitch rate,
+ // > body frame yaw rate,
+ // > total thrust adjustment.
+ // These will be requested from the Crazyflie's on-baord "inner-loop" controller
+ float rollRate_forResponse = 0;
+ float pitchRate_forResponse = 0;
+ float yawRate_forResponse = 0;
+ float thrustAdjustment = 0;
+
+ // Perform the "-Kx" LQR computation for the rates and thrust:
+ for(int i = 0; i < 9; ++i)
+ {
+ // BODY FRAME Y CONTROLLER
+ rollRate_forResponse -= gainMatrixRollRate[i] * stateErrorBody[i] * multiplier_horizontal;
+ // BODY FRAME X CONTROLLER
+ pitchRate_forResponse -= gainMatrixPitchRate[i] * stateErrorBody[i] * multiplier_horizontal;
+ // BODY FRAME YAW CONTROLLER
+ yawRate_forResponse -= gainMatrixYawRate[i] * stateErrorBody[i] * multiplier_heading;
+ // > ALITUDE CONTROLLER (i.e., z-controller):
+ thrustAdjustment -= gainMatrixThrust_NineStateVector[i] * stateErrorBody[i] * multiplier_vertical;
+ }
+
+
+ // UPDATE THE "RETURN" THE VARIABLE NAMED "response"
+
+ // Put the computed rates and thrust into the "response" variable
+ // > For roll, pitch, and yaw:
+ response.controlOutput.roll = rollRate_forResponse;
+ response.controlOutput.pitch = pitchRate_forResponse;
+ response.controlOutput.yaw = yawRate_forResponse;
+ // > For the thrust adjustment we must add the feed-forward thrust to counter-act gravity.
+ // > NOTE: remember that the thrust is commanded per motor, so you sohuld
+ // consider whether the "thrustAdjustment" computed by your
+ // controller needed to be divided by 4 or not.
+ thrustAdjustment = thrustAdjustment / 4.0;
+ // > NOTE: the "gravity_force_quarter" value was already divided by 4 when
+ // it was loaded/processes from the .yaml file.
+ response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+
+
+ // Specify that this controller is a rate controller
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
+ response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
+
+
+ // An alternate debugging technique is to print out data directly to the
+ // command line from which this node was launched.
+ if (shouldDisplayDebugInfo)
+ {
+ // An example of "printing out" the data from the "request" argument to the
+ // command line. This might be useful for debugging.
+ ROS_INFO_STREAM("x-coordinate [m]: " << request.ownCrazyflie.x);
+ ROS_INFO_STREAM("y-coordinate [m]: " << request.ownCrazyflie.y);
+ ROS_INFO_STREAM("z-coordinate [m]: " << request.ownCrazyflie.z);
+ ROS_INFO_STREAM("roll [deg]: " << request.ownCrazyflie.roll);
+ ROS_INFO_STREAM("pitch [deg]: " << request.ownCrazyflie.pitch);
+ ROS_INFO_STREAM("yaw [deg]: " << request.ownCrazyflie.yaw);
+ ROS_INFO_STREAM("Delta t [s]: " << request.ownCrazyflie.acquiringTime);
+
+ // An example of "printing out" the control actions computed.
+ ROS_INFO_STREAM("thrustAdjustment = " << thrustAdjustment);
+ ROS_INFO_STREAM("controlOutput.roll = " << response.controlOutput.roll);
+ ROS_INFO_STREAM("controlOutput.pitch = " << response.controlOutput.pitch);
+ ROS_INFO_STREAM("controlOutput.yaw = " << response.controlOutput.yaw);
+
+ // An example of "printing out" the "thrust-to-command" conversion parameters.
+ ROS_INFO_STREAM("motorPoly 0:" << motorPoly[0]);
+ ROS_INFO_STREAM("motorPoly 0:" << motorPoly[1]);
+ ROS_INFO_STREAM("motorPoly 0:" << motorPoly[2]);
+
+ // An example of "printing out" the per motor 16-bit command computed.
+ ROS_INFO_STREAM("controlOutput.cmd1 = " << response.controlOutput.motorCmd1);
+ ROS_INFO_STREAM("controlOutput.cmd3 = " << response.controlOutput.motorCmd2);
+ ROS_INFO_STREAM("controlOutput.cmd2 = " << response.controlOutput.motorCmd3);
+ ROS_INFO_STREAM("controlOutput.cmd4 = " << response.controlOutput.motorCmd4);
+ }
+}
+
+
+
+
+void calculateControlOutput_viaLQRforAngles(float stateErrorBody[12], Controller::Request &request, Controller::Response &response)
+{
+ // PERFORM THE "u=Kx" LQR CONTROLLER COMPUTATION
+
+ // Instantiate the local variables for the following:
+ // > body frame roll angle,
+ // > body frame pitch angle,
+ // > total thrust adjustment.
+ // These will be requested from the Crazyflie's on-baord "inner-loop" controller
+ float rollAngle_forResponse = 0;
+ float pitchAngle_forResponse = 0;
+ float thrustAdjustment = 0;
+
+ // Perform the "-Kx" LQR computation for the rates and thrust:
+ for(int i = 0; i < 6; ++i)
+ {
+ // BODY FRAME Y CONTROLLER
+ rollAngle_forResponse -= gainMatrixRollAngle[i] * stateErrorBody[i] * multiplier_horizontal;
+ // BODY FRAME X CONTROLLER
+ pitchAngle_forResponse -= gainMatrixPitchAngle[i] * stateErrorBody[i] * multiplier_horizontal;
+ // > ALITUDE CONTROLLER (i.e., z-controller):
+ thrustAdjustment -= gainMatrixThrust_SixStateVector[i] * stateErrorBody[i] * multiplier_vertical;
+ }
+
+ // UPDATE THE "RETURN" THE VARIABLE NAMED "response"
+
+ // Put the computed rates and thrust into the "response" variable
+ // > For roll, pitch, and yaw:
+ response.controlOutput.roll = rollAngle_forResponse;
+ response.controlOutput.pitch = pitchAngle_forResponse;
+ response.controlOutput.yaw = setpoint[3];
+ // > For the thrust adjustment we must add the feed-forward thrust to counter-act gravity.
+ // > NOTE: remember that the thrust is commanded per motor, so you sohuld
+ // consider whether the "thrustAdjustment" computed by your
+ // controller needed to be divided by 4 or not.
+ thrustAdjustment = thrustAdjustment / 4.0;
+ // > NOTE: the "gravity_force_quarter" value was already divided by 4 when
+ // it was loaded/processes from the .yaml file.
+ response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+
+
+ // Specify that this controller is a rate controller
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
+ response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
+
+
+}
+
+
+
+void calculateControlOutput_viaLQRforAnglesRatesNested( float stateErrorBody[12], Controller::Request &request, Controller::Response &response)
+{
+ // PERFORM THE NESTED "u=Kx" LQR CONTROLLER COMPUTATION
+
+ // Increment the counter variable
+ lqr_angleRateNested_counter++;
+
+ if (lqr_angleRateNested_counter > 4)
+ {
+ //ROS_INFO("Outer called");
+
+ // Reset the counter to 1
+ lqr_angleRateNested_counter = 1;
+
+ // PERFORM THE OUTER "u=Kx" LQR CONTROLLER COMPUTATION
+
+ // Reset the class variable to zero for the following:
+ // > body frame roll angle,
+ // > body frame pitch angle,
+ // > body frame yaw angle,
+ // > total thrust adjustment.
+ // These will be requested from the "inner-loop" LQR controller below
+ lqr_angleRateNested_prev_rollAngle = 0;
+ lqr_angleRateNested_prev_pitchAngle = 0;
+ lqr_angleRateNested_prev_thrustAdjustment = 0;
+
+ // Perform the "-Kx" LQR computation for the rates and thrust:
+ for(int i = 0; i < 6; ++i)
+ {
+ // BODY FRAME Y CONTROLLER
+ lqr_angleRateNested_prev_rollAngle -= gainMatrixRollAngle_50Hz[i] * stateErrorBody[i] * multiplier_horizontal;
+ // BODY FRAME X CONTROLLER
+ lqr_angleRateNested_prev_pitchAngle -= gainMatrixPitchAngle_50Hz[i] * stateErrorBody[i] * multiplier_horizontal;
+ // > ALITUDE CONTROLLER (i.e., z-controller):
+ lqr_angleRateNested_prev_thrustAdjustment -= gainMatrixThrust_SixStateVector_50Hz[i] * stateErrorBody[i] * multiplier_vertical;
+ }
+
+ // BODY FRAME Z CONTROLLER
+ //lqr_angleRateNested_prev_yawAngle = setpoint[3];
+ lqr_angleRateNested_prev_yawAngle = stateErrorBody[8];
+
+
+ }
+
+ //ROS_INFO("Inner called");
+
+ // PERFORM THE INNER "u=Kx" LQR CONTROLLER COMPUTATION
+ // Instantiate the local variables for the following:
+ // > body frame roll rate,
+ // > body frame pitch rate,
+ // > body frame yaw rate,
+ // These will be requested from the Crazyflie's on-baord "inner-loop" controller
+ float rollRate_forResponse = 0;
+ float pitchRate_forResponse = 0;
+ float yawRate_forResponse = 0;
+
+ // Create the angle error to use for the inner controller
+ float temp_stateAngleError[3] = {
+ stateErrorBody[6] - lqr_angleRateNested_prev_rollAngle,
+ stateErrorBody[7] - lqr_angleRateNested_prev_pitchAngle,
+ lqr_angleRateNested_prev_yawAngle
+ };
+
+ // Perform the "-Kx" LQR computation for the rates and thrust:
+ for(int i = 0; i < 4; ++i)
+ {
+ // BODY FRAME Y CONTROLLER
+ rollRate_forResponse -= gainMatrixRollRate_Nested[i] * temp_stateAngleError[i];
+ // BODY FRAME X CONTROLLER
+ pitchRate_forResponse -= gainMatrixPitchRate_Nested[i] * temp_stateAngleError[i];
+ // BODY FRAME Z CONTROLLER
+ yawRate_forResponse -= gainMatrixYawRate_Nested[i] * temp_stateAngleError[i] * multiplier_heading;
+ }
+
+
+ // UPDATE THE "RETURN" THE VARIABLE NAMED "response"
+
+ // Put the computed rates and thrust into the "response" variable
+ // > For roll, pitch, and yaw:
+ response.controlOutput.roll = rollRate_forResponse;
+ response.controlOutput.pitch = pitchRate_forResponse;
+ response.controlOutput.yaw = yawRate_forResponse;
+ // > For the thrust adjustment we must add the feed-forward thrust to counter-act gravity.
+ // > NOTE: remember that the thrust is commanded per motor, so you sohuld
+ // consider whether the "thrustAdjustment" computed by your
+ // controller needed to be divided by 4 or not.
+ float thrustAdjustment = lqr_angleRateNested_prev_thrustAdjustment / 4.0;
+ // > NOTE: the "gravity_force_quarter" value was already divided by 4 when
+ // it was loaded/processes from the .yaml file.
+ response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+ response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustAdjustment + gravity_force_quarter);
+
+
+ // Specify that this controller is a rate controller
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_MOTOR;
+ response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_RATE;
+ // response.controlOutput.onboardControllerType = CF_COMMAND_TYPE_ANGLE;
+
+ // Display some details (if requested)
+ if (shouldDisplayDebugInfo)
+ {
+ ROS_INFO_STREAM("thrust =" << lqr_angleRateNested_prev_thrustAdjustment );
+ ROS_INFO_STREAM("rollrate =" << response.controlOutput.roll );
+ ROS_INFO_STREAM("pitchrate =" << response.controlOutput.pitch );
+ ROS_INFO_STREAM("yawrate =" << response.controlOutput.yaw );
+ }
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+void construct_and_publish_debug_message(Controller::Request &request, Controller::Response &response)
+{
+ // ***********************************************************
+ // DDDD EEEEE BBBB U U GGGG M M SSSS GGGG
+ // D D E B B U U G MM MM S G
+ // D D EEE BBBB U U G M M M SSS G
+ // D D E B B U U G G M M S G G
+ // DDDD EEEEE BBBB UUU GGGG M M SSSS GGGG
+
+ // DEBUGGING CODE:
+ // As part of the D-FaLL-System we have defined a message type names"DebugMsg".
+ // By fill this message with data and publishing it you can display the data in
+ // real time using rpt plots. Instructions for using rqt plots can be found on
+ // the wiki of the D-FaLL-System repository
+
+ // Instantiate a local variable of type "DebugMsg", see the file "DebugMsg.msg"
+ // (located in the "msg" folder) to see the full structure of this message.
+ DebugMsg debugMsg;
+
+ // Fill the debugging message with the data provided by Vicon
+ debugMsg.vicon_x = request.ownCrazyflie.x;
+ debugMsg.vicon_y = request.ownCrazyflie.y;
+ debugMsg.vicon_z = request.ownCrazyflie.z;
+ debugMsg.vicon_roll = request.ownCrazyflie.roll;
+ debugMsg.vicon_pitch = request.ownCrazyflie.pitch;
+ debugMsg.vicon_yaw = request.ownCrazyflie.yaw;
+
+ // Fill in the debugging message with any other data you would like to display
+ // in real time. For example, it might be useful to display the thrust
+ // adjustment computed by the z-altitude controller.
+ // The "DebugMsg" type has 10 properties from "value_1" to "value_10", all of
+ // type "float64" that you can fill in with data you would like to plot in
+ // real-time.
+ // debugMsg.value_1 = thrustAdjustment;
+ // ......................
+ // debugMsg.value_10 = your_variable_name;
+
+ // Publish the "debugMsg"
+ debugPublisher.publish(debugMsg);
+}
+
+
+
+
+// ------------------------------------------------------------------------------
+// RRRR OOO TTTTT A TTTTT EEEEE III N N TTTTT OOO
+// R R O O T A A T E I NN N T O O
+// RRRR O O T A A T EEE I N N N T O O
+// R R O O T AAAAA T E I N NN T O O
+// R R OOO T A A T EEEEE III N N T OOO
+//
+// BBBB OOO DDDD Y Y FFFFF RRRR A M M EEEEE
+// B B O O D D Y Y F R R A A MM MM E
+// BBBB O O D D Y FFF RRRR A A M M M EEE
+// B B O O D D Y F R R AAAAA M M E
+// BBBB OOO DDDD Y F R R A A M M EEEEE
+// ------------------------------------------------------------------------------
+
+// The arguments for this function are as follows:
+// stateInertial
+// This is an array of length 9 with the estimates the error of of the following values
+// relative to the sepcifed setpoint:
+// stateInertial[0] x position of the Crazyflie relative to the inertial frame origin [meters]
+// stateInertial[1] y position of the Crazyflie relative to the inertial frame origin [meters]
+// stateInertial[2] z position of the Crazyflie relative to the inertial frame origin [meters]
+// stateInertial[3] x-axis component of the velocity of the Crazyflie in the inertial frame [meters/second]
+// stateInertial[4] y-axis component of the velocity of the Crazyflie in the inertial frame [meters/second]
+// stateInertial[5] z-axis component of the velocity of the Crazyflie in the inertial frame [meters/second]
+// stateInertial[6] The roll component of the intrinsic Euler angles [radians]
+// stateInertial[7] The pitch component of the intrinsic Euler angles [radians]
+// stateInertial[8] The yaw component of the intrinsic Euler angles [radians]
+//
+// stateBody
+// This is an empty array of length 9, this function should fill in all elements of this
+// array with the same ordering as for the "stateInertial" argument, expect that the (x,y)
+// position and (x,y) velocities are rotated into the body frame.
+//
+// yaw_measured
+// This is the yaw component of the intrinsic Euler angles in [radians] as measured by
+// the Vicon motion capture system
+//
+void convertIntoBodyFrame(float stateInertial[12], float (&stateBody)[12], float yaw_measured)
+{
+ if (shouldPerformConvertIntoBodyFrame)
+ {
+ float sinYaw = sin(yaw_measured);
+ float cosYaw = cos(yaw_measured);
+
+ // Fill in the (x,y,z) position estimates to be returned
+ stateBody[0] = stateInertial[0] * cosYaw + stateInertial[1] * sinYaw;
+ stateBody[1] = stateInertial[1] * cosYaw - stateInertial[0] * sinYaw;
+ stateBody[2] = stateInertial[2];
+
+ // Fill in the (x,y,z) velocity estimates to be returned
+ stateBody[3] = stateInertial[3] * cosYaw + stateInertial[4] * sinYaw;
+ stateBody[4] = stateInertial[4] * cosYaw - stateInertial[3] * sinYaw;
+ stateBody[5] = stateInertial[5];
+
+ // Fill in the (roll,pitch,yaw) estimates to be returned
+ stateBody[6] = stateInertial[6];
+ stateBody[7] = stateInertial[7];
+ stateBody[8] = stateInertial[8];
+
+ // Fill in the (roll,pitch,yaw) velocity estimates to be returned
+ stateBody[9] = stateInertial[9];
+ stateBody[10] = stateInertial[10];
+ stateBody[11] = stateInertial[11];
+ }
+ else
+ {
+ // Fill in the (x,y,z) position estimates to be returned
+ stateBody[0] = stateInertial[0];
+ stateBody[1] = stateInertial[1];
+ stateBody[2] = stateInertial[2];
+
+ // Fill in the (x,y,z) velocity estimates to be returned
+ stateBody[3] = stateInertial[3];
+ stateBody[4] = stateInertial[4];
+ stateBody[5] = stateInertial[5];
+
+ // Fill in the (roll,pitch,yaw) estimates to be returned
+ stateBody[6] = stateInertial[6];
+ stateBody[7] = stateInertial[7];
+ stateBody[8] = stateInertial[8];
+
+ // Fill in the (roll,pitch,yaw) velocity estimates to be returned
+ stateBody[9] = stateInertial[9];
+ stateBody[10] = stateInertial[10];
+ stateBody[11] = stateInertial[11];
+ }
+}
+
+
+
+
+void convert_stateInertial_to_bodyFrameError(float stateInertial[12], float setpoint[4], float (&bodyFrameError)[12])
+{
+ // Store the current YAW in a local variable
+ float temp_stateInertial_yaw = stateInertial[8];
+
+ // Adjust the INERTIAL (x,y,z) position for the setpoint
+ stateInertial[0] = stateInertial[0] - setpoint[0];
+ stateInertial[1] = stateInertial[1] - setpoint[1];
+ stateInertial[2] = stateInertial[2] - setpoint[2];
+
+ // Fill in the yaw angle error
+ // > This error should be "unwrapped" to be in the range
+ // ( -pi , pi )
+ // > First, get the yaw error into a local variable
+ float yawError = stateInertial[8] - setpoint[3];
+ // > Second, "unwrap" the yaw error to the interval ( -pi , pi )
+ while(yawError > PI) {yawError -= 2 * PI;}
+ while(yawError < -PI) {yawError += 2 * PI;}
+ // > Third, put the "yawError" into the "stateError" variable
+ stateInertial[8] = yawError;
+
+
+ if (yawError>(PI/6))
+ {
+ yawError = (PI/6);
+ }
+ else if (yawError<(-PI/6))
+ {
+ yawError = (-PI/6);
+ }
+
+ // CONVERSION INTO BODY FRAME
+ // Conver the state erorr from the Inertial frame into the Body frame
+ // > Note: the function "convertIntoBodyFrame" is implemented in this file
+ // and by default does not perform any conversion. The equations to convert
+ // the state error into the body frame should be implemented in that function
+ // for successful completion of the PPS exercise
+ convertIntoBodyFrame(stateInertial, bodyFrameError, temp_stateInertial_yaw);
+}
+
+
+
+
+// ------------------------------------------------------------------------------
+// N N EEEEE W W TTTTT OOO N N CCCC M M DDDD
+// NN N E W W T O O NN N C MM MM D D
+// N N N EEE W W T O O N N N --> C M M M D D
+// N NN E W W W T O O N NN C M M D D
+// N N EEEEE W W T OOO N N CCCC M M DDDD
+//
+// CCCC OOO N N V V EEEEE RRRR SSSS III OOO N N
+// C O O NN N V V E R R S I O O NN N
+// C O O N N N V V EEE RRRR SSS I O O N N N
+// C O O N NN V V E R R S I O O N NN
+// CCCC OOO N N V EEEEE R R SSSS III OOO N N
+// ------------------------------------------------------------------------------
+
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+float computeMotorPolyBackward(float thrust)
+{
+ // Compute the 16-bit command signal that generates the "thrust" force
+ float cmd = (-motorPoly[1] + sqrt(motorPoly[1] * motorPoly[1] - 4 * motorPoly[2] * (motorPoly[0] - thrust))) / (2 * motorPoly[2]);
+
+ // Saturate the signal to be 0 or in the range [1000,65000]
+ if (cmd < cmd_sixteenbit_min)
+ {
+ cmd = 0;
+ }
+ else if (cmd > cmd_sixteenbit_max)
+ {
+ cmd = cmd_sixteenbit_max;
+ }
+
+ return cmd;
+}
+
+
+
+
+// ----------------------------------------------------------------------------------
+// N N EEEEE W W SSSS EEEEE TTTTT PPPP OOO III N N TTTTT
+// NN N E W W S E T P P O O I NN N T
+// N N N EEE W W SSS EEE T PPPP O O I N N N T
+// N NN E W W W S E T P O O I N NN T
+// N N EEEEE W W SSSS EEEEE T P OOO III N N T
+//
+// GGG U U III CCCC A L L BBBB A CCCC K K
+// G G U U I C A A L L B B A A C K K
+// G U U I C A A L L BBBB A A C KKK
+// G G U U I C AAAAA L L B B AAAAA C K K
+// GGGG UUU III CCCC A A LLLLL LLLLL BBBB A A CCCC K K
+// ----------------------------------------------------------------------------------
+
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+void setpointCallback(const Setpoint& newSetpoint)
+{
+ setpoint[0] = newSetpoint.x;
+ setpoint[1] = newSetpoint.y;
+ setpoint[2] = newSetpoint.z;
+ setpoint[3] = newSetpoint.yaw;
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// L OOO A DDDD
+// L O O A A D D
+// L O O A A D D
+// L O O AAAAA D D
+// LLLLL OOO A A DDDD
+//
+// PPPP A RRRR A M M EEEEE TTTTT EEEEE RRRR SSSS
+// P P A A R R A A MM MM E T E R R S
+// PPPP A A RRRR A A M M M EEE T EEE RRRR SSS
+// P AAAAA R R AAAAA M M E T E R R S
+// P A A R R A A M M EEEEE T EEEEE R R SSSS
+// ----------------------------------------------------------------------------------
+
+
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+void yamlReadyForFetchCallback(const std_msgs::Int32& msg)
+{
+ // Extract from the "msg" for which controller the and from where to fetch the YAML
+ // parameters
+ int controller_to_fetch_yaml = msg.data;
+
+ // Switch between fetching for the different controllers and from different locations
+ switch(controller_to_fetch_yaml)
+ {
+
+ // > FOR FETCHING FROM THE AGENT'S OWN PARAMETER SERVICE
+ case FETCH_YAML_TUNING_CONTROLLER_FROM_OWN_AGENT:
+ {
+ // Let the user know that this message was received
+ ROS_INFO("[TUNING CONTROLLER] Received the message that YAML parameters were (re-)loaded. > Now fetching the parameter values from this agent.");
+ // Create a node handle to the parameter service running on this agent's machine
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(namespace_to_own_agent_parameter_service);
+ // Call the function that fetches the parameters
+ fetchYamlParameters(nodeHandle_to_own_agent_parameter_service);
+ break;
+ }
+
+ // > FOR FETCHING FROM THE COORDINATOR'S PARAMETER SERVICE
+ case FETCH_YAML_TUNING_CONTROLLER_FROM_COORDINATOR:
+ {
+ // Let the user know that this message was received
+ ROS_INFO("[TUNING CONTROLLER] Received the message that YAML parameters were (re-)loaded. > Now fetching the parameter values from the coordinator.");
+ // Create a node handle to the parameter service running on the coordinator machine
+ ros::NodeHandle nodeHandle_to_coordinator_parameter_service(namespace_to_coordinator_parameter_service);
+ // Call the function that fetches the parameters
+ fetchYamlParameters(nodeHandle_to_coordinator_parameter_service);
+ break;
+ }
+
+ default:
+ {
+ // Let the user know that the command was not relevant
+ //ROS_INFO("The CustomControllerService received the message that YAML parameters were (re-)loaded");
+ //ROS_INFO("> However the parameters do not relate to this controller, hence nothing will be fetched.");
+ break;
+ }
+ }
+}
+
+
+// This function CAN BE edited for successful completion of the PPS exercise, and the
+// use of parameters fetched from the YAML file is highly recommended to make tuning of
+// your controller easier and quicker.
+void fetchYamlParameters(ros::NodeHandle& nodeHandle)
+{
+ // Here we load the parameters that are specified in the CustomController.yaml file
+
+ // Add the "CustomController" namespace to the "nodeHandle"
+ ros::NodeHandle nodeHandle_for_TuningController(nodeHandle, "TuningController");
+
+
+ // ******************************************************************************* //
+ // PARAMETERS SPECIFIC TO THE TUNING CONTROL FEATURE
+
+ /// Setpoint for the HORIZONTAL test
+ getParameterFloatVector(nodeHandle_for_TuningController, "test_horizontal_setpoint1", test_horizontal_setpoint1, 4);
+ getParameterFloatVector(nodeHandle_for_TuningController, "test_horizontal_setpoint2", test_horizontal_setpoint2, 4);
+
+ // Setpoint for the VERTICAL test
+ getParameterFloatVector(nodeHandle_for_TuningController, "test_vertical_setpoint1", test_vertical_setpoint1, 4);
+ getParameterFloatVector(nodeHandle_for_TuningController, "test_vertical_setpoint2", test_vertical_setpoint2, 4);
+
+ // Setpoint for the HEADING test
+ getParameterFloatVector(nodeHandle_for_TuningController, "test_heading_setpoint1", test_heading_setpoint1, 4);
+ getParameterFloatVector(nodeHandle_for_TuningController, "test_heading_setpoint2", test_heading_setpoint2, 4);
+
+ // Setpoint for the ALL test
+ getParameterFloatVector(nodeHandle_for_TuningController, "test_all_setpoint1", test_all_setpoint1, 4);
+ getParameterFloatVector(nodeHandle_for_TuningController, "test_all_setpoint2", test_all_setpoint2, 4);
+
+ // Parameters for flying in a circle
+ test_circle_radius = getParameterFloat(nodeHandle_for_TuningController, "test_circle_radius");
+ test_circle_seconds_per_rev = getParameterFloat(nodeHandle_for_TuningController, "test_circle_seconds_per_rev");
+ test_circle_height = getParameterFloat(nodeHandle_for_TuningController, "test_circle_height");
+ test_circle_time_to_reach_start = getParameterFloat(nodeHandle_for_TuningController, "test_circle_time_to_reach_start");
+ test_circle_pirouette_per_rev = getParameterFloat(nodeHandle_for_TuningController, "test_circle_pirouette_per_rev");
+
+ // Multipliers for the HORIZONTAL contorller
+ multiplier_horizontal_min = getParameterFloat(nodeHandle_for_TuningController, "multiplier_horizontal_min");
+ multiplier_horizontal_max = getParameterFloat(nodeHandle_for_TuningController, "multiplier_horizontal_max");
+ // Multipliers for the VERTICAL contorller
+ multiplier_vertical_min = getParameterFloat(nodeHandle_for_TuningController, "multiplier_vertical_min");
+ multiplier_vertical_max = getParameterFloat(nodeHandle_for_TuningController, "multiplier_vertical_max");
+ // Multipliers for the HEADING contorller
+ multiplier_heading_min = getParameterFloat(nodeHandle_for_TuningController, "multiplier_heading_min");
+ multiplier_heading_max = getParameterFloat(nodeHandle_for_TuningController, "multiplier_heading_max");
+
+ // // DEBUGGING: Print out one of the parameters that was loaded
+ ROS_INFO_STREAM("[TUNING CONTROLLER] DEBUGGING: the fetched TuningController/multiplier_horizontal_min = " << multiplier_horizontal_min);
+
+ // ******************************************************************************* //
+
+
+
+ // > The mass of the crazyflie
+ cf_mass = getParameterFloat(nodeHandle_for_TuningController , "mass");
+
+ // Display one of the YAML parameters to debug if it is working correctly
+ //ROS_INFO_STREAM("DEBUGGING: mass leaded from loacl file = " << cf_mass );
+
+ // > The frequency at which the "computeControlOutput" is being called, as determined
+ // by the frequency at which the Vicon system provides position and attitude data
+ vicon_frequency = getParameterFloat(nodeHandle_for_TuningController, "vicon_frequency");
+
+ // > The frequency at which the "computeControlOutput" is being called, as determined
+ // by the frequency at which the Vicon system provides position and attitude data
+ control_frequency = getParameterFloat(nodeHandle_for_TuningController, "control_frequency");
+
+ // > The co-efficients of the quadratic conversation from 16-bit motor command to
+ // thrust force in Newtons
+ getParameterFloatVector(nodeHandle_for_TuningController, "motorPoly", motorPoly, 3);
+
+ // > The boolean for whether to execute the convert into body frame function
+ shouldPerformConvertIntoBodyFrame = getParameterBool(nodeHandle_for_TuningController, "shouldPerformConvertIntoBodyFrame");
+
+ // Boolean indiciating whether the "Debug Message" of this agent should be published or not
+ shouldPublishDebugMessage = getParameterBool(nodeHandle_for_TuningController, "shouldPublishDebugMessage");
+
+ // Boolean indiciating whether the debugging ROS_INFO_STREAM should be displayed or not
+ shouldDisplayDebugInfo = getParameterBool(nodeHandle_for_TuningController, "shouldDisplayDebugInfo");
+
+
+ // THE CONTROLLER GAINS:
+
+ // A flag for which controller to use:
+ controller_mode = getParameterInt( nodeHandle_for_TuningController , "controller_mode" );
+
+ // A flag for which estimator to use:
+ estimator_method = getParameterInt( nodeHandle_for_TuningController , "estimator_method" );
+
+ // The LQR Controller parameters for "LQR_MODE_RATE"
+ getParameterFloatVector(nodeHandle_for_TuningController, "gainMatrixThrust_NineStateVector", gainMatrixThrust_NineStateVector, 9);
+ getParameterFloatVector(nodeHandle_for_TuningController, "gainMatrixRollRate", gainMatrixRollRate, 9);
+ getParameterFloatVector(nodeHandle_for_TuningController, "gainMatrixPitchRate", gainMatrixPitchRate, 9);
+ getParameterFloatVector(nodeHandle_for_TuningController, "gainMatrixYawRate", gainMatrixYawRate, 9);
+
+ // The LQR Controller parameters for "LQR_MODE_ANGLE"
+ getParameterFloatVector(nodeHandle_for_TuningController, "gainMatrixThrust_SixStateVector", gainMatrixThrust_SixStateVector, 6);
+ getParameterFloatVector(nodeHandle_for_TuningController, "gainMatrixRollAngle", gainMatrixRollAngle, 6);
+ getParameterFloatVector(nodeHandle_for_TuningController, "gainMatrixPitchAngle", gainMatrixPitchAngle, 6);
+
+ // The LQR Controller parameters for "LQR_MODE_ANGLE_RATE_NESTED"
+ getParameterFloatVector(nodeHandle_for_TuningController, "gainMatrixThrust_SixStateVector_50Hz", gainMatrixThrust_SixStateVector_50Hz, 6);
+ getParameterFloatVector(nodeHandle_for_TuningController, "gainMatrixRollAngle_50Hz", gainMatrixRollAngle_50Hz, 6);
+ getParameterFloatVector(nodeHandle_for_TuningController, "gainMatrixPitchAngle_50Hz", gainMatrixPitchAngle_50Hz, 6);
+
+ getParameterFloatVector(nodeHandle_for_TuningController, "gainMatrixRollRate_Nested", gainMatrixRollRate_Nested, 3);
+ getParameterFloatVector(nodeHandle_for_TuningController, "gainMatrixPitchRate_Nested", gainMatrixPitchRate_Nested, 3);
+ getParameterFloatVector(nodeHandle_for_TuningController, "gainMatrixYawRate_Nested", gainMatrixYawRate_Nested, 3);
+
+
+ // 16-BIT COMMAND LIMITS
+ cmd_sixteenbit_min = getParameterFloat(nodeHandle_for_TuningController, "command_sixteenbit_min");
+ cmd_sixteenbit_max = getParameterFloat(nodeHandle_for_TuningController, "command_sixteenbit_max");
+
+
+ // THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
+ // > For the (x,y,z) position
+ getParameterFloatVector(nodeHandle_for_TuningController, "PMKF_Ahat_row1_for_positions", PMKF_Ahat_row1_for_positions, 2);
+ getParameterFloatVector(nodeHandle_for_TuningController, "PMKF_Ahat_row2_for_positions", PMKF_Ahat_row2_for_positions, 2);
+ getParameterFloatVector(nodeHandle_for_TuningController, "PMKF_Kinf_for_positions" , PMKF_Kinf_for_positions , 2);
+ // > For the (roll,pitch,yaw) angles
+ getParameterFloatVector(nodeHandle_for_TuningController, "PMKF_Ahat_row1_for_angles", PMKF_Ahat_row1_for_angles, 2);
+ getParameterFloatVector(nodeHandle_for_TuningController, "PMKF_Ahat_row2_for_angles", PMKF_Ahat_row2_for_angles, 2);
+ getParameterFloatVector(nodeHandle_for_TuningController, "PMKF_Kinf_for_angles" , PMKF_Kinf_for_angles , 2);
+
+
+ // DEBUGGING: Print out one of the parameters that was loaded
+ //ROS_INFO_STREAM("[TUNING CONTROLLER] DEBUGGING: the fetched TuningController/mass = " << cf_mass);
+
+
+ // Call the function that computes details an values that are needed from these
+ // parameters loaded above
+ processFetchedParameters();
+}
+
+
+// This function CAN BE edited for successful completion of the PPS exercise, and the
+// use of parameters loaded from the YAML file is highly recommended to make tuning of
+// your controller easier and quicker.
+void processFetchedParameters()
+{
+ // Compute the feed-forward force that we need to counteract gravity (i.e., mg)
+ // > in units of [Newtons]
+ gravity_force = cf_mass * 9.81/(1000);
+ gravity_force_quarter = cf_mass * 9.81/(1000*4);
+
+ // Set that the estimator frequency is the same as the control frequency
+ estimator_frequency = vicon_frequency;
+
+ // DEBUGGING: Print out one of the computed quantities
+ ROS_INFO_STREAM("[TUNING CONTROLLER] DEBUGGING: after processing the gravity_force_quarter = " << gravity_force_quarter);
+}
+
+
+
+// ----------------------------------------------------------------------------------
+// GGGG EEEEE TTTTT PPPP A RRRR A M M ( )
+// G E T P P A A R R A A MM MM ( )
+// G EEE T PPPP A A RRRR A A M M M ( )
+// G G E T P AAAAA R R AAAAA M M ( )
+// GGGG EEEEE T P A A R R A A M M ( )
+// ----------------------------------------------------------------------------------
+
+
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name)
+{
+ float val;
+ if(!nodeHandle.getParam(name, val))
+ {
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val;
+}
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length)
+{
+ if(!nodeHandle.getParam(name, val)){
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ if(val.size() != length) {
+ ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
+ }
+}
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+int getParameterInt(ros::NodeHandle& nodeHandle, std::string name)
+{
+ int val;
+ if(!nodeHandle.getParam(name, val))
+ {
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val;
+}
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+void getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length)
+{
+ if(!nodeHandle.getParam(name, val)){
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ if(val.size() != length) {
+ ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
+ }
+}
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+int getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val)
+{
+ if(!nodeHandle.getParam(name, val)){
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val.size();
+}
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name)
+{
+ bool val;
+ if(!nodeHandle.getParam(name, val))
+ {
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val;
+}
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+std::string getParameterString(ros::NodeHandle& nodeHandle, std::string name)
+{
+ std::string val;
+ if(!nodeHandle.getParam(name, val)){
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val;
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// M M A III N N
+// MM MM A A I NN N
+// M M M A A I N N N
+// M M AAAAA I N NN
+// M M A A III N N
+// ----------------------------------------------------------------------------------
+
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+int main(int argc, char* argv[]) {
+
+ // Starting the ROS-node
+ ros::init(argc, argv, "TuningControllerService");
+
+ // Create a "ros::NodeHandle" type local variable "nodeHandle" as the current node,
+ // the "~" indcates that "self" is the node handle assigned to this variable.
+ ros::NodeHandle nodeHandle("~");
+
+ // Get the namespace of this "StudentControllerService" node
+ // > This should be something like: "/dfall/agentXXX"
+ std::string m_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[TUNING CONTROLLER] ros::this_node::getNamespace() = " << m_namespace);
+
+ // Get the agent ID as the "ROS_NAMESPACE" this computer.
+ // NOTES:
+ // > If you look at the "Student.launch" file in the "launch" folder, you will see
+ // the following line of code:
+ // <param name="studentID" value="$(optenv ROS_NAMESPACE)" />
+ // This line of code adds a parameter named "studentID" to the "PPSClient"
+ // > Thus, to get access to this "studentID" paremeter, we first need to get a handle
+ // to the "PPSClient" node within which this controller service is nested.
+ // Get the handle to the "PPSClient" node
+ ros::NodeHandle PPSClient_nodeHandle(m_namespace + "/PPSClient");
+ // Get the value of the "studentID" parameter into the instance variable "my_agentID"
+ if(!PPSClient_nodeHandle.getParam("agentID", my_agentID))
+ {
+ // Throw an error if the student ID parameter could not be obtained
+ ROS_ERROR("[TUNING CONTROLLER] Failed to get agentID from PPSClient");
+ }
+
+
+ // *********************************************************************************
+ // EVERYTHING THAT RELATES TO FETCHING PARAMETERS FROM A YAML FILE
+
+
+ // EVERYTHING FOR THE CONNECTION TO THIS AGENT's OWN PARAMETER SERVICE:
+
+ // Set the class variable "namespace_to_own_agent_parameter_service" to be a the
+ // namespace string for the parameter service that is running on the machine of this
+ // agent
+ namespace_to_own_agent_parameter_service = m_namespace + "/ParameterService";
+
+ // Create a node handle to the parameter service running on this agent's machine
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(namespace_to_own_agent_parameter_service);
+
+ // Instantiate the local variable "controllerYamlReadyForFetchSubscriber" to be a
+ // "ros::Subscriber" type variable that subscribes to the "controllerYamlReadyForFetch" topic
+ // and calls the class function "yamlReadyForFetchCallback" each time a message is
+ // received on this topic and the message is passed as an input argument to the
+ // "yamlReadyForFetchCallback" class function.
+ ros::Subscriber controllerYamlReadyForFetchSubscriber_to_agent = nodeHandle_to_own_agent_parameter_service.subscribe("controllerYamlReadyForFetch", 1, yamlReadyForFetchCallback);
+
+
+ // EVERYTHING FOR THE CONNECTION THE COORDINATOR'S PARAMETER SERVICE:
+
+ // Set the class variable "nodeHandle_to_coordinator_parameter_service" to be a node handle
+ // for the parameter service that is running on the coordinate machine
+ // NOTE: the backslash here (i.e., "/") before the name of the node ("ParameterService")
+ // is very important because it specifies that the name is global
+ namespace_to_coordinator_parameter_service = "/ParameterService";
+
+ // Create a node handle to the parameter service running on the coordinator machine
+ ros::NodeHandle nodeHandle_to_coordinator = ros::NodeHandle();
+ //ros::NodeHandle nodeHandle_to_coordinator_parameter_service = ros::NodeHandle(namespace_to_own_agent_parameter_service);
+
+
+ // Instantiate the local variable "controllerYamlReadyForFetchSubscriber" to be a
+ // "ros::Subscriber" type variable that subscribes to the "controllerYamlReadyForFetch" topic
+ // and calls the class function "yamlReadyForFetchCallback" each time a message is
+ // received on this topic and the message is passed as an input argument to the
+ // "yamlReadyForFetchCallback" class function.
+ ros::Subscriber controllerYamlReadyForFetchSubscriber_to_coordinator = nodeHandle_to_coordinator.subscribe("/ParameterService/controllerYamlReadyForFetch", 1, yamlReadyForFetchCallback);
+ //ros::Subscriber controllerYamlReadyForFetchSubscriber_to_coordinator = nodeHandle_to_coordinator_parameter_service.subscribe("controllerYamlReadyForFetch", 1, yamlReadyForFetchCallback);
+
+
+ // PRINT OUT SOME INFORMATION
+
+ // Let the user know what namespaces are being used for linking to the parameter service
+ ROS_INFO_STREAM("[TUNING CONTROLLER] The namespace string for accessing the Paramter Services are:");
+ ROS_INFO_STREAM("[TUNING CONTROLLER] namespace_to_own_agent_parameter_service = " << namespace_to_own_agent_parameter_service);
+ ROS_INFO_STREAM("[TUNING CONTROLLER] namespace_to_coordinator_parameter_service = " << namespace_to_coordinator_parameter_service);
+
+
+ // FINALLY, FETCH ANY PARAMETERS REQUIRED FROM THESE "PARAMETER SERVICES"
+
+ // Call the class function that loads the parameters for this class.
+ fetchYamlParameters(nodeHandle_to_own_agent_parameter_service);
+
+ // *********************************************************************************
+
+
+
+ // Subscribe to the message that activates the tests
+ ros::Subscriber tuningActivateTestSubscriber = nodeHandle.subscribe("ActivateTest", 1, activateTestCallback);
+
+ // Subscribe to the message that changes the gains
+ ros::Subscriber tuningHorizontalGainSubscriber = nodeHandle.subscribe("HorizontalGain", 1, horizontalGainCallback);
+ ros::Subscriber tuningVerticalGainSubscriber = nodeHandle.subscribe("VerticalGain", 1, verticalGainCallback);
+ ros::Subscriber tuningHeadingGainSubscriber = nodeHandle.subscribe("HeadingGain", 1, headingGainCallback);
+
+
+
+ // Instantiate the instance variable "debugPublisher" to be a "ros::Publisher" that
+ // advertises under the name "DebugTopic" and is a message with the structure
+ // defined in the file "DebugMsg.msg" (located in the "msg" folder).
+ debugPublisher = nodeHandle.advertise<DebugMsg>("DebugTopic", 1);
+
+ // Instantiate the local variable "setpointSubscriber" to be a "ros::Subscriber"
+ // type variable that subscribes to the "Setpoint" topic and calls the class function
+ // "setpointCallback" each time a messaged is received on this topic and the message
+ // is passed as an input argument to the "setpointCallback" class function.
+ //ros::Subscriber setpointSubscriber = nodeHandle.subscribe("Setpoint", 1, setpointCallback);
+
+ // Instantiate the local variable "service" to be a "ros::ServiceServer" type
+ // variable that advertises the service called "CustomController". This service has
+ // the input-output behaviour defined in the "Controller.srv" file (located in the
+ // "srv" folder). This service, when called, is provided with the most recent
+ // measurement of the Crazyflie and is expected to respond with the control action
+ // that should be sent via the Crazyradio and requested from the Crazyflie, i.e.,
+ // this is where the "outer loop" controller function starts. When a request is made
+ // of this service the "calculateControlOutput" function is called.
+ ros::ServiceServer service = nodeHandle.advertiseService("TuningController", calculateControlOutput);
+
+ // Create a "ros::NodeHandle" type local variable "namespace_nodeHandle" that points
+ // to the name space of this node, i.e., "d_fall_pps" as specified by the line:
+ // "using namespace d_fall_pps;"
+ // in the "DEFINES" section at the top of this file.
+ ros::NodeHandle namespace_nodeHandle(ros::this_node::getNamespace());
+
+ // Print out some information to the user.
+ ROS_INFO("[TUNING CONTROLLER] Service ready :-)");
+
+ // Enter an endless while loop to keep the node alive.
+ ros::spin();
+
+ // Return zero if the "ross::spin" is cancelled.
+ return 0;
+}
diff --git a/pps_ws/src/d_fall_pps/src/ViconDataPublisher.cpp b/pps_ws/src/d_fall_pps/src/nodes/ViconDataPublisher.cpp
similarity index 100%
rename from pps_ws/src/d_fall_pps/src/ViconDataPublisher.cpp
rename to pps_ws/src/d_fall_pps/src/nodes/ViconDataPublisher.cpp
diff --git a/pps_ws/src/d_fall_pps/srv/LoadYamlFiles.srv b/pps_ws/src/d_fall_pps/srv/LoadYamlFiles.srv
new file mode 100644
index 0000000000000000000000000000000000000000..f51e18525ad2cab1e31875c4c31014a98eda1d6a
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/srv/LoadYamlFiles.srv
@@ -0,0 +1,3 @@
+string[] yamlFileNames
+---
+float64 waitTime
diff --git a/pps_wiki/faq.md b/wiki/faq.md
similarity index 100%
rename from pps_wiki/faq.md
rename to wiki/faq.md
diff --git a/pps_wiki/installation.md b/wiki/installation.md
similarity index 100%
rename from pps_wiki/installation.md
rename to wiki/installation.md
diff --git a/wiki/namespace_conventions.md b/wiki/namespace_conventions.md
new file mode 100644
index 0000000000000000000000000000000000000000..7ba26326bc2fc5a3a08005c9d59d6da0e8395ac9
--- /dev/null
+++ b/wiki/namespace_conventions.md
@@ -0,0 +1,41 @@
+# ROS Namespace Conventions
+
+## Environment Variables
+
+### ``ROS_NAMESPACE`` Environment Variable
+
+All computers part of the ``D-FaLL-System`` should have the environment variable ``ROS_NAMESPACE`` set to the value ``dfall``.
+
+You can check this from any terminal window with the command
+```
+echo $ROS_NAMESPACE
+```
+
+This environment vairable is set in the file:
+```
+pps_ws/src/d_fall_pps/launch/Config.sh
+```
+The environemnt variable is set by the following line in that file:
+```
+export ROS_NAMESPACE='dfall'
+```
+
+### ``DEFAULT_{AGENT,COORD}_ID`` Environment Variables
+
+The ``Config.sh`` also sets the environment variables for the default agent ID and the default coordinator ID, called ``DFALL_DEFAULT_AGENT_ID`` and ``DFALL_DEFAULT_COORD_ID``. These are set by the following line in the ``Config.sh`` file:
+```
+export DFALL_DEFAULT_AGENT_ID=$(cat /etc/dfall_default_agent_id)
+export DFALL_DEFAULT_COORD_ID=$(cat /etc/dfall_default_coord_id)
+```
+This is the only place in the whole ``D-FaLL-System`` where the files ``/etc/dfall_default_{agent,coord}_id`` are referred to.
+
+NOTE:
+- These files should never be access from within any code,<br>
+- To use these default ID's within any code, they should be added as parameters when launch the respective node,<br>
+- The launch files is the only place where the default ID environment variables should be used.<br>
+
+
+
+## Launching nodes
+
+To be filled in
\ No newline at end of file
diff --git a/pps_wiki/pics/LUT.png b/wiki/pics/LUT.png
similarity index 100%
rename from pps_wiki/pics/LUT.png
rename to wiki/pics/LUT.png
diff --git a/pps_wiki/pics/client_config_yaml.png b/wiki/pics/client_config_yaml.png
similarity index 100%
rename from pps_wiki/pics/client_config_yaml.png
rename to wiki/pics/client_config_yaml.png
diff --git a/pps_wiki/pics/custom_controller_src.png b/wiki/pics/custom_controller_src.png
similarity index 100%
rename from pps_wiki/pics/custom_controller_src.png
rename to wiki/pics/custom_controller_src.png
diff --git a/pps_wiki/pics/custom_controller_yaml.png b/wiki/pics/custom_controller_yaml.png
similarity index 100%
rename from pps_wiki/pics/custom_controller_yaml.png
rename to wiki/pics/custom_controller_yaml.png
diff --git a/pps_wiki/pics/ros_pics/GraphicalRepresentations.png b/wiki/pics/ros_pics/GraphicalRepresentations.png
similarity index 100%
rename from pps_wiki/pics/ros_pics/GraphicalRepresentations.png
rename to wiki/pics/ros_pics/GraphicalRepresentations.png
diff --git a/pps_wiki/pics/rqt_window_subplots.png b/wiki/pics/rqt_window_subplots.png
similarity index 100%
rename from pps_wiki/pics/rqt_window_subplots.png
rename to wiki/pics/rqt_window_subplots.png
diff --git a/pps_wiki/pics/setup_pics/calibration1.png b/wiki/pics/setup_pics/calibration1.png
similarity index 100%
rename from pps_wiki/pics/setup_pics/calibration1.png
rename to wiki/pics/setup_pics/calibration1.png
diff --git a/pps_wiki/pics/setup_pics/calibration2.png b/wiki/pics/setup_pics/calibration2.png
similarity index 100%
rename from pps_wiki/pics/setup_pics/calibration2.png
rename to wiki/pics/setup_pics/calibration2.png
diff --git a/pps_wiki/pics/setup_pics/calibration3.png b/wiki/pics/setup_pics/calibration3.png
similarity index 100%
rename from pps_wiki/pics/setup_pics/calibration3.png
rename to wiki/pics/setup_pics/calibration3.png
diff --git a/pps_wiki/pics/setup_pics/cforientation.jpg b/wiki/pics/setup_pics/cforientation.jpg
similarity index 100%
rename from pps_wiki/pics/setup_pics/cforientation.jpg
rename to wiki/pics/setup_pics/cforientation.jpg
diff --git a/pps_wiki/pics/setup_pics/chooseVicon.jpg b/wiki/pics/setup_pics/chooseVicon.jpg
similarity index 100%
rename from pps_wiki/pics/setup_pics/chooseVicon.jpg
rename to wiki/pics/setup_pics/chooseVicon.jpg
diff --git a/pps_wiki/pics/setup_pics/configsh1.png b/wiki/pics/setup_pics/configsh1.png
similarity index 100%
rename from pps_wiki/pics/setup_pics/configsh1.png
rename to wiki/pics/setup_pics/configsh1.png
diff --git a/pps_wiki/pics/setup_pics/configsh2.png b/wiki/pics/setup_pics/configsh2.png
similarity index 100%
rename from pps_wiki/pics/setup_pics/configsh2.png
rename to wiki/pics/setup_pics/configsh2.png
diff --git a/pps_wiki/pics/setup_pics/defining1.png b/wiki/pics/setup_pics/defining1.png
similarity index 100%
rename from pps_wiki/pics/setup_pics/defining1.png
rename to wiki/pics/setup_pics/defining1.png
diff --git a/pps_wiki/pics/setup_pics/defining2.png b/wiki/pics/setup_pics/defining2.png
similarity index 100%
rename from pps_wiki/pics/setup_pics/defining2.png
rename to wiki/pics/setup_pics/defining2.png
diff --git a/pps_wiki/pics/setup_pics/defining3.png b/wiki/pics/setup_pics/defining3.png
similarity index 100%
rename from pps_wiki/pics/setup_pics/defining3.png
rename to wiki/pics/setup_pics/defining3.png
diff --git a/pps_wiki/pics/setup_pics/differentlayouts.jpg b/wiki/pics/setup_pics/differentlayouts.jpg
similarity index 100%
rename from pps_wiki/pics/setup_pics/differentlayouts.jpg
rename to wiki/pics/setup_pics/differentlayouts.jpg
diff --git a/pps_wiki/pics/setup_pics/internetethernet1.png b/wiki/pics/setup_pics/internetethernet1.png
similarity index 100%
rename from pps_wiki/pics/setup_pics/internetethernet1.png
rename to wiki/pics/setup_pics/internetethernet1.png
diff --git a/pps_wiki/pics/setup_pics/internetethernet2.png b/wiki/pics/setup_pics/internetethernet2.png
similarity index 100%
rename from pps_wiki/pics/setup_pics/internetethernet2.png
rename to wiki/pics/setup_pics/internetethernet2.png
diff --git a/pps_wiki/pics/setup_pics/internetethernet3.png b/wiki/pics/setup_pics/internetethernet3.png
similarity index 100%
rename from pps_wiki/pics/setup_pics/internetethernet3.png
rename to wiki/pics/setup_pics/internetethernet3.png
diff --git a/pps_wiki/pics/setup_pics/viconNetwork1.png b/wiki/pics/setup_pics/viconNetwork1.png
similarity index 100%
rename from pps_wiki/pics/setup_pics/viconNetwork1.png
rename to wiki/pics/setup_pics/viconNetwork1.png
diff --git a/pps_wiki/pics/setup_pics/viconNetwork2.png b/wiki/pics/setup_pics/viconNetwork2.png
similarity index 100%
rename from pps_wiki/pics/setup_pics/viconNetwork2.png
rename to wiki/pics/setup_pics/viconNetwork2.png
diff --git a/pps_wiki/pics/setup_pics/viconNetwork3.png b/wiki/pics/setup_pics/viconNetwork3.png
similarity index 100%
rename from pps_wiki/pics/setup_pics/viconNetwork3.png
rename to wiki/pics/setup_pics/viconNetwork3.png
diff --git a/pps_wiki/pics/setup_pics/wand.jpg b/wiki/pics/setup_pics/wand.jpg
similarity index 100%
rename from pps_wiki/pics/setup_pics/wand.jpg
rename to wiki/pics/setup_pics/wand.jpg
diff --git a/pps_wiki/pics/student_gui.png b/wiki/pics/student_gui.png
similarity index 100%
rename from pps_wiki/pics/student_gui.png
rename to wiki/pics/student_gui.png
diff --git a/pps_wiki/pics/success_building.png b/wiki/pics/success_building.png
similarity index 100%
rename from pps_wiki/pics/success_building.png
rename to wiki/pics/success_building.png
diff --git a/pps_wiki/pps_toc.md b/wiki/pps_toc.md
similarity index 100%
rename from pps_wiki/pps_toc.md
rename to wiki/pps_toc.md
diff --git a/pps_wiki/ros_structure.md b/wiki/ros_structure.md
similarity index 100%
rename from pps_wiki/ros_structure.md
rename to wiki/ros_structure.md
diff --git a/pps_wiki/setup.md b/wiki/setup.md
similarity index 100%
rename from pps_wiki/setup.md
rename to wiki/setup.md
diff --git a/pps_wiki/workflow_for_students.md b/wiki/workflow_for_students.md
similarity index 100%
rename from pps_wiki/workflow_for_students.md
rename to wiki/workflow_for_students.md